Don't copy attached collision objects in CommandListManager::setStart…

…State (moveit#3590) CommandListManager::setStartState shall update the start state in the next MotionPlanRequest to match the reached robot pose after previous planning steps. Copying attached collision objects is not necessary there, because the RobotState is initialized once in the beginning. In each iteration of CommandListManager::solveSequenceItems(), only joint states actually update. Copying the attached bodies is not only a performance penalty, but also introduces a (numerical) drift of the attached object poses as pointed out in moveit#3590 (comment) / moveit#3569. Co-authored-by: Robert Haschke <[email protected]>
ubi-agni · Apr 29, 2024 · c43c555 · c43c555
1 parent c1d73c0
commit c43c555
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp
@@ -172,7 +172,8 @@ void CommandListManager::setStartState(const MotionResponseCont& motion_plan_res
   RobotState_OptRef rob_state_op{ getPreviousEndState(motion_plan_responses, group_name) };
   if (rob_state_op)
   {
-    moveit::core::robotStateToRobotStateMsg(rob_state_op.value(), start_state);
+    moveit::core::robotStateToRobotStateMsg(rob_state_op.value(), start_state, false);
+    start_state.is_diff = true;
   }
 }