Consider attached bodies in Pilz planner #2773

[cambel]

Description

See to #2773.

The Pilz planners create a start_state based only on the robot_model stored on instantiation of the planner, thus any new attached bodies are not present during planning. The solution here is to use the current state from the planning scene instead.

Note: the planning on the Pilz planner side still succeed (and maybe it shouldn't), however, with this PR's changes the extra recheck on the planning pipeline catch the invalid plan here: https://github.com/ros-planning/moveit/blob/e8b54db3ae61a1314e49656ad84795d4cfc2e523/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp#L260-L336

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[rhaschke]

I generally approve, but I suggest to pass only the minimally required info, namely the start state (instead of the full planning scene).

[jschleicher]

A question about the general approach: In #2773 (comment) you conclude, that the start state is taken from the robot model instantiated in the constructor.
Would it be feasible, to update the TrajectoryGenerator::robot_model_ member variable from the scene on every new planning request (dropping the const-ness)? Or maybe drop the robot_model_ member completely and use scene->getRobotModel () throughout the TrajectoryGenerator class since we now have the scene available everywhere?
Could you add a testcase failing before and passing after your fix?

[cambel]

Based on @jschleicher comment, I change the code to a minimal change.

maybe drop the robot_model_ member completely and use scene->getRobotModel () throughout the TrajectoryGenerator class since we now have the scene available everywhere?

I agree these kinds of errors may be avoided by using the scene instead of a fixed robot model.

Could you add a testcase failing before and passing after your fix?

It will take me some time to prepare that, but sure I will try.

[v4hn]

I agree these kinds of errors may be avoided by using the scene instead of a fixed robot model.

This error would still have been there. Without looking through the code layers in the pilz planner, the core of this is most likely that MoveIt's MotionPlanRequest allows to add a diff to the planning scene's robot state and sadly all planners have to handle this diff field individually for historical reasons.

Nonetheless 👍 to remove robot_model_ and get it from the scene where required. This might become relevant in the ongoing debates to allow for dynamic robot models.

[codecov]

Codecov Report

Merging #2824 (79562cc) into master (7855e1f) will decrease coverage by 0.03%.
The diff coverage is 100.00%.

@@            Coverage Diff             @@
##           master    #2824      +/-   ##
==========================================
- Coverage   60.83%   60.81%   -0.02%     
==========================================
  Files         366      366              
  Lines       31717    31715       -2     
==========================================
- Hits        19293    19283      -10     
- Misses      12424    12432       +8     

Impacted Files	Coverage Δ
...z_industrial_motion_planner/trajectory_generator.h	100.00% <ø> (ø)
...strial_motion_planner/src/trajectory_generator.cpp	100.00% <100.00%> (ø)
.../ompl_interface/src/detail/constrained_sampler.cpp	43.25% <0.00%> (-16.21%)	⬇️
...meterization/work_space/pose_model_state_space.cpp	83.02% <0.00%> (-0.62%)	⬇️
...raint_samplers/src/default_constraint_samplers.cpp	80.42% <0.00%> (-0.29%)	⬇️
...bot_state/include/moveit/robot_state/robot_state.h	85.04% <0.00%> (ø)

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 7855e1f...79562cc. Read the comment docs.

[cambel]

Thanks for your comments. For now, I reverted to passing down the start state from the current planning scene, but only passing down the RobotState as suggested by @rhaschke

[rhaschke]

I further simplified the API. Thanks for your work @cambel.

[jschleicher]

Looks good to me now. Are you working on a testcase? Or should we merge as-is?

[cambel]

@jschleicher I don't really have time these days for the testcase. If it can be merged as-is, great. I will try to do the test cases by the end of September.

[welcome]

Congrats on getting your first MoveIt pull request merged and improving open source robotics!

[felixvd]

Thanks for bringing this upstream!

[riv-robot]

@cambel Thank you for this PR!