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

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

Step 2: Prepare two Dynamixels

We need two Dynamixels which are assembled common mobile platform.

wheel_example.jpg

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

First we need a launch file in launch folder in my_dynamixel_workbench_tutorial package. This launch file connects to the dynamixel_workbench_controller package and sets device name and baudrate of a Dynamixel. We assume that the Dynamixel is connected to /dev/ttyUSB0 and baudrate 57600. If not, make sure you set your device name and baudrate correctly. NOTE: IN THIS TUTORIAL, WE USE XM430-W210

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="left_wheel"              default="1"/>
<arg name="right_wheel"             default="2"/>

<arg name="profile_velocity"        default="200"/>
<arg name="profile_acceleration"    default="50"/>

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

<param name="left_wheel"            value="$(arg left_wheel)"/>
<param name="right_wheel"           value="$(arg right_wheel)"/>

<param name="profile_velocity"      value="$(arg profile_velocity)"/>
<param name="profile_acceleration"  value="$(arg profile_acceleration)"/>

<node name="velocity_control" 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:

-----------------------------------------------------------------------
dynamixel_workbench controller; velocity control example             
-----------------------------------------------------------------------

MODEL   : XM430-W350
ID      : 1

MODEL   : XM430-W350
ID      : 2

-----------------------------------------------------------------------

Step 4: Check state of Dynamixel

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

$ rostopic echo /dynamixel_state

For example:

dynamixel_state: 
- 
    model_name: "XM430-W350"
    id: 1
    torque_enable: 1
    goal_current: 0
    goal_velocity: 0
    goal_position: 2050
    present_current: 0
    present_velocity: 0
    present_position: 2049
    moving: 0
- 
    model_name: "XM430-W350"
    id: 2
    torque_enable: 1
    goal_current: 0
    goal_velocity: 0
    goal_position: 1969
    present_current: 0
    present_velocity: 0
    present_position: 1967
    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 -- [right_vel] [left_vel]

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 2018-01-17 10:11:44 by Darby Lim)