About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Dynamixel Position Control

Description: This tutorial explains how to use the dynamixel motor in position mode, this simple application can be used with a camera that takes photos from many angles. This application use only publisher and subscriber. The motor should be in multiturn mode, you can configure it using the Mixcell program.

Tutorial Level: BEGINNER

Installing Drives

To install dynamixel-msgs, open the terminal and type:

sudo apt-get install ros-kinetic-dynamixel-msgs

To install dynamixel-controllers, open the terminal and type:

sudo apt-get install ros-kinetic-dynamixel-controllers

Creating a Dynamixel Package

The following command creates a 'dynamixel_position_control' package. This package is dependent on the 'message_generation', 'std_msgs', and 'roscpp' packages.

$ cd ~/catkin_ws/src
$ catkin_create_pkg dynamixel_position_control message_generation std_msgs roscpp

Modifying the Package Configuration (package.xml)

The ‘package.xml’ file, one of the required ROS configuration files, is an XML file containing the package information such as the package name, author, license, and dependent packages. Let’s open the file using an editor (such as gedit, vim, emacs, etc.) with the following command and modify it for the current node.

$ cd dynamixel_position_control
$ gedit package.xml

The following code shows how to modify the 'package.xml' file to match package you created.

<?xml version="1.0"?>
<package>
<name>dynamixel_position_control</name>
<version>0.1.0</version>
<description>ROS tutorial package to learn the topic</description>
<license>Apache License 2.0</license>
<author email="pyo@robotis.com">Yoonseok Pyo</author>
<maintainer email="pyo@robotis.com">Yoonseok Pyo</maintainer>
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ros_tutorials/issues</url>
<url type="repository">https://github.com/ROBOTIS-GIT/ros_tutorials.git</url>
<url type="website">http://www.robotis.com</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<export></export>
</package>

Modifying the Build Configuration File (CMakelist.txt)

Catkin, which is the build system of ROS, uses CMake. The build environment is described in the 'CMakelist.txt' file in the package folder. This file configures executable file creating, dependency package priority build, link creation, and so on.

$ gedit CMakelist.txt

The following is the modified code of CMakeLists.txt for the package we created.

cmake_minimum_required(VERSION 2.8.3)
project(dynamixel_position_control)
## A component package required when building the Catkin.
## Has dependency on message_generation, std_msgs, roscpp.
## An error occurs during the build if these packages do not exist.
find_package(catkin REQUIRED COMPONENTS message_generation std_msgs roscpp)
## Declaration Message: MsgDynamixel.msg
add_message_files(FILES MsgDynamixel.msg)
## an option to configure the dependent message.
## An error occurs duing the build if "std_msgs" is not installed.
generate_messages(DEPENDENCIES std_msgs)
## A Catkin package option that describes the library, the Catkin build dependencies,
## and the system dependent packages.
catkin_package(
LIBRARIES dynamixel_position_control
CATKIN_DEPENDS std_msgs roscpp
)
## Include directory configuration.
include_directories(${catkin_INCLUDE_DIRS})
## Build option for the "topic_publisher" node.
## Configuration of Executable files, target link libraries, and additional dependencies.
add_executable(motor_controller src/motor_controller.cpp)
add_dependencies(motor_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(motor_controller ${catkin_LIBRARIES})

Writing the Message File

The following option is added to the CMakeLists.txt file.

add_message_files(FILES MsgDynamixel.msg)

The above option indicates to include the message file 'MsgDynamixel.msg', which will be used in this example node, when building the package. Let's create the file in the following order:

$ roscd dynamixel_position_control
$ mkdir msg
$ cd msg
$ gedit MsgDynamixel.msg

The content in the message file is simple:

float64 data

Writing the Controller Node

The following option was previously configured in the ‘CMakeLists.txt’ file to create an executable file:

add_executable(motor_controller src/motor_controller.cpp)

That is, the ‘motor_controller.cpp’ file is built in the src’ folder to create the ‘motor_controller’ executable file. Let’s create a code that performs publisher and subscriber nodes functions in the following order:

$ roscd dynamixel_position_control/src
$ gedit motor_controller.cpp

The following code should be put in motor_controller.cpp

// ROS Default Header File
#include "ros/ros.h"
// MsgTutorial Message File Header
// The header file is automatically created when building the package.

#include "dynamixel_position_control/MsgDynamixel.h"
#include "dynamixel_msgs/JointState.h"

#define GOAL_POS 0
#define CURRENT_POS 1
#define ERROR 2
#define LOAD 3
#define ERROR_POS 0.01
#define ERROR_NEG -0.01
#define TX 0
#define RX 1

void motor_command(ros::Publisher dynamixel_publisher); //Receive the current motor position and send the next position
bool motor_init(float qtd_pos); //Initialize motor variables

struct Motor{
 float motor_state[4], count, pos;
 int Estado;
 bool moving;
 dynamixel_position_control::MsgDynamixel msg;
}MX28;

//Initialize motor variables
bool motor_init(float qtd_pos)
{
  if(qtd_pos > 360 || qtd_pos <= 0){
   ROS_ERROR("Qtd_pos value should be between 0 and 360");
   return false;
  }
  MX28.Estado = TX;
  MX28.count = 0;
  MX28.pos = 6.14/qtd_pos;
  for(int i = 0; i < 4; i++) MX28.motor_state[i] = 0xff;
  MX28.moving = false;
  MX28.msg.data = 0;
  return true;
}

// Message callback function. This is a function is called when a topic
// message named 'tilt_controller/state' is received.
void msgCallback(const dynamixel_msgs::JointState::ConstPtr& msg)
{
 MX28.motor_state[GOAL_POS] = msg->goal_pos;
 MX28.motor_state[CURRENT_POS] = msg->current_pos;
 MX28.motor_state[ERROR] = msg->error;
 MX28.motor_state[LOAD] = msg->load;
 MX28.moving = msg->is_moving;
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "dynamixel_publisher"); // Initializes Node Name
  ros::init(argc, argv, "dynamixel_subscriber");
  ros::NodeHandle nh; // Node handle declaration for communication with ROS system

  float qtd_pos;
  nh.param("Qtd_Pos", qtd_pos, qtd_pos); //Receive qtd_pos from launch file (value should be between 0 and 360)

  if(!motor_init(qtd_pos)) return 0;

  // Declare publisher, create publisher 'dynamixel_publisher' using the 'MsgDynamixel'
  // message file from the 'dynamixel_control_position' package. The topic name is
  // 'tilt_controller/command' and the size of the publisher queue is set to 100
   ros::Publisher dynamixel_publisher = nh.advertise<dynamixel_position_control::MsgDynamixel>("tilt_controller/command", 100);

  // Declares subscriber. Create subscriber 'dynamixel_subscriber' using the 'MsgDynamixel'
  // message file from the 'dynamixel_control_position' package. The topic name is
  // 'tilt_controller/state' and the size of the subscribe queue is set to 100.
  ros::Subscriber dynamixel_subscriber = nh.subscribe("tilt_controller/state", 100, msgCallback);

  ros::Rate loop_rate(6); // Set the loop period (Hz)


   while (ros::ok()){
    // Goes to sleep according to the loop rate defined above.
    loop_rate.sleep();

    // A function for calling a callback function, waiting for a message to be
    // received, and executing a callback function when it is received
    ros::spinOnce();

    motor_command(dynamixel_publisher);
   }

   return 0;
}

