motor state publishing in position control demo significantly slower than the old dynamixel_motor package #53

jysung · 2017-06-23T12:02:25Z

While running pan and tilt demo, I noted that in the code, there is readDynamixelState function which causes bottle neck because it is calling readRegister many times.

bool PositionControl::readDynamixelState()
bool DynamixelDriver::readRegister(std::string addr_name, int32_t *value)

It has a consequence of slowing down publishing motor state to 2Hz.
On the same computer, when I use dynamixlel_motor package with same set of 2 Dynamixel motors, it can publish motor state at 30Hz. When I looked at their code, they read all the registers with a single read, is that possible in dynamxiel-workbench?

The text was updated successfully, but these errors were encountered:

routiful · 2017-06-26T00:30:05Z

Hi :)

I checked frequency of publishing data in position control demo.
when Dynamixel(XM430-W210-R) is set by 57600 baudrate, the package can publish data at 10 Hz.
And the Dynamixel is set by 1000000 baudrate, the package can publish data at 30 Hz.

I checked it using topic monitor in rqt.
Can you give me some more information about Dynamixel you used and how to check frequency.

Thanks.

jysung · 2017-06-26T06:56:24Z

I am using MX-12W and MX-64AT together both set at baudrate of 1000000.

$ rostopic hz /motor_states 
subscribed to [/motor_states]
average rate: 2.084
        min: 0.480s max: 0.480s std dev: 0.00000s window: 2
average rate: 2.084
        min: 0.480s max: 0.480s std dev: 0.00004s window: 4
average rate: 2.084
        min: 0.480s max: 0.480s std dev: 0.00005s window: 6
average rate: 2.084
        min: 0.480s max: 0.480s std dev: 0.00005s window: 8
average rate: 2.084
        min: 0.480s max: 0.480s std dev: 0.00005s window: 10

When I run the same command while running position control with dynamixel_motor code

$ rostopic hz /motor_states 
subscribed to [/motor_states]
average rate: 29.686
	min: 0.032s max: 0.080s std dev: 0.00891s window: 29
average rate: 30.506
	min: 0.032s max: 0.080s std dev: 0.00615s window: 61
average rate: 30.762
	min: 0.032s max: 0.080s std dev: 0.00501s window: 92
average rate: 30.890
	min: 0.032s max: 0.080s std dev: 0.00433s window: 123
average rate: 30.967
	min: 0.032s max: 0.080s std dev: 0.00386s window: 155

routiful · 2017-06-27T01:52:50Z

Hi :)

I tried MX-12W and MX-64AT which are set at baudrate 1000000.

[ INFO] [1498526196.591339462]: Succeeded to open the port(/dev/ttyUSB0)!
[ INFO] [1498526196.591796070]: Succeeded to change the baudrate(1000000)!
[ INFO] [1498526196.638102729]: -----------------------------------------------------------------------
[ INFO] [1498526196.638138357]: dynamixel_workbench controller; position control example(Pan & Tilt)
[ INFO] [1498526196.638159854]: -----------------------------------------------------------------------
[ INFO] [1498526196.638174412]: PAN MOTOR INFO
[ INFO] [1498526196.638195314]: ID : 1
[ INFO] [1498526196.638214379]: MODEL : MX_12W
[ INFO] [1498526196.638226668]:
[ INFO] [1498526196.638241311]: TILT MOTOR INFO
[ INFO] [1498526196.638256082]: ID : 2
[ INFO] [1498526196.638270253]: MODEL : MX_64
[ INFO] [1498526196.638289513]: -----------------------------------------------------------------------

