Note: Test note. |
![]() |
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
Contents
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).