Attach functions to stages

[dave992]

The current stages of this package seem to be focussing purely on the motion aspect of a task and do not seem to incorporate ways to call functions at the start, during, or at the end of a motion stage. Having this ability would allow for far more complex tasks, which interact with other equipment via IO, service calls or other custom functions.

Are there any plans for incorporating function stages, of attaching functions to the current motion stages?

Do you have any recommendations on how this could be achieved if this is currently not planned? I was thinking of writing a custom stage for this purpose.

Thanks in advance!

[v4hn]

The current stages of this package seem to be focussing purely on the motion aspect of a task

This is not true, as any kind of scene updates are specifically incorporated.

and do not seem to incorporate ways to call functions at the start, during, or at the end of a motion stage.

Yes, this is because the default execution happens by sending the whole sequence of trajectories to the move_group node for execution. Passing functions there is not feasible.

Currently, the trivial way to add arbitrary user-code between stages is by splitting up the final solution message before sending the separate parts for execution one after the other.

A rather ugly alternative is to add auxiliary objects to the scene to indicate progress through the execution. The object appearance can be used as a trigger.

The third, in a sense "proper", way to go is to fully move away from the move_group node altogether and allow users to add custom code hooks between stages.
We will probably go with this option in the future, but I will not make promises on time frames.

[v4hn]

One more addendum: If you want to activate / deactivate end-effectors (such as suction grippers), the preferred way should be to add custom GripperCommand or FollowJointTrajectory actions for virtual joints and support them through MoveIt.

This way, you can also run simulation properly and debug issues from motion traces.

[dave992]

Thank you for your reply and the explanation!

Yes, this is because the default execution happens by sending the whole sequence of trajectories to the move_group node for execution. Passing functions there is not feasible.

That makes sense indeed.

Currently, the trivial way to add arbitrary user-code between stages is by splitting up the final solution message before sending the separate parts for execution one after the other.

This solution should be useful for most of the more complex tasks I envisioned. I will have a look at how this can be done.

The third, in a sense "proper", way to go is to fully move away from the move_group node altogether and allow users to add custom code hooks between stages. We will probably go with this option in the future, but I will not make promises on time frames.

Are there any plans or ideas on how this would be done? Just asking to see if I could be of assistance or that some of my work may be used for this in the future.

One more addendum: If you want to activate / deactivate end-effectors (such as suction grippers), the preferred way should be to add custom GripperCommand or FollowJointTrajectory actions for virtual joints and support them through MoveIt.

Some of the tasks would indeed look at this kind of behavior, but we also intend to use other sensors or tools such as dept cameras, laser line scanners, and force-torque sensors.

[felixvd]

This solution should be useful for most of the more complex tasks I envisioned. I will have a look at how this can be done.

Just for reference, #192 is the way we worked around this, as everything else seemed to involve much more overhead. Did you solve this differently or have any updates to share?