Skip to content

fix: ensure attached objects update during motion execution#3327

Merged
sjahr merged 10 commits intomoveit:mainfrom
MarcoMagriDev:fix/collision-monitoring-attached-objects-not-updating
Apr 3, 2025
Merged

fix: ensure attached objects update during motion execution#3327
sjahr merged 10 commits intomoveit:mainfrom
MarcoMagriDev:fix/collision-monitoring-attached-objects-not-updating

Conversation

@MarcoMagriDev
Copy link
Copy Markdown
Contributor

@MarcoMagriDev MarcoMagriDev commented Feb 6, 2025

Description

  • Check that attached objects in the monitored robot match those in the planned trajectory.
  • If an object disappears from the monitored robot, remove it from the trajectory way-point robot_state. (avoid false positives, stop trajectories due to collisions between environment and objects that are not attached to the robot anymore)
  • If an object is attached to the monitored robot but missing in the trajectory, add it to enable meaningful collision checking. (avoids disruptive condition in which even if the object has been added to the robot as attached collision object it is not considered for online collision checking)

Implications

This could impact a few different situations:

  • Imagine you’re catching an object during motion, and an external node tells the planning scene to add the new attached collision object. Even though MoveIt knows about the new attached object, the collision check still only considers the attached objects that were there at the planning stage, so it misses the new one. This could lead to a situation where a collision is missed because the newly attached object isn’t part of the check.
  • Similarly, dropping an object during motion may result in preempting a valid path due to a non existsent collision.

@MarcoMagriDev
fix: ensure attached objects update during motion execution
b94e50b 
- Check that attached objects in the monitored robot match those in the planned trajectory.
- If an object disappears from the monitored robot, remove it from the trajectory waypoint robot_state.
- If an object is attached to the monitored robot but missing in the trajectory, add it to enable meaningful collision checking.
sea-bass
sea-bass reviewed Feb 6, 2025
View reviewed changes
@sea-bass sea-bass added backport-humble Mergify label that triggers a PR backport to Humble backport-jazzy Mergify label that triggers a PR backport to Jazzy labels Feb 6, 2025
sea-bass
sea-bass reviewed Feb 6, 2025
View reviewed changes
@codecov-commenter
Copy link
Copy Markdown

codecov-commenter commented Feb 6, 2025 •
edited by codecov bot
Loading

Codecov Report

Attention: Patch coverage is 0% with 42 lines in your changes missing coverage. Please review.

Project coverage is 46.22%. Comparing base (49ff5d8) to head (2777007).
Report is 1 commits behind head on main.

Files with missing lines Patch % Lines
...ros/planning/plan_execution/src/plan_execution.cpp 0.00% 37 Missing ⚠️
moveit_core/robot_state/src/robot_state.cpp 0.00% 5 Missing ⚠️
Additional details and impacted files 
@@            Coverage Diff             @@
##             main    #3327      +/-   ##
==========================================
+ Coverage   45.87%   46.22%   +0.36%     
==========================================
  Files         717      717              
  Lines       62592    62648      +56     
  Branches     7570     7581      +11     
==========================================
+ Hits        28708    28953     +245     
+ Misses      33718    33528     -190     
- Partials      166      167       +1

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

🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

MarcoMagriDev and others added 3 commits February 7, 2025 16:06
@MarcoMagriDev
fix: removes vector allocation in getAttachedBodies map override
99bdd41 
style: `sample_attached_object` -> `sample_attached_objects`
@MarcoMagriDev
Merge branch 'main' into fix/collision-monitoring-attached-objects-no…
b63718d 
…t-updating
@MarcoMagriDev
feat: automatically determine whether attached objects need to be re-…
8ccb626 
…queried based on their consistency in the planned trajectory.

---
Before entering the monitoring phase, it checks whether the attached objects remain consistent throughout the trajectory. If they do, they are stored and later used by the isRemainingPathValid method without needing to be queried again. If the attached objects change during the planned trajectory, the map is left empty, signaling isRemainingPathValid to query them at each waypoint.
sea-bass
sea-bass reviewed Feb 12, 2025
View reviewed changes
Copy link
Copy Markdown
Contributor

@sea-bass sea-bass left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Mostly looks good to me, with some minor comments!

Also tagged a few other maintainers for a second look.

@MarcoMagriDev
Copy link
Copy Markdown
Contributor Author

MarcoMagriDev commented Mar 26, 2025

Hey all, just checking in on this PR. It’s been open for a bit—let me know if I need to update anything to move it forward.

@sjahr sjahr enabled auto-merge (squash) April 3, 2025 07:29
@sjahr sjahr disabled auto-merge April 3, 2025 07:30
@sjahr sjahr enabled auto-merge (squash) April 3, 2025 07:30
@sjahr sjahr merged commit 89acadd into moveit:main Apr 3, 2025
10 checks passed
mergify bot pushed a commit that referenced this pull request Apr 3, 2025
@MarcoMagriDev @mergify
fix: ensure attached objects update during motion execution (#3327)
de3b775 
(cherry picked from commit 89acadd)

# Conflicts:
#	moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp
#	moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.hpp
#	moveit_ros/planning/plan_execution/src/plan_execution.cpp
@mergify mergify bot mentioned this pull request Apr 3, 2025
Open
mergify bot pushed a commit that referenced this pull request Apr 3, 2025
@MarcoMagriDev @mergify
fix: ensure attached objects update during motion execution (#3327)
2f264c6 
(cherry picked from commit 89acadd)
@mergify mergify bot mentioned this pull request Apr 3, 2025
Merged
sea-bass pushed a commit that referenced this pull request Apr 4, 2025
@mergify @MarcoMagriDev
fix: ensure attached objects update during motion execution (#3327) (#…
63e7bfa 
…3414)

(cherry picked from commit 89acadd)

Co-authored-by: Marco Magri <94347649+MarcoMagriDev@users.noreply.github.com>
sussybot5258 pushed a commit to GreyCatAI/moveit2 that referenced this pull request Apr 19, 2025
@MarcoMagriDev @sussybot5258
fix: ensure attached objects update during motion execution (moveit#3327
16dd792 
)
@MarcoMagriDev MarcoMagriDev deleted the fix/collision-monitoring-attached-objects-not-updating branch April 23, 2025 13:56
@MarqRazz MarqRazz mentioned this pull request May 6, 2025
Merged
This was referenced May 12, 2025
Closed
Merged
Markus-Simonsen pushed a commit to Markus-Simonsen/moveit2 that referenced this pull request Aug 12, 2025
@MarcoMagriDev
fix: ensure attached objects update during motion execution (moveit#3327
60f6091 
)
helen9975 pushed a commit to personalrobotics/moveit2 that referenced this pull request Feb 17, 2026
@MarcoMagriDev
fix: ensure attached objects update during motion execution (moveit#3327
2850294 
)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

@sjahr sjahr sjahr approved these changes

@henningkayser henningkayser Awaiting requested review from henningkayser

+1 more reviewer

@sea-bass sea-bass sea-bass left review comments

Reviewers whose approvals may not affect merge requirements

Assignees

No one assigned

Labels

backport-humble Mergify label that triggers a PR backport to Humble backport-jazzy Mergify label that triggers a PR backport to Jazzy

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

4 participants

@MarcoMagriDev @codecov-commenter @sea-bass @sjahr