diff --git a/core/src/storage.cpp b/core/src/storage.cpp index b5bb827c..4bdbb69a 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -237,6 +237,9 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::msg::Solution& msg, I trajectory()->getRobotTrajectoryMsg(t.trajectory); this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff); + // reset JointStates (joints are already handled in trajectories) + t.scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState(); + t.scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState(); } double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const { diff --git a/core/src/task.cpp b/core/src/task.cpp index 3e8d3715..4811c6fa 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -281,13 +281,6 @@ moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) { moveit_task_constructor_msgs::action::ExecuteTaskSolution::Goal goal; s.toMsg(goal.solution, pimpl()->introspection_.get()); - // Reset joint values in scene diff to prevent any execution capability from - // modifying the planning scene joint values - for (auto& sub_trajectory : goal.solution.sub_trajectory) { - sub_trajectory.scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState(); - sub_trajectory.scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState(); - } - moveit_msgs::msg::MoveItErrorCodes error_code; error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; auto goal_handle_future = ac->async_send_goal(goal);