Joint trajectory controller: Keep executing trajectory when aborted?

[adolfo-rt]

The joint trajectory controller implements a behavior inherited from its predecessor that I'd like to discuss. Currently, when a trajectory goal is aborted because of a (path or goal) tolerance violation, its execution is not canceled, but actually keeps on going as best as possible. The first time I saw this it struck me as unexpected behavior, but I learned to live with it. Now that I'm documenting the controller in the ros wiki, the issue came back to my mind.

I think that a good safety layer should not live in the controllers but in software closer to the hardware (enforcing joint limits, reflex emergency behaviors, etc.) and mechanical levels (low inertia, passive compliance, soft covers ,etc.).

For now I'm just stating the expected controller behavior clearly in the doc, and that the controller is not intended to enforce low level safety.

Any thoughts on this?.

[davetcoleman]

I've noticed this too an its confused me as well. I'd prefer it to just stop, unless perhaps with some option you explicitly ask it to continue trying.

[adolfo-rt]

I don't think we need a new option. You can currently prevent the robot from stopping at mid-trajectory by simply not specifying path tolerances in your controller (yaml) configuration. Setting ample goal tolerances would also allow for approximate goal configurations.

At best, we can improve the documentation, maybe adding a "Understanding tolerances" subpage in the controller wiki page.

This is not an API-breaking change, but it is a behavior-breaking change, so I wonder if we should slip it in now or wait until Indigo. I lean towards doing it now, as there's probably not that many users of the controller (it just got released) and tolerances --especially path tolerances-- are seldom used in practice. Any objections?.

I'll be away for the next three weeks, so I could take on this on my return. The implementation is trivial, but I'd like the behavior to be verified in the unit tests.

[davetcoleman]

No objections here.

[jim-rothrock]

Yes, you should modify the code now so that motion stops when there is a tolerance violation. That way, people (like me) will not get accustomed to coding in a "Just ignore the action result, you can just assume the arm reached the goal" manner.

[adolfo-rt]

Or even worse (dark humor note), Robot says: I just aborted the goal because I ran into a person. I'm letting you know, but I'll keep on going...

So, roger that. I'll push a fix when I'm back in three weeks.

[davetcoleman]

👍 dark humor note

[waldezjr]

+1 on the issue and +1 on the dark humor note =]

[mitchellwills]

Was this ever resolved?

[davetcoleman]

With indigo release looming, this is a good time to fix it indeed.

[mitchellwills]

Is this also present in the gripper_action_controller?

[adolfo-rt]

Was this ever resolved?

No, not yet. The fix will be in Indigo probably in the next patch release. Expect a few weeks before this gets done (code change, test suite update). If there is urgency for the patch, consider submitting a PR and I'll review it.

[adolfo-rt]

Is this also present in the gripper_action_controller?

It might. When the controller times out, it aborts the goal, but no additional action is taken. In the joint_trajectory_controller we should hold position when the goal aborts, I don't know if this would be the right policy for the gripper_action_controller, I expect @sachinchitta (the author) to know better.

Also, consider opening a separate issue for discussing gripper_action_controller specifics in detail, to prevent this thread from going off-topic.

[dcconner]

What was the resolution of this thread?

I was wondering about adding safety protection from sending a trajectory that has a large initial error (causing robot to jerk). If the trajectory will abort and hold CURRENT position, then it would seem to satisfy the need to protect against inadvertent commands and deviations due to disturbance (e.g. hitting a person per dark humor example above).

[adolfo-rt]

For the record, what must be done is to cancel trajectory execution when path and goal tolerance checks are performed.

Canceling trajectory execution consists of calling the setHoldPosition(time) method after aborting the goal.

I'd advise to first write the tests (that exercises both abort triggers, path and goal constraints), see that they fail, then implement the fix, and verify that tests now pass.

[fmessmer]

I tried to implement your proposed procedure here.
While the desired position of the controller is set as expected after the action is aborted (i.e. stays constant at the current position rather then moving towards the original trajectory goal position), however, now I don't receive an action result message anymore for the action that has been aborted....
Thus, e.g. MoveIt! transitions the goal to LOST after a while...

Any ideas what is missing/not correct?

I'd advise to first write the tests (that exercises both abort triggers, path and goal constraints), see that they fail, then implement the fix, and verify that tests now pass.

As to the tests:
I think we could simply use the tests already implemented here_path and here_goal.
However, I'm not sure what the additional EXPECT clauses should look like?
Any suggestion?

[fmessmer]

ping
I'm willing to look into this again, but any hint would be appreciated...

[dcconner]

I'm looking into this as well. It appears that after the rt_active_goal_ is reset, the RealtimeServerGoalHandle is destroyed and therefore the runNonRealtime function is never called to publish the result.

This would apparently be a race condition between the checktolerances function exiting and the runNonRealtime getting called by the timer.

One idea I had was to pass the rt_sement_goal_ pointer into the setAborted/Succeded function instead of just the result, storing that with the req_result_, then resetting that pointer after the runNonRealtime is called.

Thoughts?

[onionsflying]

I think it's a good idea to call the setHoldPosition(time) method after aborting a goal. Man can't cancel a goal by publishing a cancel topic after the goal is already aborted. That means a moving robot won't stop when you want it to stop. So there is a safety issue. Is there anyone also working on it now?

[dcconner]

I would like to revisit this issue as I have a state machined based implementation that needs to halt on error.

I made the following change to get our simulation set up working:
CNURobotics@569ef90?diff=unified

I'm holding off on issuing a pull request given that similar fixes have been rejected in the past, but would like to prompt a more detailed discussion.

The setHoldPosition method comment says that it is real time safe, and there are no obvious blockers in any of the methods that it calls, so what is the problem referenced above.

(Note that I was calling setHold before canceling the goal as some had tried.)

The existing unit tests seem to run OK on my system, but I know that is no guarantee with realtime performance.

Note: For goal tolerance, it should keep the same terminal command, so I did not see the need to call setHold there.

[mathias-luedtke]

The problem is/was

however, now I don't receive an action result message anymore for the action that has been aborted....

Your code looks alright (it's the same that we dicussed back then).
However, we need a test to ensure that the action result was received.