From 8e3ea97e2d03f76a88caa3bfd5caa0e708607aa5 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Sat, 10 Aug 2024 12:34:16 -0400 Subject: [PATCH 1/3] Fix Pilz blending times... the right way --- .../src/command_list_manager.cpp | 20 +------------------ .../src/plan_components_builder.cpp | 18 +++++++++++++---- .../trajectory_blender_transition_window.cpp | 3 ++- 3 files changed, 17 insertions(+), 24 deletions(-) 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 de829de34a..49e58df4bd 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 @@ -113,25 +113,7 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst (i > 0 ? radii.at(i - 1) : 0.)); } - const auto res_vec = plan_comp_builder_.build(); - - // De-duplicate trajectory points with the same time value. - // This is necessary since some controllers do not allow times that are not monotonically increasing. - // TODO: Ideally, we would not need this code if the trajectory segments were created without - // duplicate time points in the first place. Leaving this note to revisit this with a more principled fix. - for (const auto& traj : res_vec) - { - for (size_t i = 0; i < traj->size() - 1; ++i) - { - if (traj->getWayPointDurationFromStart(i) == traj->getWayPointDurationFromStart(i + 1)) - { - RCLCPP_WARN(getLogger(), "Removed duplicate point at time=%f", traj->getWayPointDurationFromStart(i)); - traj->removeWayPoint(i + 1); - } - } - } - - return res_vec; + return plan_comp_builder_.build(); } bool CommandListManager::checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp index 39c9d1e06c..e52efac272 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp @@ -54,13 +54,16 @@ std::vector PlanComponentsBuilder::build() void PlanComponentsBuilder::appendWithStrictTimeIncrease(robot_trajectory::RobotTrajectory& result, const robot_trajectory::RobotTrajectory& source) { - if (result.empty() || - !pilz_industrial_motion_planner::isRobotStateEqual(result.getLastWayPoint(), source.getFirstWayPoint(), - result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON)) + if (result.empty()) { result.append(source, 0.0); return; } + if (!pilz_industrial_motion_planner::isRobotStateEqual(result.getLastWayPoint(), source.getFirstWayPoint(), + result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON)) { + result.append(source, source.getWayPointDurationFromStart(0)); + return; + } for (size_t i = 1; i < source.getWayPointCount(); ++i) { @@ -94,7 +97,14 @@ void PlanComponentsBuilder::blend(const planning_scene::PlanningSceneConstPtr& p // Append the new trajectory elements appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.first_trajectory); - traj_cont_.back()->append(*blend_response.blend_trajectory, 0.0); + for (size_t i = 0; i < traj_cont_.back()->getWayPointCount(); ++i) { + std::cout << "PRE-BLEND TRAJ CONT IDX " << i << ", dt = " << traj_cont_.back()->getWayPointDurationFromStart(i) << std::endl; + } + // traj_cont_.back()->append(*blend_response.blend_trajectory, 0.0); + appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.blend_trajectory); + for (size_t i = 0; i < traj_cont_.back()->getWayPointCount(); ++i) { + std::cout << "POST-BLEND TRAJ CONT IDX " << i << ", dt = " << traj_cont_.back()->getWayPointDurationFromStart(i) << std::endl; + } // Store the last new trajectory element for future processing traj_tail_ = blend_response.second_trajectory; // first for next blending segment } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index 159640da14..ebdc3d3ee7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -70,7 +70,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( std::size_t second_intersection_index; if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index)) { - RCLCPP_ERROR(getLogger(), "Blend radius to large."); + RCLCPP_ERROR(getLogger(), "Blend radius too large."); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -127,6 +127,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( // append the blend trajectory res.blend_trajectory->setRobotTrajectoryMsg(req.first_trajectory->getFirstWayPoint(), blend_joint_trajectory); + // copy the points [second_intersection_index, len] from the second trajectory for (size_t i = second_intersection_index + 1; i < req.second_trajectory->getWayPointCount(); ++i) { From b5775bcc1f51f8b9a0538d984b51b6ebb4d1bb30 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Sat, 10 Aug 2024 12:37:51 -0400 Subject: [PATCH 2/3] Remove more debugs --- .../src/plan_components_builder.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp index e52efac272..836f618e34 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp @@ -97,14 +97,8 @@ void PlanComponentsBuilder::blend(const planning_scene::PlanningSceneConstPtr& p // Append the new trajectory elements appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.first_trajectory); - for (size_t i = 0; i < traj_cont_.back()->getWayPointCount(); ++i) { - std::cout << "PRE-BLEND TRAJ CONT IDX " << i << ", dt = " << traj_cont_.back()->getWayPointDurationFromStart(i) << std::endl; - } - // traj_cont_.back()->append(*blend_response.blend_trajectory, 0.0); appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.blend_trajectory); - for (size_t i = 0; i < traj_cont_.back()->getWayPointCount(); ++i) { - std::cout << "POST-BLEND TRAJ CONT IDX " << i << ", dt = " << traj_cont_.back()->getWayPointDurationFromStart(i) << std::endl; - } + // Store the last new trajectory element for future processing traj_tail_ = blend_response.second_trajectory; // first for next blending segment } From b3b3f683d47947ffe9ba029ab50fa3f2528b7413 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Sat, 10 Aug 2024 12:39:24 -0400 Subject: [PATCH 3/3] Format --- .../src/plan_components_builder.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp index 836f618e34..0513d22b49 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp @@ -60,7 +60,8 @@ void PlanComponentsBuilder::appendWithStrictTimeIncrease(robot_trajectory::Robot return; } if (!pilz_industrial_motion_planner::isRobotStateEqual(result.getLastWayPoint(), source.getFirstWayPoint(), - result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON)) { + result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON)) + { result.append(source, source.getWayPointDurationFromStart(0)); return; }