Why writing 3 times the same Goal_Position in "writeCallback"?

[andreaeusebi]

Hello,

I'm usign Dyamixel SDK 3.7.21 and Dynamixel Workbench 2.1.0 on ROS. I'm usign 3 XM430-W210.

I've noticed that inside the method writeCallback of class DynamixelController there are 2 counters: point_cnt and position_cnt. I've understood that the first is used simply to iterate over the points specified by the trajectory message from /joint_trajectory topic, however, I really do not understand the role of position_cnt. I've added these instructions before the first increment of position_cnt:

  ROS_INFO("Point Counter: %d --- Position Counter: %d", point_cnt, position_cnt);
  ROS_INFO("syncWrite executed correclty. Message sent: [%d, %d, %d]", dynamixel_position[0], dynamixel_position[1], dynamixel_position[2]);

So as the last part of writeCallback method results to be:

ROS_INFO("Point Counter: %d --- Position Counter: %d", point_cnt, position_cnt);
ROS_INFO("syncWrite executed correclty. Message sent: [%d, %d, %d]", dynamixel_position[0], dynamixel_position[1], dynamixel_position[2]);

position_cnt++;
if (position_cnt >= jnt_tra_msg_->points[point_cnt].positions.size())
{
  point_cnt++;
  position_cnt = 0;
  if (point_cnt >= jnt_tra_msg_->points.size())
  {
    is_moving_ = false;
    point_cnt = 0;
    position_cnt = 0;

    ROS_INFO("Complete Execution");
  }
}

Which prints the values of the counters and the integer values commands written to the dynamixels. I set a motion time of 0.2 sec and then commanded a trajectory. This is what I get:

