fix: ensure attached objects update during motion execution #3327

MarcoMagriDev · 2025-02-06T17:09:14Z

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.

- 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 · 2025-02-06T18:52:22Z

moveit_ros/planning/plan_execution/src/plan_execution.cpp

+    auto getAttachedObjects = [](const moveit::core::RobotState& state) {
+      std::vector<const moveit::core::AttachedBody*> attached_bodies;
+      state.getAttachedBodies(attached_bodies);
+      std::map<std::string, const moveit::core::AttachedBody*> attached_objects;


Does it make sense to add this function to RobotState itself instead? Like a getAttachedBodiedMap() function?

That sounds like a good idea! Adding a getAttachedBodiesMap() function to RobotState could make it more reusable and improve clarity. Would you like to propose this as a separate PR, or do you plan to integrate it into your current changes?

I was suggesting including this in your PR, if you would be up for it?

I have just created the getAttachedBodies version that returns a map instead of a vector and updated the PR accordingly. Let me know if this aligns with your ideas!

It looks good, though I would consider not having it call the other one since it seems inefficient to create a vector, then loop through that to create a map. Maybe just go straight by looping through and putting together the map?

sea-bass · 2025-02-06T18:52:36Z

moveit_ros/planning/plan_execution/src/plan_execution.cpp

    for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i)
    {
+      state = t.getWayPoint(i);


Thank you for this!

The only simplification I could think of is that, under the assumption that "attached objects are set to the waypoint's robot state at planning time" (which actually holds) we could query sample_attached_object only once. However, I proposed updating it for each waypoint to ensure robustness, even if this assumption doesn't hold. Do you see any potential issues or improvements with this?

Ah thanks for asking.

I guess I would be thinking about:

Does it slow things down to be potentially attaching/detaching objects at each waypoint during this check?

It's difficult for me to know what the user's intent would be on a case by case basis. Right now it seems like the current state is treated as the source of truth, but I'm unsure whether users want that in every case. I guess the previous implementation did the opposite and treated the attached objects from the pre-planned waypoint as the source of truth.

Is it worth maybe adding a flag to this function for whether one wants to prioritize current state vs. planned states? And elevate this up to the config/parameter level?

… a vector

sea-bass · 2025-02-06T20:28:20Z

moveit_ros/planning/plan_execution/src/plan_execution.cpp

@@ -283,34 +283,60 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
    collision_detection::CollisionRequest req;
    req.group_name = t.getGroupName();
    req.pad_environment_collisions = false;
+    moveit::core::RobotState state = plan.planning_scene->getCurrentState();
+    std::map<std::string, const moveit::core::AttachedBody*> current_attached_objects, sample_attached_object;


Nit: sample_attached_objects (with an s at the end) for consistency.

codecov-commenter · 2025-02-06T20:46:51Z

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

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

Project coverage is 45.56%. Comparing base (af6d762) to head (20df675).
Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
...ros/planning/plan_execution/src/plan_execution.cpp	0.00%	19 Missing ⚠️
moveit_core/robot_state/src/robot_state.cpp	0.00%	7 Missing ⚠️

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

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3327      +/-   ##
==========================================
- Coverage   45.59%   45.56%   -0.03%     
==========================================
  Files         716      716              
  Lines       62400    62465      +65     
  Branches     7548     7563      +15     
==========================================
+ Hits        28445    28454       +9     
- Misses      33788    33844      +56     
  Partials      167      167

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

sea-bass reviewed Feb 6, 2025

View reviewed changes

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

feat: adds getAttachedBodies override that returns a map instead of…

20df675

… a vector

sea-bass reviewed Feb 6, 2025

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix: ensure attached objects update during motion execution #3327

fix: ensure attached objects update during motion execution #3327

MarcoMagriDev commented Feb 6, 2025

sea-bass Feb 6, 2025

MarcoMagriDev Feb 6, 2025

sea-bass Feb 6, 2025

MarcoMagriDev Feb 6, 2025

sea-bass Feb 6, 2025

sea-bass Feb 6, 2025

MarcoMagriDev Feb 6, 2025

sea-bass Feb 6, 2025

sea-bass Feb 6, 2025

codecov-commenter commented Feb 6, 2025

fix: ensure attached objects update during motion execution #3327

Are you sure you want to change the base?

fix: ensure attached objects update during motion execution #3327

Conversation

MarcoMagriDev commented Feb 6, 2025

Description

Implications

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

codecov-commenter commented Feb 6, 2025

Codecov Report