Velocity interpolation and small changes

[marshallpowell97]

Biggest change is using "velocity interpolation" to remove jerk at end of trajectories as described in #2

Small changes such as replacing magic numbers (#7) and check for VAL 3 version (#5)

Some edits to closer reflect official Staubli terminology

[gavanderhoorn]

Thanks for the PR 👍 very much appreciated.

Some minor comments and questions, with one or two things I believe should be changed.

Could you take a look at those?

And one additional comment: should velocity interpolation be done at a 'higher level'? Right now it's done in the message decoding functions, which doesn't seem like it would be the right place to do it: it's duplicated in both the traj pt files and it requires a bit of history, which I would expect to be handled at a higher level (of abstraction).

[gavanderhoorn]

@gonzalocasas and @nilsmelchert: would you perhaps see any chance to test this PR?

[gonzalocasas]

Would it be possible to split this into two PRs, one for the velocity smoothing (which looks good on a quick glance), and a second one for the small changes?

EDIT:
Because I can easily test the smaller change of the velocity smoothing, but it would take me longer to test the larger (although simpler) change.

[gavanderhoorn]

Tbh I'd rather not ask this of @marshallpowell97. He's already had to migrate his commits to this repository, after having worked on ros-industrial/staubli_experimental.

Could you perhaps cherry-pick the commits you'd be willing to test?

[gonzalocasas]

Sure, I can do that.

[gavanderhoorn]

@marshallpowell97: would you have a chance to take a look at the outstanding comments?

@gonzalocasas: have you had a chance to test the velocity/acceleration related changes?

re: cherry-picking: that won't work, as the repositories do not share a common ancestor, but you could see whether git apply can merge the relevant patches into your fork of ros-industrial/staubli_experimental.

[gavanderhoorn]

@marshallpowell97: once #9 is merged, could you rebase your PR on top? So we have CI info on this PR as well.

[marshallpowell97]

@gavanderhoorn just to make sure, should I go ahead and split this into 2 PRs?

[gavanderhoorn]

@gavanderhoorn just to make sure, should I go ahead and split this into 2 PRs?

I don't believe that would be needed.

It won't make it really easier for others to test those changes, and we already have everything in separate commits.

[gavanderhoorn]

To clarify: @marshallpowell97: in order to merge this PR we'd need to address some of my outstanding comments/questions (see the review comments).

Additionally it would be great if someone from the community (ie: @gonzalocasas or perhaps @nilsmelchert) could test it out.

[nilsmelchert]

If you tell me exactly what you want me to test, I'll be happy to do it.
What kind of trajectory should I use to show the improvement?

[gavanderhoorn]

I would say any normal trajectory, as you would normally execute.

The idea of these changes is that they make regular use of the robots better, due to better processing of trajectories.

I'm expecting the changes @marshallpowell97 introduces here together with reducing joint velocity limits to the Nominal ones (#2) to significantly improve the (perceived) quality of trajectory execution.

[gonzalocasas]

@gavanderhoorn I am testing this changes now, and I don't seen a big difference, so either I'm looking at the wrong trajectory, or the effect is rather small. I would agree with @nilsmelchert that a reference trajectory to test on would be of help.

[gavanderhoorn]

Thanks for testing.

Have you also updated the velocity limits of your model(s) to the Nominal values, instead of the maximum ones?

I would expect the biggest differences at the end of the trajectory.

[gonzalocasas]

Nope, I haven't. I'm running them with a minimal load (I haven't weighed it, but it's a very light end-effector that is not picking up anything), and if I understand correctly, that fits right in the definition of what "max" is for according to #2 (comment)

[gonzalocasas]

I tested a bit more, with a larger set of trajectories; and I think I saw the effect at the end of some. Not a very scientific observation thou ;)

[marshallpowell97]

Back on this after being MIA. @gonzalocasas The only difference that you should notice is at the very end of the trajectory. When using the maximum joint velocity limits, there is a "spike" in velocity on the very last point sent. It seems to be that the default velocity for industrial_robot_client is 0.1 when the planned velocity (for the last waypoint) is 0 (I'm not super familiar with the terminology, please correct me if I'm wrong).

Per #2, if you use the nominal values for the joint velocity limits, it doesn't seem to have much of an effect (if any), because the robot will have reached its destination by the time the last waypoint is received.