[ INFO] [1594983512.038498445]: Succeeded to get joint trajectory! [ INFO] [1594983512.038659365]: Point Counter: 0 --- Position Counter: 0 [ INFO] [1594983512.038718637]: syncWrite executed correclty. Message sent: [2569, 2563, 2542] [ INFO] [1594983512.041183899]: Point Counter: 0 --- Position Counter: 1 [ INFO] [1594983512.041235450]: syncWrite executed correclty. Message sent: [2569, 2563, 2542] [ INFO] [1594983512.046219068]: Point Counter: 0 --- Position Counter: 2 [ INFO] [1594983512.046276415]: syncWrite executed correclty. Message sent: [2569, 2563, 2542] [ INFO] [1594983512.051295357]: Point Counter: 1 --- Position Counter: 0 [ INFO] [1594983512.051384208]: syncWrite executed correclty. Message sent: [2568, 2562, 2541] [ INFO] [1594983512.056275426]: Point Counter: 1 --- Position Counter: 1 [ INFO] [1594983512.056341848]: syncWrite executed correclty. Message sent: [2568, 2562, 2541] [ INFO] [1594983512.061277969]: Point Counter: 1 --- Position Counter: 2 [ INFO] [1594983512.061344056]: syncWrite executed correclty. Message sent: [2568, 2562, 2541] [ INFO] [1594983512.066263643]: Point Counter: 2 --- Position Counter: 0 [ INFO] [1594983512.066339166]: syncWrite executed correclty. Message sent: [2568, 2562, 2541] [ INFO] [1594983512.071300260]: Point Counter: 2 --- Position Counter: 1 [ INFO] [1594983512.071408727]: syncWrite executed correclty. Message sent: [2568, 2562, 2541] [ INFO] [1594983512.076269745]: Point Counter: 2 --- Position Counter: 2 [ INFO] [1594983512.076333136]: syncWrite executed correclty. Message sent: [2568, 2562, 2541] [ INFO] [1594983512.081230993]: Point Counter: 3 --- Position Counter: 0 [ INFO] [1594983512.081281059]: syncWrite executed correclty. Message sent: [2565, 2559, 2538] [ INFO] [1594983512.086247542]: Point Counter: 3 --- Position Counter: 1 [ INFO] [1594983512.086311566]: syncWrite executed correclty. Message sent: [2565, 2559, 2538] [ INFO] [1594983512.091240907]: Point Counter: 3 --- Position Counter: 2 [ INFO] [1594983512.091306936]: syncWrite executed correclty. Message sent: [2565, 2559, 2538] [ INFO] [1594983512.096309415]: Point Counter: 4 --- Position Counter: 0 [ INFO] [1594983512.096390709]: syncWrite executed correclty. Message sent: [2561, 2555, 2534] [ INFO] [1594983512.101428228]: Point Counter: 4 --- Position Counter: 1 [ INFO] [1594983512.101558005]: syncWrite executed correclty. Message sent: [2561, 2555, 2534] [ INFO] [1594983512.106302990]: Point Counter: 4 --- Position Counter: 2 [ INFO] [1594983512.106362455]: syncWrite executed correclty. Message sent: [2561, 2555, 2534] ... ... [ INFO] [1594983512.561275653]: Point Counter: 35 --- Position Counter: 0 [ INFO] [1594983512.561342070]: syncWrite executed correclty. Message sent: [1734, 1733, 1733] [ INFO] [1594983512.566322629]: Point Counter: 35 --- Position Counter: 1 [ INFO] [1594983512.566387659]: syncWrite executed correclty. Message sent: [1734, 1733, 1733] [ INFO] [1594983512.571296484]: Point Counter: 35 --- Position Counter: 2 [ INFO] [1594983512.571368624]: syncWrite executed correclty. Message sent: [1734, 1733, 1733] [ INFO] [1594983512.576426926]: Point Counter: 36 --- Position Counter: 0 [ INFO] [1594983512.576587305]: syncWrite executed correclty. Message sent: [1727, 1727, 1727] [ INFO] [1594983512.581349067]: Point Counter: 36 --- Position Counter: 1 [ INFO] [1594983512.581453571]: syncWrite executed correclty. Message sent: [1727, 1727, 1727] [ INFO] [1594983512.586254652]: Point Counter: 36 --- Position Counter: 2 [ INFO] [1594983512.586307160]: syncWrite executed correclty. Message sent: [1727, 1727, 1727] [ INFO] [1594983512.591248435]: Point Counter: 37 --- Position Counter: 0 [ INFO] [1594983512.591310787]: syncWrite executed correclty. Message sent: [1723, 1723, 1723] [ INFO] [1594983512.596251120]: Point Counter: 37 --- Position Counter: 1 [ INFO] [1594983512.596307179]: syncWrite executed correclty. Message sent: [1723, 1723, 1723] [ INFO] [1594983512.601381875]: Point Counter: 37 --- Position Counter: 2 [ INFO] [1594983512.601462943]: syncWrite executed correclty. Message sent: [1723, 1723, 1723] [ INFO] [1594983512.606294279]: Point Counter: 38 --- Position Counter: 0 [ INFO] [1594983512.606375098]: syncWrite executed correclty. Message sent: [1721, 1721, 1721] [ INFO] [1594983512.611320676]: Point Counter: 38 --- Position Counter: 1 [ INFO] [1594983512.611439016]: syncWrite executed correclty. Message sent: [1721, 1721, 1721] [ INFO] [1594983512.616345269]: Point Counter: 38 --- Position Counter: 2 [ INFO] [1594983512.616401667]: syncWrite executed correclty. Message sent: [1721, 1721, 1721] [ INFO] [1594983512.621246150]: Point Counter: 39 --- Position Counter: 0 [ INFO] [1594983512.621290598]: syncWrite executed correclty. Message sent: [1720, 1720, 1720] [ INFO] [1594983512.626221018]: Point Counter: 39 --- Position Counter: 1 [ INFO] [1594983512.626266024]: syncWrite executed correclty. Message sent: [1720, 1720, 1720] [ INFO] [1594983512.631302452]: Point Counter: 39 --- Position Counter: 2 [ INFO] [1594983512.631367715]: syncWrite executed correclty. Message sent: [1720, 1720, 1720] [ INFO] [1594983512.631437892]: Complete Execution

As you can see what happens is that the same command input is given three times to the dynamixels causing the motion time to be 3 times bigger (from printed time it can be seen that the trajectory has taken 0.6 sec instead of 0.2). This seems to me to be an error honestly, I can not give any other explanation.

Moreover, I then removed the first if (the one checking dimension of position_cnt) and left only the inner if about point_cnt, the resulting code is simply:

ROS_INFO("Point Counter: %d --- Position Counter: %d", point_cnt, position_cnt);
ROS_INFO("syncWrite executed correclty. Message sent: [%d, %d, %d]", dynamixel_position[0], dynamixel_position[1], dynamixel_position[2]);

point_cnt++;
if (point_cnt >= jnt_tra_msg_->points.size())
{
  is_moving_ = false;
  point_cnt = 0;
  position_cnt = 0;

  ROS_INFO("Complete Execution");
}

Then I performed the same trajectory of before but then what I obtain is:

