GeneratePose creates many solutions - possible option for new filter?

[joseph-moore-CO]

I'm planning a pick and place task where I use ComputeIK to generate IK solutions for both the pick and the place poses. The task contains several planning scene modifications to attach objects and adjust collisions, and a central Connect stage to join them together. The planning scene is very constrained so a large number of the IK combinations will be unable to successfully plan the connection.

In order to maintain compatibility of the two sides of the problem at the Connect stage, the place pose GeneratePose generator stage is monitoring one of the ModifyPlanningScene stages from the "pick section" which has the collision object attached.

The monitored stage has multiple solutions, so I end up creating 8 solutions for my GeneratePose stage which have slightly different notions of where the robot is, but have exactly the same pose. The wrapping ComputeIK stage then creates >150 total solutions, which bogs down the pipeline with failed Connect plans from practically the same states.

My thought was to create a wrapper stage which only passes on the first/first N solution(s). I could then wrap the GeneratePose stage with that to filter out to just one solution before passing into ComputeIK. Does this sound like a reasonable approach, and would it make a useful contribution to this project?

Perhaps a simpler approach might be to extend the GeneratePose stage to have a filter (max_num_solutions) which defaults to None?

Here's a sketch of my entire planning task.

↕ CurrentState
↓ ModifyPlanningScene "/allow_collisions"
• Connect
↑ MoveTo
↕ ComputeIK
== ↕ GeneratePose (monitors "/allow_collisions")
↓ ModifyPlanningScene "/object_attached" (adds and attaches collision object)
↓ MoveTo
↓ MoveTo
↓ ModifyPlanningScene (forbids collisions)
• Connect
↑ ModifyPlanningScene (allows collisions)
↑ MoveTo
↑ MoveTo
↑ MoveTo
↕ ComputeIK
== ↕ GeneratePose (monitors "/object_attached", generates 8 solutions)
↓ ModifyPlanningScene (detaches object)
↓ MoveTo

[rhaschke]

Creating a filter stage that only passes N solutions is definitely a useful contribution. I suggest an implementation that passes solutions lazily on compute() only - and uses sorting of incoming solutions internally. In this fashion, if solutions are incoming faster than outgoing, we would pass the N best ones instead of the N first ones.

The wrapping ComputeIK stage then creates >150 total solutions, which bogs down the pipeline with failed Connect plans from practically the same states.

In this particular case, it might be useful to have a filter stage that filters for different solutions 😉

[joseph-moore-CO]

Thanks for the suggestion. I'll have a look into making a contribution like this.

I suggest an implementation that passes solutions lazily on compute() only - and uses sorting of incoming solutions internally.