Do not switch object and shape poses of attached objects with subframes

moveit · Dec 2, 2024 · 438609f · 438609f
1 parent e7872eb
commit 438609f
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -1817,7 +1817,7 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::
   shape_poses.reserve(num_shapes);
 
   bool switch_object_pose_and_shape_pose = false;
-  if (num_shapes == 1 && moveit::core::isEmpty(object.pose))
+  if (num_shapes == 1 && (!object.subframe_poses.size()) && moveit::core::isEmpty(object.pose))
   {
     // If the object pose is not set but the shape pose is, use the shape's pose as the object pose.
     switch_object_pose_and_shape_pose = true;