From 4488894c53c9175c08df2eadbd3ac7afcb7a3102 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Sun, 11 Aug 2024 10:23:26 -0400 Subject: [PATCH 1/4] Fix Pilz blending times... the right way (#2961) * Fix Pilz blending times... the right way * Remove more debugs * Format (cherry picked from commit 4fad0d0de6b2b5b17ecdca21923203d2b902369c) # Conflicts: # moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp # moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp --- .../src/command_list_manager.cpp | 4 ++++ .../src/plan_components_builder.cpp | 13 +++++++++---- .../src/trajectory_blender_transition_window.cpp | 5 +++++ 3 files changed, 18 insertions(+), 4 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 5f84806869..3b50c7f6d3 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 @@ -105,6 +105,10 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst // therefore: "i-1". (i > 0 ? radii.at(i - 1) : 0.)); } +<<<<<<< HEAD +======= + +>>>>>>> 4fad0d0de (Fix Pilz blending times... the right way (#2961)) return plan_comp_builder_.build(); } 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..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 @@ -54,13 +54,17 @@ 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 +98,8 @@ 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); + appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.blend_trajectory); + // 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 920c43530a..8340cae15c 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 @@ -67,7 +67,11 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( std::size_t second_intersection_index; if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index)) { +<<<<<<< HEAD RCLCPP_ERROR(LOGGER, "Blend radius to large."); +======= + RCLCPP_ERROR(getLogger(), "Blend radius too large."); +>>>>>>> 4fad0d0de (Fix Pilz blending times... the right way (#2961)) res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -124,6 +128,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 b607efd93511c7351d2bd9f15f65b3a0c566b816 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Sat, 14 Sep 2024 09:08:48 -0400 Subject: [PATCH 2/4] Update command_list_manager.cpp --- .../src/command_list_manager.cpp | 4 ---- 1 file changed, 4 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 3b50c7f6d3..5f84806869 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 @@ -105,10 +105,6 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst // therefore: "i-1". (i > 0 ? radii.at(i - 1) : 0.)); } -<<<<<<< HEAD -======= - ->>>>>>> 4fad0d0de (Fix Pilz blending times... the right way (#2961)) return plan_comp_builder_.build(); } From 368f12e4b2024f0e97d1aa56413d9f53fa05f5d1 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Sat, 14 Sep 2024 09:09:12 -0400 Subject: [PATCH 3/4] Update trajectory_blender_transition_window.cpp --- .../src/trajectory_blender_transition_window.cpp | 4 ---- 1 file changed, 4 deletions(-) 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 8340cae15c..4ed257bbf2 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 @@ -67,11 +67,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( std::size_t second_intersection_index; if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index)) { -<<<<<<< HEAD - RCLCPP_ERROR(LOGGER, "Blend radius to large."); -======= RCLCPP_ERROR(getLogger(), "Blend radius too large."); ->>>>>>> 4fad0d0de (Fix Pilz blending times... the right way (#2961)) res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } From f622ef24f44357ce2f089652b21ef7c14c608d07 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Sat, 14 Sep 2024 09:11:26 -0400 Subject: [PATCH 4/4] Update trajectory_blender_transition_window.cpp --- .../src/trajectory_blender_transition_window.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 4ed257bbf2..0fa35a84ef 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 @@ -67,7 +67,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 too large."); + RCLCPP_ERROR(LOGGER, "Blend radius too large."); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; }