start_time - amount of motors multiplies the amount of time it takes to execute a joint_trajectory

[jan-krueger]

Before you open issue, please refer to ROBOTIS e-Manual

1. How to setup? N/A - I think

2. Which Dynamixel have you used? and how many? (Please describe below format to all connected Dynamixels)

   • Model Name: XM430-W350
   • ID: 1020
   • Baud Rate of Dynamixels: 57600
   • Protocol Version: 2.0

3. Write down the commands you used in order

I launch a custom launch file that looks like this:

<launch>
<arg name="usb_port"                default="/dev/ttyUSB0"/>
<arg name="dxl_baud_rate"           default="57600"/>
<arg name="namespace"               default="delta"/>

<arg name="use_moveit"              default="false"/>
<arg name="use_joint_state"         default="true"/>
<arg name="use_cmd_vel"             default="true"/>

<group ns="delta">
  <param name="dynamixel_info"          value="$(find delta)/config/setup.yaml"/>
  <node name="motor_controller" pkg="dynamixel_workbench_controllers" type="dynamixel_workbench_controllers"
        required="true" output="screen" args="$(arg usb_port) $(arg dxl_baud_rate)">
    <param name="use_moveit"              value="$(arg use_moveit)"/>
    <param name="use_joint_states_topic"  value="$(arg use_joint_state)"/>
    <param name="use_cmd_vel_topic"       value="$(arg use_cmd_vel)"/>
    <rosparam>
      publish_period: 0.01
      dxl_read_period: 0.01
      dxl_write_period: 0.01

      mobile_robot_config:
        seperation_between_wheels: 0.160
        radius_of_wheel: 0.033
    </rosparam>
  </node>

  <node name="delta_node" pkg="delta" type="delta_node" output="screen">
  </node>
</group>
</launch>

4. Copy and Paste your error message on terminal

... logging to /home/jan/.ros/log/47ea4884-04eb-11ec-af62-21885efed5f2/roslaunch-hydrogen-47043.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://hydrogen:33651/

SUMMARY
========

PARAMETERS
* /delta/dynamixel_info: /home/jan/Workspa...
* /delta/motor_controller/dxl_read_period: 0.01
* /delta/motor_controller/dxl_write_period: 0.01
* /delta/motor_controller/mobile_robot_config/radius_of_wheel: 0.033
* /delta/motor_controller/mobile_robot_config/seperation_between_wheels: 0.16
* /delta/motor_controller/publish_period: 0.01
* /delta/motor_controller/use_cmd_vel_topic: True
* /delta/motor_controller/use_joint_states_topic: True
* /delta/motor_controller/use_moveit: False
* /rosdistro: noetic
* /rosversion: 1.15.11

NODES
/delta/
  delta_node (delta/delta_node)
  motor_controller (dynamixel_workbench_controllers/dynamixel_workbench_controllers)

auto-starting new master
process[master]: started with pid [47063]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 47ea4884-04eb-11ec-af62-21885efed5f2
process[rosout-1]: started with pid [47092]
started core service [/rosout]
process[delta/motor_controller-2]: started with pid [47099]
process[delta/delta_node-3]: started with pid [47100]
[ INFO] [1629816912.059510138]: d >>> 0.010000
[ INFO] [1629816912.110783496]: Name : m1, ID : 1, Model Number : 1020
[ INFO] [1629816912.158769896]: Name : m2, ID : 2, Model Number : 1020
[ INFO] [1629816912.206745141]: Name : m3, ID : 3, Model Number : 1020
[ INFO] [1629816912.446796574]: [DynamixelDriver] Succeeded to add sync write handler
[ INFO] [1629816912.446849119]: [DynamixelDriver] Succeeded to add sync write handler
[ INFO] [1629816916.063225760]: time: 0.833
[ INFO] [1629816916.094555205]: 'm1' is ready to move
[ INFO] [1629816916.094607410]: 'm2' is ready to move
[ INFO] [1629816916.094628901]: 'm3' is ready to move
[ INFO] [1629816916.094679545]: >>> move_time 0.83
[ INFO] [1629816916.094711269]: >>> pre_goal_size_ 0.26 0.26 0.26
[ INFO] [1629816916.096232888]: Succeeded to get joint trajectory!
[ INFO] [1629816918.589085388]: Complete Execution
[ INFO] [1629816924.062125303]: time: 0.833
[ INFO] [1629816924.094146208]: 'm1' is ready to move
[ INFO] [1629816924.094199545]: 'm2' is ready to move
[ INFO] [1629816924.094222045]: 'm3' is ready to move
[ INFO] [1629816924.094255540]: >>> move_time 0.83
[ INFO] [1629816924.094279778]: >>> pre_goal_size_ 0.25 0.25 0.26
[ INFO] [1629816924.095765310]: Succeeded to get joint trajectory!
[delta/delta_node-3] process has finished cleanly
log file: /home/jan/.ros/log/47ea4884-04eb-11ec-af62-21885efed5f2/delta-delta_node-3*.log
[ INFO] [1629816926.596306717]: Complete Execution
^C[delta/motor_controller-2] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

5. Please, describe detailedly what difficulty you are in

   • Hello, I have a delta arm with three Dynamixels. I have broken my code down to the minimum in order to track down this issue, and this is what it looks like:

   double time = 2.5;
    msg.joint_names = {"m1","m2","m3"};
    msg.header.seq++;
    msg.header.stamp = ros::Time::now();
    msg.points.clear();
    Vector3d lastVel(0,0,0);
    Vector3d currentVel(0,0,0);
    currentVel = (newPos-currentPosition)/time;
    Vector3d pos(0,0,0);
    Vector3d vel(0,0,0);
    Vector3d acc(0,0,0);
    Kinematics::calculatePosVelAcc(pos,vel,acc,newPos,currentVel,(currentVel-lastVel)/time);
    trajectory_msgs::JointTrajectoryPoint point1;
    
    point1.time_from_start = ros::Duration(time);
    point1.positions.push_back(pos.x());
    point1.positions.push_back(pos.y());
    point1.positions.push_back(pos.z());

    msg.points.push_back(point1);
    jointTrajectoryPublisher.publish(msg);
    currentPosition = newPos;

I use this code to move to two different positions, and this works fine. So the kinematics are correct. The issue is that when I set e.g. the time_from_start to 2.5, 5, 10 seconds (it doesn't really matter) it always takes time_from_start * n (where n is the amount of motors that are being commanded) to actually execute the trajectory. I have tested this by adding a fourth motor and then it would multiply the time by before. I could fix this by simply dividing the time by the amount of motors but that seems hacky, and I am not sure what is going wrong.

In the sample output I set the time to 2.5/3 to test my theory, and it took 2.5s to move the arm in total.

[ROBOTIS-Will]

Hi @jan-krueger
In case of reading from multiple DYNAMIXEL modules, this could happen within several milliseconds.
If you are updating variables with the values read from DYNAMIXEL, check how long does it take to acquire those data.
Simply using "Read" instruction multiple times may work, but causes a proportional delay to the number of DYNAMIXEL.
Therefore, using Sync Read or Bulk Read instructions are strongly recommended.
https://emanual.robotis.com/docs/en/dxl/protocol2/#instruction

[cbteeple]

This is still happening for me as well, and seems to be related to #306 from 2020, and appears to have actually been fixed in PR #238 from 2019.

@ROBOTIS-Will When can we expect to see a resolution here? This is a particularly critical bug to fix because it fundamentally breaks JointTrajectory commands with multiple motors.