Fix Pilz blending times (backport #2961) (#3001)

Co-authored-by: Sebastian Castro <[email protected]>
moveit · Oct 29, 2024 · b98ca87 · b98ca87
1 parent 0a4f1ff
commit b98ca87
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 5 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
@@ -54,13 +54,17 @@ std::vector<robot_trajectory::RobotTrajectoryPtr> 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
@@ -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(LOGGER, "Blend radius to large.");
+    RCLCPP_ERROR(LOGGER, "Blend radius too large.");
     res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
     return false;
   }
@@ -124,6 +124,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)
   {