[marshallpowell97]

To give an example of the effect of the 'velocity interpolation', I took recordings of the trajectory with and without it enabled:

Disabled:

Enabled:

This was done with a "real-time" simulation, so it won't be exactly the same as a real robot, but it should get the idea across.

[gavanderhoorn]

@marshallpowell97: it'd be really great if we could move the velocity averaging to a more appropriate place, as that is about the only thing keeping this PR from being merged.

[marshallpowell97]

@gavanderhoorn anything else that needs to be fixed before merging?

[gavanderhoorn]

I've pushed a few minor fixup commits. @marshallpowell97: if you could verify everything still works (I don't have easy access to SRS here).

Also added a single in-line comment, and I'll open some issues to track remaining issues.

If/when you confirm the VAL 3 application still works @marshallpowell97, we can merge.

Big thanks for the improvements here 👍 your contributions are very much appreciated.

[gavanderhoorn]

Does this affect compatibility with older OS versions in any way?

[marshallpowell97]

Hold on, I may have done this wrong. @gavanderhoorn Did I need to make a new commit or 'review changes' on the previous one? My apologies, still learning Git...

[gavanderhoorn]

Hold on, I may have done this wrong. @gavanderhoorn Did I need to make a new commit or 'review changes' on the previous one?

To fix the identifier length limit problem you needed to commit. With the way the repository is configured right now, this leads to you dismissing the review. That was expected.

As this is your own PR, you cannot review it.

[marshallpowell97]

@marshallpowell97: if you could verify everything still works (I don't have easy access to SRS here).

I can confirm that this does still work.

[gavanderhoorn]

GH merge button was set to rebase and merge, instead of regular merge.

Manually fixed this by force-pushing to master.

[gavanderhoorn]

Thanks for iterating @marshallpowell97 👍

and thanks again for the PR.

[gonzalocasas]

@gavanderhoorn @marshallpowell97: Couldn't this end up in a negative velocity if nVel[0] == 0.0? And if so, what would the controller do? Would it skip the point completely?

I am asking because I see a concrete problem on our setup: after I cherry picked this change to my private fork (*) I started noticing that the very last point of some trajectories is never reached, at all. I am not sure this change causes it, however, it is the main suspect at the moment.

(*disclaimer: my fork is not identical, however, the joint trajectory execution part should be untouched and matching what you have here)

[gavanderhoorn]

Technically it could become negative.

In practive however, iirc, 0 velocities are not used by the nodes in the IRC. Are you using a custom client as well?

[gavanderhoorn]

@marshallpowell97: could you comment on what the controller would do in that case?

[gonzalocasas]

Are you using a custom client as well?

I am using the industrial robot client (IRC) with the modifications for cartesian trajectories I showed you long ago. In principle, I haven't changed anything on the joint trajectory handling thou, and we've been used this modified IRC client for many months and this behavior only started occurring recently. After going through changes applied to the setup, this stands out as the most likely candidate, however, I cannot 100% rule out something else.

[gavanderhoorn]

Well, I'm not claiming the code is faultless. Could you perhaps share a .bag/wireshark capture of a trajectory which exhibits the problem you're encountering?

[gavanderhoorn]

This is not really a format @marshallpowell97 can directly use.

@gonzalocasas: it would be very much appreciated if you could convert this into something like this. That would make reproduction much easier.

[gavanderhoorn]

I would also suggest to post both a .bag (with the trajectory topics as well as /joint_states) and a wireshark capture, so we have both the ROS side and the Simple Message side and can compare.

[gavanderhoorn]

Finally: let's open a new issue, as a comment on a already merged PR is not very well suited for this.

[gonzalocasas]

I added the rosbag file to the gist (https://gist.github.com/gonzalocasas/a52a63b579d8a1d8fc4ad983a319b39d#file-unfinished-trajectory_2020-03-20-14-57-09-bag). The robot is namespaced under /microphone/. The trajectory is sent directly to the service interface (joint_path_command) so I guess it does not show up in this bag file.

[gonzalocasas]

Finally: let's open a new issue, as a comment on a already merged PR is not very well suited for this.

Agree! Just created a new issue here #16 (I started off as comment, because I am unsure about the source of this, but now it is long enough to be an issue on its own)