(!) 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.

Dynamixel workbench position controller with joint command

Description: This tutorial describes how to running position controller

Keywords: robotis, dynamixel, position, pan, tilt

Tutorial Level: BEGINNER

Next Tutorial: Dynamixel workbench velocity controller

IF YOU ALREADY CREATE my_dynamixel_workbench_tutorial PACKAGE, THEN YOU JUMP TO STEP 2.

Step 1: Create a package

This package contains default launch file(position_control.launch) in launch folder. But, in this tutorial, we create a new tutorial package to show how to run dynamixel_workbench_controllers in other package.

$ cd ~/catkin_ws/src
$ catkin_create_pkg my_dynamixel_workbench_tutorial std_msgs roscpp

Step 2: Prepare two Dynamixels

We need to two Dynamixels set by position control mode and assemble it to pan tilt structure. If you need to check your Dynamixel states, you might use dynamixel_workbench_single_manager

pan_tilt_example.jpg

Step 3: Create a launch file for the position control node

Now we need a launch file in launch folder in my_dynamixel_workbench_tutorial package. This launch file connects to the dynamixel_workbench_controllers package and sets device name, baudrate, protocol version and id of Dynamixels. We assume that the XM430-W210-R is connected to /dev/ttyUSB0 and has 57600 baudrate, 2.0 protocol_version and each motor is set id 1 and 2. If not, make sure you set your Dynamixel correctly.

1. Make a launch file in ros package which we created

$ cd my_dynamixel_workbench_tutorial
$ mkdir launch
$ cd launch
$ gedit position_control.launch

2. Type or copy&paste code below to connect dynamixel_workbench_controllers packages and set parameters

<launch>
  <arg name="device_name"      default="/dev/ttyUSB0"/>
  <arg name="baud_rate"        default="57600"/>
  <arg name="protocol_version" default="2.0"/>

  <arg name="pan_id"           default="1"/>
  <arg name="tilt_id"          default="2"/>

  <param name="device_name"      value="$(arg device_name)"/>
  <param name="baud_rate"        value="$(arg baud_rate)"/>
  <param name="protocol_version" value="$(arg protocol_version)"/>

  <param name="pan_id"           value="$(arg pan_id)"/>
  <param name="tilt_id"          value="$(arg tilt_id)"/>

  <node name="potision_control_example" pkg="dynamixel_workbench_controllers" type="position_control" required="true" output="screen">
    <rosparam>
      profile_velocity: 100
      profile_acceleration: 10
    </rosparam>
  </node>
</launch>

3. Before we operating this package, we need to access permission for USB device

$ sudo chmod a+rw /dev/ttyUSB0

4. Now we can run tutorial package:

$ cd ~/catkin_ws && catkin_make
$ roslaunch my_dynamixel_workbench_tutorial position_control.launch

If position_controllers find linked Dynamixels, we could show state of it and command list and set torque on:

[ INFO] [1498122845.734230049]: Succeeded to open the port(/dev/ttyUSB0)!
[ INFO] [1498122845.734694362]: Succeeded to change the baudrate(57600)!
[ INFO] [1498122845.774595659]: -----------------------------------------------------------------------
[ INFO] [1498122845.774621277]:   dynamixel_workbench controller; position control example(Pan & Tilt) 
[ INFO] [1498122845.774630837]: -----------------------------------------------------------------------
[ INFO] [1498122845.774647858]: PAN MOTOR INFO
[ INFO] [1498122845.774659373]: ID    : 1
[ INFO] [1498122845.774671990]: MODEL : XM430_W210
[ INFO] [1498122845.774681590]:  
[ INFO] [1498122845.774690925]: TILT MOTOR INFO
[ INFO] [1498122845.774700485]: ID    : 2
[ INFO] [1498122845.774710310]: MODEL : XM430_W210
[ INFO] [1498122845.774721509]:  
[ INFO] [1498122845.774730931]: Profile Velocity     : 100
[ INFO] [1498122845.774740424]: Profile Acceleration : 10
[ INFO] [1498122845.774750915]: -----------------------------------------------------------------------

Step 4: Check state of Dynamixel

Now, we can check a state of linked Dynamixels through /position_control/dynamixel_state topic:

$ rostopic echo /position_control/dynamixel_state

For example:

dynamixel_state: 
  - 
    model_name: XM430_W350
    id: 1
    torque_enable: 1
    goal_current: 1193
    goal_velocity: 350
    goal_position: 2045
    present_current: 0
    present_velocity: 0
    present_position: 2045
    moving: 0
  - 
    model_name: XM430_W350
    id: 2
    torque_enable: 1
    goal_current: 1193
    goal_velocity: 350
    goal_position: 2049
    present_current: 0
    present_velocity: 0
    present_position: 2049
    moving: 0

Step 5: Run pan & tilt

We run the two linked Dynamixels using rosservice call or dynamixel_workbench_operators with different unit radian or raw value.

 $ rosservice call /joint_command -- [unit] [pan_pos] [tilt_pos]

or

 $ rosrun dynamixel_workbench_operators joint_operator [unit] [pan_pos] [tilt_pos]

Example

 $ rosservice call /joint_command -- rad -1.0 2.0

or

 $ rosrun dynamixel_workbench_operators joint_operator raw 2048 3000

The pan & tilt is running!! If you have a question about running position_control, please make anew issue.

Wiki: dynamixel_workbench_controllers/Tutorials/PositionControl (last edited 2017-08-07 00:07:52 by Darby Lim)