[ INFO] [1594984868.298619525]: Succeeded to get joint trajectory! [ INFO] [1594984868.298876701]: Point Counter: 0 --- Position Counter: 0 [ INFO] [1594984868.298976341]: syncWrite executed correclty. Message sent: [2572, 2572, 2572] [ INFO] [1594984868.302889195]: Point Counter: 1 --- Position Counter: 0 [ INFO] [1594984868.302953278]: syncWrite executed correclty. Message sent: [2571, 2571, 2571] [ INFO] [1594984868.307914250]: Point Counter: 2 --- Position Counter: 0 [ INFO] [1594984868.307984028]: syncWrite executed correclty. Message sent: [2571, 2571, 2571] [ INFO] [1594984868.312946333]: Point Counter: 3 --- Position Counter: 0 [ INFO] [1594984868.313034422]: syncWrite executed correclty. Message sent: [2568, 2568, 2568] [ INFO] [1594984868.317928062]: Point Counter: 4 --- Position Counter: 0 [ INFO] [1594984868.318005848]: syncWrite executed correclty. Message sent: [2564, 2564, 2564] [ INFO] [1594984868.322993376]: Point Counter: 5 --- Position Counter: 0 [ INFO] [1594984868.323095301]: syncWrite executed correclty. Message sent: [2558, 2558, 2558] [ INFO] [1594984868.327952484]: Point Counter: 6 --- Position Counter: 0 [ INFO] [1594984868.328039613]: syncWrite executed correclty. Message sent: [2549, 2549, 2549] [ INFO] [1594984868.332933833]: Point Counter: 7 --- Position Counter: 0 [ INFO] [1594984868.333025877]: syncWrite executed correclty. Message sent: [2537, 2537, 2537] [ INFO] [1594984868.337856242]: Point Counter: 8 --- Position Counter: 0 [ INFO] [1594984868.337915282]: syncWrite executed correclty. Message sent: [2522, 2522, 2522] ... ... [ INFO] [1594984868.447921325]: Point Counter: 30 --- Position Counter: 0 [ INFO] [1594984868.447999051]: syncWrite executed correclty. Message sent: [1808, 1808, 1808] [ INFO] [1594984868.453027634]: Point Counter: 31 --- Position Counter: 0 [ INFO] [1594984868.453151288]: syncWrite executed correclty. Message sent: [1787, 1787, 1787] [ INFO] [1594984868.457924822]: Point Counter: 32 --- Position Counter: 0 [ INFO] [1594984868.457983009]: syncWrite executed correclty. Message sent: [1769, 1769, 1769] [ INFO] [1594984868.462944876]: Point Counter: 33 --- Position Counter: 0 [ INFO] [1594984868.462999866]: syncWrite executed correclty. Message sent: [1754, 1754, 1754] [ INFO] [1594984868.467945809]: Point Counter: 34 --- Position Counter: 0 [ INFO] [1594984868.468004610]: syncWrite executed correclty. Message sent: [1743, 1743, 1743] [ INFO] [1594984868.472846147]: Point Counter: 35 --- Position Counter: 0 [ INFO] [1594984868.472892564]: syncWrite executed correclty. Message sent: [1734, 1734, 1734] [ INFO] [1594984868.477924965]: Point Counter: 36 --- Position Counter: 0 [ INFO] [1594984868.477983534]: syncWrite executed correclty. Message sent: [1727, 1727, 1727] [ INFO] [1594984868.482896615]: Point Counter: 37 --- Position Counter: 0 [ INFO] [1594984868.482960913]: syncWrite executed correclty. Message sent: [1723, 1723, 1723] [ INFO] [1594984868.487999465]: Point Counter: 38 --- Position Counter: 0 [ INFO] [1594984868.488071047]: syncWrite executed correclty. Message sent: [1721, 1721, 1721] [ INFO] [1594984868.492881866]: Point Counter: 39 --- Position Counter: 0 [ INFO] [1594984868.492946464]: syncWrite executed correclty. Message sent: [1720, 1720, 1720] [ INFO] [1594984868.492987409]: Complete Execution

Now position_cnt is always 0 since it is never incremented. Moreover if you look at time it can be noticed that the motion time of trajectory is exactly 0.2 sec as requested. The difference from before is that now each waypoint computed by trajectory generator is command only once to dynamixels while in the previous case each position was commanded three times (why??).

In conclusion, I wanted just to inform you about this. I still do not have clear if this is something that you designed expressly or it s a simple error. Please let me know something about it.

[ROBOTIS-Will]

Hi,

It looks like the position counter is used in order to check whether the platform has remaining trajectory motion and update the is_moving flag. Please modify the code for your needs if you don't need the is_moving flag. Thank you.

[rgarofalo]

I had the same problem, I don't understand the usefulness of this approach, it multiplies the duration of a movement n times based on the number of motors in a chain

[cbteeple]

I agree with @rgarofalo, this approach fundamentally breaks JointTrajectory commands with multiple motors. Since this is implemented as a subscriber to a joint trajectory topic (rather than a goal executed in an action server), top-level nodes have not way of knowing whether the trajectory is complete and are relying on timing to be accurate. If a trajectory takes N-times as long as its supposed to (where N is the number of motors), you could pre-process the trajectory and divide all times by the number of motors, then wait for the original amount of time before returning, but the scaling is not exactly N because there are other fixed times associated with sending trajectory values.

@ROBOTIS-Will When can we expect to see a resolution here? This bug persists, even in the develop branch, and I think it's about time to resolve it.