Skip to content

Fix resolved constraint frames with object as link_name#2975

Merged
rhaschke merged 5 commits intomoveit:masterfrom
captain-yoshi:fix-resolved-constraint-frames
Oct 31, 2024
Merged

Fix resolved constraint frames with object as link_name#2975
rhaschke merged 5 commits intomoveit:masterfrom
captain-yoshi:fix-resolved-constraint-frames

Conversation

@captain-yoshi
Copy link
Copy Markdown
Contributor

@captain-yoshi captain-yoshi commented Nov 25, 2021 •
edited
Loading

Description

This PR fixes #2250 for path constraint with an object as the constraint link_name. The resolve constraint frames plugin works well for planning but the constraints are not propagated/computed for the path and state validation.

Quick example to demonstrate the issue.

// Constraint frames are resolved for planning
solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res, adapter_added_state_index);
...
if (solved && res.trajectory_)
{
   // Constraint frames are NOT resolved for 'isPathValid' and 'isStateValid'
   planning_scene->isPathValid(*res.trajectory_, req.path_constraints, req.group_name, false, &index));
    ...
   planning_scene->isStateValid(robot_state, req.path_constraints, req.group_name, true);
}

The end result is that some paths are valid which should not.

PR Information

The resolved_contraint_frames plugin is removed because it is also needed by isPathValid and isStateValid. The constraint conversion is now done in the planning pipeline. The downside of this removal is that someone who only relies on constraints wrt. a link of the robot will have a small time penalty for checking if a conversion is necessary.

@captain-yoshi captain-yoshi requested a review from v4hn as a code owner November 25, 2021 19:11
@captain-yoshi
Fix constraint frames for path and state validation
306b2e7 
The isPathValid and isStateValid methos needs the resolved
@captain-yoshi
Remove obsolete resolve constraint frames plugin
425a565 
The constraint resolve frame is now done in the planning pipeline.
@captain-yoshi captain-yoshi force-pushed the fix-resolved-constraint-frames branch from 0e1b7aa to 425a565 Compare November 25, 2021 19:23
@captain-yoshi captain-yoshi mentioned this pull request Nov 25, 2021
Closed
@captain-yoshi captain-yoshi changed the title WIP: Fix resolved constraint frames with object as link_name Fix resolved constraint frames with object as link_name Nov 25, 2021
@codecov
Copy link
Copy Markdown

codecov bot commented Nov 25, 2021 •
edited
Loading

Codecov Report

Merging #2975 (425a565) into master (ab42a1d) will decrease coverage by 0.01%.
The diff coverage is 100.00%.

Impacted file tree graph

@@            Coverage Diff             @@
##           master    #2975      +/-   ##
==========================================
- Coverage   61.56%   61.56%   -0.00%     
==========================================
  Files         370      369       -1     
  Lines       33147    33140       -7     
==========================================
- Hits        20405    20400       -5     
+ Misses      12742    12740       -2
Impacted Files Coverage Δ
...anning/planning_pipeline/src/planning_pipeline.cpp 83.61% <100.00%> (+1.59%) ⬆️
.../ompl_interface/src/detail/constrained_sampler.cpp 43.91% <0.00%> (-17.07%) ⬇️
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp 76.03% <0.00%> (-2.24%) ⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp 66.88% <0.00%> (+0.13%) ⬆️
...raint_samplers/src/default_constraint_samplers.cpp 79.89% <0.00%> (+0.29%) ⬆️
moveit_core/kinematic_constraints/src/utils.cpp 32.06% <0.00%> (+0.33%) ⬆️
...meterization/work_space/pose_model_state_space.cpp 81.77% <0.00%> (+1.77%) ⬆️
...ipulation/pick_place/src/manipulation_pipeline.cpp 74.39% <0.00%> (+2.48%) ⬆️
...eit_ros/manipulation/pick_place/src/pick_place.cpp 92.39% <0.00%> (+3.81%) ⬆️

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 ab42a1d...425a565. Read the comment docs.

@captain-yoshi captain-yoshi changed the title Fix resolved constraint frames with object as link_name WIP: Fix resolved constraint frames with object as link_name Nov 25, 2021
@captain-yoshi

This comment was marked as outdated.

Sign in to view
@captain-yoshi
Copy link
Copy Markdown
Contributor Author

captain-yoshi commented Dec 2, 2021

Brainstorming... Would it makes sense to put this section of code as request adapters ? That way it would get the modified request with the modified frames. Also the check_solution_paths_ and the display_computed_motion_plans_ could be changed at runtime.

@rhaschke rhaschke force-pushed the master branch 5 times, most recently from e1b0ec1 to fdc3b1c Compare July 28, 2023 10:37
@captain-yoshi captain-yoshi mentioned this pull request Oct 21, 2024
Merged
1 task
@rhaschke rhaschke self-requested a review as a code owner October 31, 2024 10:51
@codecov-commenter
Copy link
Copy Markdown

codecov-commenter commented Oct 31, 2024

⚠️ Please install the 'codecov app svg image' to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 47.81%. Comparing base (c174715) to head (1eb1311).
Report is 46 commits behind head on master.

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files 
@@            Coverage Diff             @@
##           master    #2975      +/-   ##
==========================================
+ Coverage   47.15%   47.81%   +0.67%     
==========================================
  Files         603      604       +1     
  Lines       58999    60979    +1980     
  Branches     6969     7092     +123     
==========================================
+ Hits        27814    29151    +1337     
- Misses      30773    31410     +637     
- Partials      412      418       +6

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

@captain-yoshi captain-yoshi changed the title WIP: Fix resolved constraint frames with object as link_name Fix resolved constraint frames with object as link_name Oct 31, 2024
@rhaschke rhaschke merged commit f1a6071 into moveit:master Oct 31, 2024
@riv-mjohnson riv-mjohnson mentioned this pull request Feb 5, 2025
Closed
This was referenced May 26, 2025
Closed
Closed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

@rhaschke rhaschke rhaschke approved these changes

@v4hn v4hn Awaiting requested review from v4hn v4hn is a code owner

Assignees

No one assigned

Labels

None yet

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

Path constraints not working with attached body

3 participants

@captain-yoshi @codecov-commenter @rhaschke