//Receive the current motor position and send the next position
void motor_command(ros::Publisher dynamixel_publisher)
{
  switch(MX28.Estado)
  {
   case TX:
    MX28.msg.data = MX28.count;
    dynamixel_publisher.publish(MX28.msg); //Publishes 'MX28.msg' message
    ROS_INFO("Send Position = %f", MX28.msg.data);
    MX28.Estado = RX;
   break;

   case RX:
    if((MX28.count - MX28.motor_state[CURRENT_POS] <= ERROR_POS) && (MX28.count - MX28.motor_state[CURRENT_POS] >= ERROR_NEG))
    {
     ROS_INFO("Current Position = %f", MX28.motor_state[CURRENT_POS]);
     MX28.count += MX28.pos;
     if(MX28.count >= 6.14) MX28.count = 0;
     MX28.Estado = TX;
    }else MX28.Estado = TX;
   break;
  }
}

Let's build it with catkin_make.

$ cd ~/catkin_ws
$ catkin_make

Writing the Launch File

Roslaunch is a command that executes more than one node in the specified package or sets execution options.

The following command creates the launch file.

$ roscd dynamixel_position_control
$ mkdir launch
$ cd launch
$ gedit controller.launch

The following code should be put in controller.launch

<!-- -*- mode: XML -*- -->

<launch>
    <node name="dynamixel_manager" pkg="dynamixel_controllers" type="controller_manager.py" required="true" output="screen">
        <rosparam>
            namespace: dxl_manager
            serial_ports:
                pan_tilt_port:
                    port_name: "/dev/ttyUSB0"
                    baud_rate: 57600
                    min_motor_id: 1
                    max_motor_id: 25
                    update_rate: 20
        </rosparam>
    </node>
    <!-- Start tilt joint controller -->
    <rosparam file="$(find dynamixel_position_control)config/multi_turn.yaml" command="load"/>
    <node name="tilt_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
          args="--manager=dxl_manager
                --port pan_tilt_port
                tilt_controller"
          output="screen"/> 
   <param name="Qtd_Pos" value="16" type="int"/> 
   <node pkg="dynamixel_position_control" type="motor_controller" name="motor_controller" output="screen"/>
</launch>

Qtd_Pos is a parameter that indicates the number of motor positions in 360 degrees, the value should be between 0 and 360.

The file multi_turn.yaml is your configure file, so let's create it.

$ roscd dynamixel_position_control
$ mkdir config
$ cd config
$ gedit gedit multi_turn.yaml

The following code should be put in your multi_turn.yaml file.

tilt_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: tilt_joint
    joint_speed: 4
    motor:
        id: 1
        init: 0 
        min: 0
        max: 49152

Running the Launch File

Open the terminal and type:

roslaunch dynamixel_position_control controller.launch

If all is right, the motor must move between 0 and 2PI in 16 steps (Qtd_Pos).