Generate Multiple Poses

[pawelir]

Hello, I'm trying to create a Pick Task collaborating with the output of external ML module responsible for generating a grasp pose candidates. Basically the approach is as described here.

Logic flow

1. Rely on grasp pose candidates computed externally (input to my MTC-based module)
2. Do not generate new grasp poses
3. Try to find collision free IK solution based on the given candidates
4. Ideally choose the optimal solution among possible ones

Question

1. Is it possible to achieve this with currently existing stages (ROS 2 Humble)?
2. What should be the stage sequence in MTC to achieve behavior described above?

[rhaschke]

This is perfectly possible. Have a look at the task configuration in pick_place_task.cpp. You just need to replace the GenerateGraspPose stage with a new one generating grasp poses using your ML module.

[rhaschke]

I'm not sure I understand you question correctly: Do you want to access the grasp pose used in one particular (the best) solution?
I'm afraid that's not possible currently. You could just access the robot state of the corresponding SubTrajectory of the solution (you need to know which index of solution.sub_trajectory[] to access) and compute the pose of a link.

[pawelir]

Thank you @rhaschke for swift response.

Do you want to access the grasp pose used in one particular (the best) solution?

Correct

You could just access the robot state of the corresponding SubTrajectory of the solution (you need to know which index of solution.sub_trajectory[] to access) and compute the pose of a link.

Understood. Is there any elegant way of finding stage ID by stage name or I need to hard-code either this or sub-trajectory index?

[rhaschke]

You can use Introspection::stageId() for this.

[rhaschke]

If you have access to the original solution (not only a solution msg), you could also store the grasp pose in a property field of a solution's start or end InterfaceState. MTC's GeneratePose stages already store the pose as property target_pose:

moveit_task_constructor/core/src/stages/generate_pose.cpp

Line 94 in 192a9f3

state.properties().set("target_pose", target_pose);

You can access this property via: solution.start()->properties().get<T>("target_pose")

[pawelir]

I noticed that sub_trajectory.info.comment can be used to smuggle information, so I used it to convey my Pose. However, I must admit that the approach proposed above looks more elegant and convenient, thanks 👍🏼

[pawelir]

@rhaschke, referring to the above,

While working on a custom stage, I've been relying heavily on the MoveIt Task Constructor documentation. It's great for the high-level concepts, but I feel it could be more detailed for users building their own tasks and stages.

I was thinking it would be super helpful to:

• Add more examples beyond the standard pick-and-place to show different ways of orchestrating stages.
• Expand on the "how-to" for defining custom stages, maybe with more detail on the implementation of each stage type.

Just a thought! I think it would really help lower the learning curve.

[rhaschke]

We are open to PRs extending the documentation 😉
Did you also look into the main MTC documentation: https://moveit.github.io/moveit_task_constructor?
Do you have something specific in mind already?

[pawelir]

Did you also look into the main MTC documentation: https://moveit.github.io/moveit_task_constructor?

I saw it, but did a quick scroll only. Thanks for indicating this doc, might be helpful in the future 👍🏼

Do you have something specific in mind already?

• Examples: I don't have anything specific. Some rough ideas might be assembly or surface finish task.
• How to: I meant tutorials treating how to extend existing stages with new ones, e.g. "Writing new generator", ''Writing new propagator", etc.. Similar to those ros2_control, nav2