darby@kingod ~ $ rostopic hz /position_control/dynamixel_state
subscribed to [/position_control/dynamixel_state]
average rate: 77.241
min: 0.012s max: 0.014s std dev: 0.00069s window: 77
average rate: 75.952
min: 0.012s max: 0.017s std dev: 0.00099s window: 151
average rate: 74.527
min: 0.012s max: 0.018s std dev: 0.00121s window: 223
average rate: 75.016
min: 0.012s max: 0.018s std dev: 0.00112s window: 300
average rate: 73.884
min: 0.012s max: 0.018s std dev: 0.00127s window: 369
average rate: 73.775
min: 0.012s max: 0.018s std dev: 0.00126s window: 442
average rate: 74.206
min: 0.012s max: 0.018s std dev: 0.00122s window: 519
average rate: 74.566
min: 0.012s max: 0.018s std dev: 0.00118s window: 596
average rate: 74.812
min: 0.012s max: 0.018s std dev: 0.00114s window: 673
average rate: 74.416
min: 0.012s max: 0.019s std dev: 0.00123s window: 744
average rate: 74.715
min: 0.012s max: 0.019s std dev: 0.00120s window: 822
average rate: 74.912
min: 0.012s max: 0.019s std dev: 0.00117s window: 899
average rate: 74.946
min: 0.012s max: 0.019s std dev: 0.00117s window: 912

I think you have a mistake something.
How to subscribe or publish /motor_state topic??
If i can show your code, i would tell you more

Thanks.

jysung · 2017-07-01T07:20:49Z

Okay. Good to know. I will debug my code.
And, are you running this using "master" branch or "kinetic-devel" branch?

I just pulled master branch and it seems that it stopped compiling.

/media/secondary/research/third_arm/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_toolbox/src/dynamixel_tool.cpp:47:1: error: prototype for ‘dynamixel_tool::DynamixelTool::DynamixelTool(uint8_t, uint16_t)’ does not match any in class ‘dynamixel_tool::DynamixelTool’
 DynamixelTool::DynamixelTool(uint8_t id, uint16_t model_number)
 ^
In file included from /media/secondary/research/third_arm/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_toolbox/src/dynamixel_tool.cpp:19:0:
/opt/ros/kinetic/include/dynamixel_workbench_toolbox/dynamixel_tool.h:64:7: error: candidates are: dynamixel_tool::DynamixelTool::DynamixelTool(const dynamixel_tool::DynamixelTool&)
 class DynamixelTool
       ^
/opt/ros/kinetic/include/dynamixel_workbench_toolbox/dynamixel_tool.h:92:3: error:                 dynamixel_tool::DynamixelTool::DynamixelTool(uint8_t, std::__cxx11::string, float)
   DynamixelTool(uint8_t id, std::string model_name, float protocol_version);
   ^
/opt/ros/kinetic/include/dynamixel_workbench_toolbox/dynamixel_tool.h:91:3: error:                 dynamixel_tool::DynamixelTool::DynamixelTool(uint8_t, uint16_t, float)
   DynamixelTool(uint8_t id, uint16_t model_number, float protocol_version);
   ^
/media/secondary/research/third_arm/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_toolbox/src/dynamixel_tool.cpp:60:1: error: prototype for ‘dynamixel_tool::DynamixelTool::DynamixelTool(uint8_t, std::__cxx11::string)’ does not match any in class ‘dynamixel_tool::DynamixelTool’
 DynamixelTool::DynamixelTool(uint8_t id, std::string model_name)
 ^
In file included from /media/secondary/research/third_arm/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_toolbox/src/dynamixel_tool.cpp:19:0:
/opt/ros/kinetic/include/dynamixel_workbench_toolbox/dynamixel_tool.h:64:7: error: candidates are: dynamixel_tool::DynamixelTool::DynamixelTool(const dynamixel_tool::DynamixelTool&)
 class DynamixelTool
       ^
/opt/ros/kinetic/include/dynamixel_workbench_toolbox/dynamixel_tool.h:92:3: error:                 dynamixel_tool::DynamixelTool::DynamixelTool(uint8_t, std::__cxx11::string, float)
   DynamixelTool(uint8_t id, std::string model_name, float protocol_version);
   ^
/opt/ros/kinetic/include/dynamixel_workbench_toolbox/dynamixel_tool.h:91:3: error:                 dynamixel_tool::DynamixelTool::DynamixelTool(uint8_t, uint16_t, float)
   DynamixelTool(uint8_t id, uint16_t model_number, float protocol_version);
   ^
/media/secondary/research/third_arm/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_toolbox/src/dynamixel_tool.cpp: In member function ‘bool dynamixel_tool::DynamixelTool::getModelName(uint16_t)’:
/media/secondary/research/third_arm/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_toolbox/src/dynamixel_tool.cpp:77:3: error: ‘name_path_’ was not declared in this scope
   name_path_  = ros::package::getPath("dynamixel_workbench_toolbox") + "/dynamixel/model_info.list";
   ^
