(!) 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 velocity controller with wheel command

Description: This tutorial describes how to running velocity controller

Keywords: robotis, dynamixel, velocity, wheel

Tutorial Level: BEGINNER

Next Tutorial: Dynamixel workbench torque 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(velocity_control.launch) in launch folder. But, in this tutorial, we create a new tutorial package to show how to run dynamixel_workbench_contorllers 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 velocity control mode and assemble it to mobile platform. If you need to check your Dynamixel states, you might use dynamixel_workbench_single_manager

wheel_example.jpg

Step 3: Create a launch file for the velocity 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, model name, protocol version and id of Dynamixel. 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(reversed mode ON). 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 velocity_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="left_id"           default="1"/>
  <arg name="right_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="left_id"           value="$(arg left_id)"/>
  <param name="right_id"          value="$(arg right_id)"/>

  <node name="velocity_control_example" pkg="dynamixel_workbench_controllers" type="velocity_control" required="true" output="screen"/>
</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 velocity_control.launch

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

[ INFO] [1498123155.082127632]: Succeeded to open the port(/dev/ttyUSB0)!
[ INFO] [1498123155.082561764]: Succeeded to change the baudrate(57600)!
[ INFO] [1498123155.121064903]: -----------------------------------------------------------------------
[ INFO] [1498123155.121091173]:   dynamixel_workbench controller; velocity control example             
[ INFO] [1498123155.121100252]: -----------------------------------------------------------------------
[ INFO] [1498123155.121110565]: PAN MOTOR INFO
[ INFO] [1498123155.121121295]: ID    : 1
[ INFO] [1498123155.121132022]: MODEL : XM430_W210
[ INFO] [1498123155.121146306]:  
[ INFO] [1498123155.121156065]: TILT MOTOR INFO
[ INFO] [1498123155.121167943]: ID    : 2
[ INFO] [1498123155.121184882]: MODEL : XM430_W210
[ INFO] [1498123155.121196264]: -----------------------------------------------------------------------

Step 4: Check state of Dynamixel

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

$ rostopic echo /velocity_control/dynamixel_state

For example:

dynamixel_state: 
  - 
    model_name: XM430_W350
    id: 1
    torque_enable: 1
    goal_current: 1193
    goal_velocity: 0
    goal_position: 1294
    present_current: -2
    present_velocity: 0
    present_position: 1294
    moving: 0
  - 
    model_name: XM430_W210
    id: 2
    torque_enable: 1
    goal_current: 1193
    goal_velocity: 0
    goal_position: 1673
    present_current: 1
    present_velocity: 0
    present_position: 1673
    moving: 0

Step 5: Run mobile platform

We run the two linked Dynamixels using rosservice call or dynamixel_workbench_operators [unit : rad/sec].

 $ rosrun dynamixel_workbench_operators wheel_operator 

Then, we can set velocity using keyboard.

[ INFO] [1498123238.521332487]: Set angular velocity(+-0.2 rad/sec) to your Dynamixel!! by using keyboard
[ INFO] [1498123238.521369361]: w : Forward
[ INFO] [1498123238.521380496]: x : Backward
[ INFO] [1498123238.521389146]: a : Left
[ INFO] [1498123238.521397016]: d : Right
[ INFO] [1498123238.521404724]: s : STOPS

[ INFO] [1498123238.521414139]: ESC : exit

Press 'w' then the mobile plaform is running!! or

 $ rosservice call /wheel_command -- [left_wheel_velocity] [right_wheel_velocity]

The mobile plaform is running!!

If you have a question about running velocity_controller, please make anew issue.

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