Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Open
andreaeusebi opened this issue Jul 17, 2020 · 3 comments
Open

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

andreaeusebi opened this issue Jul 17, 2020 · 3 comments

Comments

@andreaeusebi
Copy link

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 ROBOTIS-Will transferred this issue from ROBOTIS-GIT/DynamixelSDK Aug 21, 2020
@ROBOTIS-Will
Copy link
Contributor

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

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 cbteeple mentioned this issue Apr 25, 2022
Open
@cbteeple
Copy link

cbteeple commented Apr 25, 2022 •
edited
Loading

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
4 participants
@rgarofalo @cbteeple @ROBOTIS-Will @andreaeusebi