dynamixel-workbench/dynamixel_workbench_toolbox/CMakeFiles/dynamixel_workbench_toolbox.dir/build.make:62: recipe for target 'dynamixel-workbench/dynamixel_workbench_toolbox/CMakeFiles/dynamixel_workbench_toolbox.dir/src/dynamixel_tool.cpp.o' failed
make[2]: *** [dynamixel-workbench/dynamixel_workbench_toolbox/CMakeFiles/dynamixel_workbench_toolbox.dir/src/dynamixel_tool.cpp.o] Error 1
CMakeFiles/Makefile2:2047: recipe for target 'dynamixel-workbench/dynamixel_workbench_toolbox/CMakeFiles/dynamixel_workbench_toolbox.dir/all' failed
make[1]: *** [dynamixel-workbench/dynamixel_workbench_toolbox/CMakeFiles/dynamixel_workbench_toolbox.dir/all] Error 2
Makefile:138: recipe for target 'all' failed
make: *** [all] Error 2

routiful · 2017-07-03T00:01:46Z

Hi :)

'kinetic-devel' branch is used to upload for official ROS packages. It points that which version is downloaded by 'apt-get install' commands.
So you can latest version using 'master' branch.

The error messages shows last version of dynamixel-workbench.
You can use this command to delete it.

$ sudo apt-get purge ros-kinetic-dynamixel-workbench*

After delete installed metapackage, clone again it your catkin_workspace.

Thanks.

jysung · 2017-07-03T19:13:07Z

Okay. I cleaned up all system install of dynamixel workbench.
And, tried the latest master branch.
This time, I just changed config file in roslaunch file.

<launch>
  <arg name="device_name"      default="/dev/ttyUSB0"/>
  <arg name="baud_rate"        default="1000000"/>
  <arg name="protocol_version" default="1.0"/>

  <arg name="pan_id"           default="14"/>
  <arg name="tilt_id"          default="7"/>

  <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>

$ roslaunch position_control.launch 
[ INFO] [1499108907.265811080]: Succeeded to open the port(/dev/ttyUSB0)!
[ INFO] [1499108907.266293428]: Succeeded to change the baudrate(1000000)!
[ INFO] [1499108907.345796961]: -----------------------------------------------------------------------
[ INFO] [1499108907.345851816]:   dynamixel_workbench controller; position control example(Pan & Tilt) 
[ INFO] [1499108907.345861532]: -----------------------------------------------------------------------
[ INFO] [1499108907.345869865]: PAN MOTOR INFO
[ INFO] [1499108907.345878198]: ID    : 14
[ INFO] [1499108907.345887935]: MODEL : MX_12W
[ INFO] [1499108907.345894997]:  
[ INFO] [1499108907.345902192]: TILT MOTOR INFO
[ INFO] [1499108907.345911397]: ID    : 7
[ INFO] [1499108907.345918163]: MODEL : MX_64
[ INFO] [1499108907.345926846]: -----------------------------------------------------------------------

And, I still only get 5Hz on this same machine (fairly powerful laptop) that can achieve 30Hz with dynamixel_motor package.

$ rostopic hz /position_control/dynamixel_state                                                      
subscribed to [/position_control/dynamixel_state]
average rate: 5.321
        min: 0.176s max: 0.192s std dev: 0.00689s window: 5
average rate: 5.254
        min: 0.176s max: 0.192s std dev: 0.00478s window: 11
average rate: 5.239
        min: 0.176s max: 0.192s std dev: 0.00397s window: 16
average rate: 5.254
        min: 0.176s max: 0.192s std dev: 0.00479s window: 21
average rate: 5.245
        min: 0.176s max: 0.192s std dev: 0.00433s window: 26
average rate: 5.267
        min: 0.160s max: 0.192s std dev: 0.00674s window: 32
average rate: 5.283
        min: 0.160s max: 0.192s std dev: 0.00707s window: 37

jysung · 2017-07-04T18:58:02Z

Here is little more debugging info in timing. I measured both package how long it takes.
When using dynamixel_motor package, def get_feedback(self, servo_id): takes up about 15ms to prepare single motor_state message.

