(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Connecting to Dynamixel bus

Description: This tutorial describes how to connect and examine raw feedback from Robotis Dynamixel servos.

Keywords: robotis,dynamixel,connecting,feedback

Tutorial Level: BEGINNER

Next Tutorial: Creating meta controller

Step 1: Create a package

If the Dynamixel Controllers driver is not installed open the terminal and type:

sudo apt-get install ros-<distro>-dynamixel-controllers

Before we begin we need to create a package that has proper dependencies and will contain all the files we create in this tutorial. Note: before executing command listed below make sure that the current directory is in one of the directories of the ROS_PACKAGE_PATH environment variable. Refer to the environment tutorials for how to best create a workspace and overlay directory.

# You should have created this in the Creating a Workspace Tutorial
cd ~/catkin_ws/src
catkin_create_pkg my_dynamixel_tutorial dynamixel_controllers std_msgs rospy roscpp

Step 1: Create a package

Before we begin we need to create a package that has proper dependencies and will contain all the files we create in this tutorial. Note: before executing command listed below make sure that the current directory is in one of the directories of the ROS_PACKAGE_PATH environment variable. Refer to the environment tutorials for how to best create a workspace and overlay directory.

roscreate-pkg my_dynamixel_tutorial dynamixel_controllers
roscd my_dynamixel_tutorial

Step 2: Create a launch file for the manager node

We assume that the Dynamixel servos are connected to /dev/ttyUSB0 serial port.

First we need to start up the controller manager that will connect to the motors and publish raw feedback data (e.g. current position, goal position, error, etc.) at a specified rate. The easiest way to do that is to write a launch file that will set all necessary parameters. Let's copy and paste the following text into a controller_manager.launch file.

https://raw.githubusercontent.com/arebgun/dynamixel_motor/master/dynamixel_tutorials/launch/controller_manager.launch

   1 <!-- -*- mode: XML -*- -->
   2 
   3 <launch>
   4     <node name="dynamixel_manager" pkg="dynamixel_controllers" type="controller_manager.py" required="true" output="screen">
   5         <rosparam>
   6             namespace: dxl_manager
   7             serial_ports:
   8                 pan_tilt_port:
   9                     port_name: "/dev/ttyUSB0"
  10                     baud_rate: 1000000
  11                     min_motor_id: 1
  12                     max_motor_id: 25
  13                     update_rate: 20
  14         </rosparam>
  15     </node>
  16 </launch>

Note: Make sure you set your baud rate correctly. If you are using RX-28 motors, it should be set to 57142. If you have further problems, see if your user account needs to be added to the dialout group and if the USB port needs to be made writeable.

Now if we execute:

roslaunch my_dynamixel_tutorial controller_manager.launch

We should see the status output similar to the following:

[INFO] [WallTime: 1295282870.051953] pan_tilt_port: Pinging motor IDs 1 through 25...
[INFO] [WallTime: 1295282870.103676] pan_tilt_port: Found 4 motors - 4 AX-12 [4, 5, 6, 13], initialization complete.

Naturally the output depends on what motors are connected to the USB bus.

Step 3: Examine motor feedback

Now the controller manager is publishing feedback on /motor_states/pan_tilt_port topic. First, let's check that the topic is there:

rostopic list

The output should look similar to the one shown below:

/motor_states/pan_tilt_port
/rosout
/rosout_agg

One of the topic in the list should be the /motor_states/pan_tilt_port topic. Let's see what kind of information is published:

rostopic echo /motor_states/pan_tilt_port

In this case we have four AX-12+ motors connected:

motor_states:
motor_states:
  -
    timestamp: 1351555461.28
    id: 4
    goal: 517
    position: 527
    error: 10
    speed: 0
    load: 0.3125
    voltage: 12.4
    temperature: 39
    moving: False
  -
    timestamp: 1351555461.28
    id: 5
    goal: 512
    position: 483
    error: -29
    speed: 0
    load: 0.0
    voltage: 12.6
    temperature: 40
    moving: False
  -
    timestamp: 1351555461.28
    id: 6
    goal: 511
    position: 516
    error: 5
    speed: 0
    load: 0.0
    voltage: 12.3
    temperature: 41
    moving: False
  -
    timestamp: 1351555461.28
    id: 13
    goal: 256
    position: 256
    error: 0
    speed: 0
    load: 0.0
    voltage: 12.3
    temperature: 38
    moving: False
---

That's it! In the next tutorial you'll learn how to connect to several motors.

Wiki: dynamixel_controllers/Tutorials/ConnectingToDynamixelBus (last edited 2021-05-18 08:16:04 by Mads Rossen)