fix: TrajectoryBlenderTransitionWindow

- computes shift of the second_trajectory using average instead of max. (this shift may be exactly the sample_time of the second trajectory if uniform)
- reduces sampling_time that approximates continuous time to 0.001 increasing the minimum distance between points to 1mm
- ensures the addition of the first and the last points

Tests:
- adapts `checkBlendingJointSpaceContinuity` and `checkBlendingCartSpaceContinuity` acceleration computations to take into account different time samples among the trajectories
- fixes occurrences of `joint_velocity_tolerance` and `joint_accleration_tolerance` in `checkBlendingJointSpaceContinuity`
- adjust `checkBlendingCartSpaceContinuity` to return False in case of failed checks

Author: MarcoMagriDev

moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp

@@ -136,7 +136,6 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend(
 
   // adjust the time from start of the first second trajectory point to ensure continuity of velocities (this implicitly
   // ensures continuity of accelerations)
-  std::vector<double> time_offsets;
   auto second_start = res.second_trajectory->getFirstWayPointPtr();
   auto blend_end = res.blend_trajectory->getLastWayPointPtr();
 
@@ -146,17 +145,22 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend(
   std::vector<double> second_start_velocities;
   second_start->copyJointGroupVelocities(req.first_trajectory->getGroup()->getName(), second_start_velocities);
   double time_from_start = 0.0;
+  std::size_t non_zero_velocity_count = 0;
   for (std::size_t i = 0; i < second_start_velocities.size(); ++i)
   {
-    if (second_start_velocities[i] == 0)
+    if (second_start_velocities[i] != 0)
     {
-      continue;
+      time_from_start += (second_start_positions[i] - blend_end_positions[i]) / second_start_velocities[i];
+      ++non_zero_velocity_count;
     }
-    time_from_start = (second_start_positions[i] - blend_end_positions[i]) / second_start_velocities[i];
-    time_offsets.push_back(time_from_start);
   }
-  time_from_start = *std::max_element(time_offsets.begin(), time_offsets.end());
-  res.second_trajectory->setWayPointDurationFromPrevious(0, time_from_start);
+  if (non_zero_velocity_count > 0)
+  {
+    RCLCPP_INFO_STREAM(getLogger(),
+                       "Adjusting time from start of second trajectory to ensure velocity continuity. delta_time: "
+                           << time_from_start / non_zero_velocity_count);
+    res.second_trajectory->setWayPointDurationFromPrevious(0, time_from_start / non_zero_velocity_count);
+  }
   res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
   return true;
 }
@@ -303,17 +307,23 @@ void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTra
   Eigen::Isometry3d blend_sample_pose;
 
   // Define an arbitrary small sample time to sample the blending trajectory
-  double sampling_time = 0.01;
+  double sampling_time = 0.001;
 
   int num_samples = std::floor(blend_duration / sampling_time);
   sampling_time = blend_duration / num_samples;
 
   double blend_time = 0.0;
   Eigen::Isometry3d last_blend_sample_pose = blend_sample_pose1;
-  while (blend_time <= blend_duration)
+
+  // Add the first point
+  double time_offset = req.first_trajectory->getWayPointDurationFromPrevious(first_interse_index);
+  waypoint.pose = tf2::toMsg(blend_sample_pose1);
+  waypoint.time_from_start = rclcpp::Duration::from_seconds(time_offset);
+  trajectory.points.push_back(waypoint);
+  while (blend_time <= blend_duration + EPSILON)
   {
     // if the first trajectory does not reach the last sample, update
-    if ((t_start + blend_time) < req.first_trajectory->getDuration())
+    if ((t_start + blend_time) <= req.first_trajectory->getDuration())
     {
       blend_sample_pose1 = interpolatePose(req.first_trajectory, req.link_name, t_start + blend_time);
     }
@@ -338,20 +348,19 @@ void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTra
 
     blend_time += sampling_time;
     // Ensures samples are far enough apart to avoid numerical issues in numerical inverse kinematics
-    if (((blend_sample_pose.translation() - last_blend_sample_pose.translation()).norm() < EPSILON) &&
-        (blend_sample_pose.rotation().isApprox(last_blend_sample_pose.rotation(), 1e-3)))
+    if (((blend_sample_pose.translation() - last_blend_sample_pose.translation()).norm() < 1e-3) &&
+        (blend_sample_pose.rotation().isApprox(last_blend_sample_pose.rotation(), 1e-3)) &&
+        (blend_time < blend_duration))  // Force the addition of the last point
     {
       continue;
     }
 
-    // std::cout << "blend_time: " << blend_time << std::endl;
-
     // Store the last insert pose
     last_blend_sample_pose = blend_sample_pose;
 
     // push to the trajectory
     waypoint.pose = tf2::toMsg(blend_sample_pose);
-    waypoint.time_from_start = rclcpp::Duration::from_seconds(blend_time);
+    waypoint.time_from_start = rclcpp::Duration::from_seconds(time_offset + blend_time);
     trajectory.points.push_back(waypoint);
   }
 }

moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp

@@ -362,6 +362,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory(
     if (i == 0)
     {
       duration_current = trajectory.points.front().time_from_start.seconds();
+      // This still assumes all the points in first_trajectory have the same duration
       duration_last = duration_current;
     }
     else

moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp

@@ -560,7 +560,7 @@ bool testutils::toTCPPose(const moveit::core::RobotModelConstPtr& robot_model, c
 
 bool testutils::checkOriginalTrajectoryAfterBlending(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req,
                                                      const pilz_industrial_motion_planner::TrajectoryBlendResponse& res,
-                                                     const double time_tolerance)
+                                                     const double /* time_tolerance */)
 {
   for (std::size_t i = 0; i < res.first_trajectory->getWayPointCount(); ++i)
   {
@@ -679,6 +679,7 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p
 
   // check the continuity between first trajectory and blend trajectory
   trajectory_msgs::msg::JointTrajectoryPoint first_end, blend_start;
+  auto first_pend = first_traj.joint_trajectory.points[first_traj.joint_trajectory.points.size() - 2];
   first_end = first_traj.joint_trajectory.points.back();
   blend_start = blend_traj.joint_trajectory.points.front();
 
@@ -709,13 +710,16 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p
       return false;
     }
 
-    double blend_start_acc =
-        (blend_start_velo - first_end.velocities.at(i)) / rclcpp::Duration(blend_start.time_from_start).seconds();
-    if (fabs(blend_start_acc - blend_start.accelerations.at(i)) > joint_velocity_tolerance)
+    double blend_start_acc = 2 * (blend_start_velo - first_end.velocities.at(i)) /
+                             (rclcpp::Duration(blend_start.time_from_start).seconds() +
+                              rclcpp::Duration(first_end.time_from_start).seconds() -
+                              rclcpp::Duration(first_pend.time_from_start).seconds());
+    if (fabs(blend_start_acc - blend_start.accelerations.at(i)) > joint_accleration_tolerance)
     {
       std::cout << "Acceleration computed from positions/velocities are "
                    "different from the value in trajectory."
                 << '\n'
+                << "position: " << blend_start.positions.at(i) << "; " << first_end.positions.at(i)
                 << "computed: " << blend_start_acc << "; "
                 << "in trajectory: " << blend_start.accelerations.at(i) << '\n';
       return false;
@@ -724,6 +728,7 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p
 
   // check the continuity between blend trajectory and second trajectory
   trajectory_msgs::msg::JointTrajectoryPoint blend_end, second_start;
+  auto blend_pend = blend_traj.joint_trajectory.points[blend_traj.joint_trajectory.points.size() - 2];
   blend_end = blend_traj.joint_trajectory.points.back();
   second_start = second_traj.joint_trajectory.points.front();
 
@@ -746,7 +751,7 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p
   {
     double second_start_velo = (second_start.positions.at(i) - blend_end.positions.at(i)) /
                                rclcpp::Duration(second_start.time_from_start).seconds();
-    if (fabs(second_start_velo - second_start.velocities.at(i)) > joint_accleration_tolerance)
+    if (fabs(second_start_velo - second_start.velocities.at(i)) > joint_velocity_tolerance)
     {
       std::cout << "Velocity computed from positions are different from the "
                    "value in trajectory."
@@ -756,8 +761,10 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p
       return false;
     }
 
-    double second_start_acc =
-        (second_start_velo - blend_end.velocities.at(i)) / rclcpp::Duration(second_start.time_from_start).seconds();
+    double second_start_acc = 2 * (second_start_velo - blend_end.velocities.at(i)) /
+                              (rclcpp::Duration(second_start.time_from_start).seconds() +
+                               rclcpp::Duration(blend_end.time_from_start).seconds() -
+                               rclcpp::Duration(blend_pend.time_from_start).seconds());
     if (fabs(second_start_acc - second_start.accelerations.at(i)) > joint_accleration_tolerance)
     {
       std::cout << "Acceleration computed from positions/velocities are "
@@ -843,9 +850,13 @@ bool testutils::checkBlendingCartSpaceContinuity(const pilz_industrial_motion_pl
 
   // check the connection points between first trajectory and blend trajectory
   Eigen::Vector3d v_1, w_1, v_2, w_2, v_3, w_3;
-  computeCartVelocity(pose_first_end_1, pose_first_end, duration, v_1, w_1);
-  computeCartVelocity(pose_first_end, pose_blend_start, duration, v_2, w_2);
-  computeCartVelocity(pose_blend_start, pose_blend_start_1, duration, v_3, w_3);
+  double duration_first_end =
+      res.first_trajectory->getWayPointDurationFromPrevious(res.first_trajectory->getWayPointCount() - 1);
+  double duration_blend_start = res.blend_trajectory->getWayPointDurationFromPrevious(0);
+  double duration_blend_start_1 = res.blend_trajectory->getWayPointDurationFromPrevious(1);
+  computeCartVelocity(pose_first_end_1, pose_first_end, duration_first_end, v_1, w_1);
+  computeCartVelocity(pose_first_end, pose_blend_start, duration_blend_start, v_2, w_2);
+  computeCartVelocity(pose_blend_start, pose_blend_start_1, duration_blend_start_1, v_3, w_3);
 
   // translational velocity
   if (v_2.norm() > max_trans_velo)
@@ -854,6 +865,7 @@ bool testutils::checkBlendingCartSpaceContinuity(const pilz_industrial_motion_pl
                  "trajectory exceeds the limit."
               << "Actual velocity (norm): " << v_2.norm() << "; "
               << "Limits: " << max_trans_velo << '\n';
+    return false;
   }
   // rotational velocity
   if (w_2.norm() > max_rot_velo)
@@ -862,65 +874,76 @@ bool testutils::checkBlendingCartSpaceContinuity(const pilz_industrial_motion_pl
                  "trajectory exceeds the limit."
               << "Actual velocity (norm): " << w_2.norm() << "; "
               << "Limits: " << max_rot_velo << '\n';
+    return false;
   }
   // translational acceleration
-  Eigen::Vector3d a_1 = (v_2 - v_1) / duration;
-  Eigen::Vector3d a_2 = (v_3 - v_2) / duration;
+  Eigen::Vector3d a_1 = (v_2 - v_1) / duration_blend_start;
+  Eigen::Vector3d a_2 = (v_3 - v_2) / duration_blend_start_1;
   if (a_1.norm() > max_trans_acc || a_2.norm() > max_trans_acc)
   {
     std::cout << "Translational acceleration between first trajectory and "
                  "blend trajectory exceeds the limit."
-              << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; "
+              << "Actual acceleration (norm): " << a_1.norm() << ", " << a_2.norm() << "; "
               << "Limits: " << max_trans_acc << '\n';
+    return false;
   }
 
   // rotational acceleration
-  a_1 = (w_2 - w_1) / duration;
-  a_2 = (w_3 - w_2) / duration;
+  a_1 = (w_2 - w_1) / duration_blend_start;
+  a_2 = (w_3 - w_2) / duration_blend_start_1;
   if (a_1.norm() > max_rot_acc || a_2.norm() > max_rot_acc)
   {
     std::cout << "Rotational acceleration between first trajectory and blend "
                  "trajectory exceeds the limit."
-              << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; "
+              << "Actual acceleration (norm): " << a_1.norm() << ", " << a_2.norm() << "; "
               << "Limits: " << max_rot_acc << '\n';
+    return false;
   }
 
-  computeCartVelocity(pose_blend_end_1, pose_blend_end, duration, v_1, w_1);
-  computeCartVelocity(pose_blend_end, pose_second_start, duration, v_2, w_2);
-  computeCartVelocity(pose_second_start, pose_second_start_1, duration, v_3, w_3);
+  double duration_blend_end =
+      res.blend_trajectory->getWayPointDurationFromPrevious(res.blend_trajectory->getWayPointCount() - 1);
+  double duration_second_start = res.second_trajectory->getWayPointDurationFromPrevious(0);
+  double duration_second_start_1 = res.second_trajectory->getWayPointDurationFromPrevious(1);
+  computeCartVelocity(pose_blend_end_1, pose_blend_end, duration_blend_end, v_1, w_1);
+  computeCartVelocity(pose_blend_end, pose_second_start, duration_second_start, v_2, w_2);
+  computeCartVelocity(pose_second_start, pose_second_start_1, duration_second_start_1, v_3, w_3);
 
   if (v_2.norm() > max_trans_velo)
   {
     std::cout << "Translational velocity between blend trajectory and second "
                  "trajectory exceeds the limit."
               << "Actual velocity (norm): " << v_2.norm() << "; "
               << "Limits: " << max_trans_velo << '\n';
+    return false;
   }
   if (w_2.norm() > max_rot_velo)
   {
     std::cout << "Rotational velocity between blend trajectory and second "
                  "trajectory exceeds the limit."
               << "Actual velocity (norm): " << w_2.norm() << "; "
               << "Limits: " << max_rot_velo << '\n';
+    return false;
   }
-  a_1 = (v_2 - v_1) / duration;
-  a_2 = (v_3 - v_2) / duration;
+  a_1 = (v_2 - v_1) / duration_second_start;
+  a_2 = (v_3 - v_2) / duration_second_start_1;
   if (a_1.norm() > max_trans_acc || a_2.norm() > max_trans_acc)
   {
     std::cout << "Translational acceleration between blend trajectory and "
                  "second trajectory exceeds the limit."
-              << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; "
+              << "Actual acceleration (norm): " << a_1.norm() << ", " << a_2.norm() << "; "
               << "Limits: " << max_trans_acc << '\n';
+    return false;
   }
   // check rotational acceleration
-  a_1 = (w_2 - w_1) / duration;
-  a_2 = (w_3 - w_2) / duration;
+  a_1 = (w_2 - w_1) / duration_second_start;
+  a_2 = (w_3 - w_2) / duration_second_start_1;
   if (a_1.norm() > max_rot_acc || a_2.norm() > max_rot_acc)
   {
     std::cout << "Rotational acceleration between blend trajectory and second "
                  "trajectory exceeds the limit."
-              << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; "
+              << "Actual acceleration (norm): " << a_1.norm() << ", " << a_2.norm() << "; "
               << "Limits: " << max_rot_acc << '\n';
+    return false;
   }
 
   return true;