In dynamixel_motor/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py:

    def get_feedback(self, servo_id):
        """  
        Returns the id, goal, position, error, speed, load, voltage, temperature
        and moving values from the specified servo.
        """
        # read in 17 consecutive bytes starting with low value for goal position
        response = self.read(servo_id, DXL_GOAL_POSITION_L, 17)

Found 2 motors - 1 MX-12W [14], 1 MX-64 [7], initialization complete.
(get_feedback) elapsed time in ms: 13.000000
(get_feedback) elapsed time in ms: 15.000000
(get_feedback) elapsed time in ms: 15.000000
(get_feedback) elapsed time in ms: 15.000000
(get_feedback) elapsed time in ms: 15.000000
(get_feedback) elapsed time in ms: 16.000000

Similarly, I measured times of three functions in dynamixel_workbench:

bool DynamixelDriver::readRegister(std::string addr_name, int32_t *value)
bool DynamixelMultiDriver::readMultiRegister(std::string addr_name)
bool PositionControl::readDynamixelState()

[ INFO] [1499192037.084453839]: Succeeded to open the port(/dev/ttyUSB0)!
[ INFO] [1499192037.084906530]: Succeeded to change the baudrate(1000000)!
[ INFO] [1499192037.168458791]: -----------------------------------------------------------------------
[ INFO] [1499192037.168507758]:   dynamixel_workbench controller; position control example(Pan & Tilt) 
[ INFO] [1499192037.168520322]: -----------------------------------------------------------------------
[ INFO] [1499192037.168531143]: PAN MOTOR INFO
[ INFO] [1499192037.168542895]: ID    : 14
[ INFO] [1499192037.168554621]: MODEL : MX_12W
[ INFO] [1499192037.168565413]:  
[ INFO] [1499192037.168576045]: TILT MOTOR INFO
[ INFO] [1499192037.168586456]: ID    : 7
[ INFO] [1499192037.168596203]: MODEL : MX_64
[ INFO] [1499192037.168607375]: -----------------------------------------------------------------------
(DynamixelDriver::readRegister) Time elapsed in ms: 13.932000
(DynamixelDriver::readRegister) Time elapsed in ms: 15.943000
(DynamixelMultiDriver::readMultiRegister) Time elapsed in ms: 29.939000
(DynamixelDriver::readRegister) Time elapsed in ms: 15.971000
(DynamixelDriver::readRegister) Time elapsed in ms: 16.008000
(DynamixelMultiDriver::readMultiRegister) Time elapsed in ms: 32.021000
(DynamixelDriver::readRegister) Time elapsed in ms: 16.027000
(DynamixelDriver::readRegister) Time elapsed in ms: 15.887000
(DynamixelMultiDriver::readMultiRegister) Time elapsed in ms: 31.952000
(DynamixelDriver::readRegister) Time elapsed in ms: 15.998000
(DynamixelDriver::readRegister) Time elapsed in ms: 15.968000
(DynamixelMultiDriver::readMultiRegister) Time elapsed in ms: 32.015000
(DynamixelDriver::readRegister) Time elapsed in ms: 15.962000
(DynamixelDriver::readRegister) Time elapsed in ms: 15.987000
(DynamixelMultiDriver::readMultiRegister) Time elapsed in ms: 31.980000
(DynamixelDriver::readRegister) Time elapsed in ms: 17.293000
(DynamixelDriver::readRegister) Time elapsed in ms: 15.946000
(DynamixelMultiDriver::readMultiRegister) Time elapsed in ms: 33.286000
(PositionControl::readDynamixelState) Time elapsed in ms: 191.251000

Notice that to prepare one motor_state message, it took 191 ms. However, similar to dynamixel_motor package, it only took 15 ms to read a single register from the motor. But because it is calling readRegister for every different motor state values (e.g. torque_enable, present_position, goal_position, moving, etc.), it takes total of 191ms for two motors while it should have only taken 30ms (2x 15ms).

So, I think readDynamixelState should also be changed so that it actually reads the motor value only once.

    def get_feedback(self, servo_id):
        # read in 17 consecutive bytes starting with low value for goal position

Just like get_feedback function dynamixel_motor package reads 17 consecutive bytes, can packet_handler do the same instead of separately reading a byte or two bytes at a time?

