From c43c555704a9a3292256f049135619b273df97c8 Mon Sep 17 00:00:00 2001 From: Tom Noble <53005340+TSNoble@users.noreply.github.com> Date: Mon, 29 Apr 2024 12:26:44 +0100 Subject: [PATCH] Don't copy attached collision objects in CommandListManager::setStartState (#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 https://github.com/ros-planning/moveit/pull/3590#issuecomment-2079575541 / #3569. Co-authored-by: Robert Haschke --- .../src/command_list_manager.cpp | 3 ++- 1 file changed, 2 insertions(+), 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 index c5233ee221..e1f53c71fa 100644 --- 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; } }