[JTC] Non-0 velocity commanded after closed-loop execution

[egordon]

Describe the bug
PR #565 sets traj_point_active_ptr_ to nullptr after trajectory execution to avoid a race condition.
This changes the behavior of closed-loop execution from (a) continuously commanding the last trajectory waypoint, to (b) leaving the last interface command in place after the goal tolerance has been reached.

This leads to behavior where, if the goal tolerance has been reached with a non-0 velocity (likely in a closed-loop velocity-only command setting), we continue to command that velocity after reporting a successful execution.

If we with to maintain (b), we should explicitly command the final desired velocity when traj_point_active_ptr_ becomes null, I'll submit a PR to this effect.

To Reproduce
Steps to reproduce the behavior:

1. Command a trajectory to any robot with only a velocity interface and closed-loop control and non-0 goal tolerance
2. Execute the trajectory
3. Observe non-0 velocity after the Execution has been reported a success.

Expected behavior
Comment out L339.

Repeat previous steps. Observe 0 velocity after execution has been reported a success.

Screenshots
N/A
But I can provide video of our physical robot (a modified Kinova JACO2) if necessary.

Environment (please complete the following information):

• OS: Ubuntu 22.04 Jammy
• ROS Version Humble
• With current package ros-humble-joint-trajectory-controller v2.20.0-1jammy.20230522.072811

[tingelst]

I can confirm that this is also an issue for effort command interfaces. The expected behavior was replicated on a Franka Panda by commenting out L339 as proposed by @egordon.

[egordon]

That's a good point, (b) is almost certainly not the desired behavior for closed loop effort control.

So unless we have more info about how to prevent the race condition, we may just need to revert to 2.17.3 for the time being.

[christophfroehlich]

Isn't the real source of the problem that set_hold_position does not really set the commands to zero? see #558 et al.
After the call of set_hold_position, in the next run of the update() method we have again a non-null traj_point_active_ptr_ and the control loop is active again.

After the PR you proposed, the action tests fail with a segmentation fault -> see #688, but I haven't found the explanation for that yet.

[destogl]

@christophfroehlich you are right. Should we revert this merge? @bmagyar and I were probably too optimistic when agreeing to merge this.

[christophfroehlich]

See #688 (comment)

I'm not sure about the change introduced with #682 makes sense for itself, but tending to revert and bring #558

[christophfroehlich]

@egordon @tingelst Isn't this issue a duplicate of #514? At least the same behavior causing both issues IMHO.

[egordon]

@christophfroehlich Apologies, I was out the past couple of weeks.

I believe the PR that fixes #514 can fix this as well, but this issue specifically highlights successful execution (where set_hold_position() isn't called), while #514 covers unsuccessful execution (where set_hold_position() is called but doesn't do anything).

#565 exposes successful executions to 514's issue by removing the previous behavior where the final trajectory waypoint was continuously commanded.

So a PR that both fixes set_hold_position and makes it so that the previous behavior is effectively restored (i.e. final waypoint is commanded continuously, perhaps by calling set_hold_position on successful trajectory execution) should address this as well.

We'll continue to use our fork until said fix is back-ported to humble, and happy to help in any way to expedite that.

[christophfroehlich]

fyi: #558 got merged, but first will be released to rolling/iron before an eventual backport to humble.
If by chance you have a test environment with rolling, we're happy for feedback if this solves your issue.

[egordon]

While we're not on rolling, I did base our internal force gate controller off of the Rolling JTC, and it appears to solve the issue.

Thanks! I think this can either be closed now or after a humble backport.

[christophfroehlich]

It'll be backported only after some feedback on the changes from the rolling branch..