routiful · 2017-07-05T01:20:22Z

Hi :)

Thank you for your contribution. I will be check it in my environment.
Which version of ROS and Ubuntu do you use?

Thanks.

jysung · 2017-07-05T01:49:28Z

Both packages were tested with ROS Kinetic on Ubuntu 16.04.
For dynamixel-workbench, master branch was used.

Thanks!
Jae

jysung · 2017-07-17T06:05:41Z

This issue of slow publishing was happening with all the motors we had (including XM430, MX-64AT, MX-12W, AX-18A) and on all the desktop machines we tried.

I fixed dynamixel_driver such that it reads all registers at once.
In kinetic using XM430-W350-T (baudrate: 1000000), using this new method, publishing of motor states in single_dynamixel_monitor improved from 1.22Hz to 55.57Hz. (rostopic hz /dynamixel/XM430_W350)

I only fixed it for XM motors in single_dynamixel_monitor but you can take a look at my code here.
jysung@26fc2a2

routiful · 2017-07-19T05:33:20Z

Hi :)

Thank you for sharing your code.
I will be update code.

Thanks.

gjjab · 2018-05-17T07:32:56Z

Excuse me
Is this problem solved?
I also have the same problem!!
I use the latest dynamixel_workbench to controll 7 motors in ROS kinetic !!
How can I solve this problem about low frequency dynamixel state???

routiful · 2018-05-23T02:19:03Z

This closed issue might be help you #145

jysung changed the title ~~motor state publishing in pan and tilt demo significantly slower than the old dynamixel_motor package~~ motor state publishing in position control demo significantly slower than the old dynamixel_motor package Jun 23, 2017

routiful added the help wanted label Jul 4, 2017

routiful added enhancement and removed help wanted labels Sep 23, 2017

routiful added the check it later label Oct 10, 2017

Lukaszm94 mentioned this issue Jan 31, 2018

position_control crashes #103

Closed

robotpilot added accepted and removed check it later labels Apr 8, 2018

robotpilot assigned routiful Apr 8, 2018

robotpilot added in progress and removed accepted labels Apr 8, 2018

routiful added the may be this help you label Apr 8, 2018

routiful closed this as completed Apr 8, 2018

routiful removed the in progress label Apr 8, 2018

robotpilot removed the may be this help you label Apr 9, 2018

naoki-mizuno mentioned this issue Apr 9, 2018

Improve performance of the control loop resibots/dynamixel_control_hw#7

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

motor state publishing in position control demo significantly slower than the old dynamixel_motor package #53

motor state publishing in position control demo significantly slower than the old dynamixel_motor package #53

jysung commented Jun 23, 2017

routiful commented Jun 26, 2017

jysung commented Jun 26, 2017

routiful commented Jun 27, 2017

jysung commented Jul 1, 2017 •

edited

Loading

routiful commented Jul 3, 2017

jysung commented Jul 3, 2017 •

edited

Loading

jysung commented Jul 4, 2017 •

edited

Loading

routiful commented Jul 5, 2017

jysung commented Jul 5, 2017

jysung commented Jul 17, 2017 •

edited

Loading

routiful commented Jul 19, 2017

gjjab commented May 17, 2018

routiful commented May 23, 2018

motor state publishing in position control demo significantly slower than the old dynamixel_motor package #53

motor state publishing in position control demo significantly slower than the old dynamixel_motor package #53

Comments

jysung commented Jun 23, 2017

routiful commented Jun 26, 2017

jysung commented Jun 26, 2017

routiful commented Jun 27, 2017

jysung commented Jul 1, 2017 • edited Loading

routiful commented Jul 3, 2017

jysung commented Jul 3, 2017 • edited Loading

jysung commented Jul 4, 2017 • edited Loading

routiful commented Jul 5, 2017

jysung commented Jul 5, 2017

jysung commented Jul 17, 2017 • edited Loading

routiful commented Jul 19, 2017

gjjab commented May 17, 2018

routiful commented May 23, 2018

jysung commented Jul 1, 2017 •

edited

Loading

jysung commented Jul 3, 2017 •

edited

Loading

jysung commented Jul 4, 2017 •

edited

Loading

jysung commented Jul 17, 2017 •

edited

Loading