Test/pilz interpolation params

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp

@@ -51,6 +51,7 @@
 #include <pilz_industrial_motion_planner/cartesian_trajectory.hpp>
 #include <pilz_industrial_motion_planner/limits_container.hpp>
 #include <pilz_industrial_motion_planner/trajectory_generation_exceptions.hpp>
+#include <pilz_industrial_motion_planner/interpolation_parameters.hpp>
 
 namespace pilz_industrial_motion_planner
 {
@@ -112,6 +113,17 @@ bool verifySampleJointLimits(const std::map<std::string, double>& position_last,
                              const std::map<std::string, double>& position_current, double duration_last,
                              double duration_current, const JointLimitsContainer& joint_limits);
 
+/**
+ * @brief Compute time samples for a given trajectory based on interpolation parameters.
+ *
+ * @param trajectory The input KDL trajectory.
+ * @param interpolation_params Parameters for interpolation.
+ * @param time_samples Output vector of time samples.
+ * @return true if time samples are successfully computed, false otherwise.
+ */
+bool computeTimeSamples(const KDL::Trajectory& trajectory, const interpolation::Params& interpolation_params,
+                        std::vector<double>& time_samples);
+
 /**
  * @brief Interpolates between two poses.
  *
@@ -146,7 +158,8 @@ void interpolate(const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& e
 bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr& scene,
                              const JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory,
                              const std::string& group_name, const std::string& link_name,
-                             const std::map<std::string, double>& initial_joint_position, double sampling_time,
+                             const std::map<std::string, double>& initial_joint_position,
+                             const std::vector<double>& time_samples,
                              trajectory_msgs::msg::JointTrajectory& joint_trajectory,
                              moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision = false);
 
@@ -166,6 +179,7 @@ bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr& scene,
                              const std::string& group_name, const std::string& link_name,
                              const std::map<std::string, double>& initial_joint_position,
                              const std::map<std::string, double>& initial_joint_velocity,
+                             const double& last_sample_duration,
                              trajectory_msgs::msg::JointTrajectory& joint_trajectory,
                              moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision = false);
 

moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp

@@ -94,12 +94,13 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend(
     initial_joint_velocity[joint_name] =
         req.first_trajectory->getWayPoint(first_intersection_index - 1).getVariableVelocity(joint_name);
   }
+  double duration_last_sample = req.first_trajectory->getWayPointDurationFromPrevious(first_intersection_index - 1);
   trajectory_msgs::msg::JointTrajectory blend_joint_trajectory;
   moveit_msgs::msg::MoveItErrorCodes error_code;
 
   if (!generateJointTrajectory(planning_scene, limits_.getJointLimitContainer(), blend_trajectory_cartesian,
                                req.group_name, req.link_name, initial_joint_position, initial_joint_velocity,
-                               blend_joint_trajectory, error_code))
+                               duration_last_sample, blend_joint_trajectory, error_code))
   {
     // LCOV_EXCL_START
     RCLCPP_INFO(getLogger(), "Failed to generate joint trajectory for blending trajectory.");

moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp

@@ -215,11 +215,146 @@ void pilz_industrial_motion_planner::interpolate(const Eigen::Isometry3d& start_
   interpolated_pose.linear() = quat1.slerp(interpolation_factor, quat2).toRotationMatrix();
 }
 
+bool pilz_industrial_motion_planner::computeTimeSamples(const KDL::Trajectory& trajectory,
+                                                        const interpolation::Params& interpolation_params,
+                                                        std::vector<double>& time_samples)
+{
+  const double epsilon = 10e-6;
+  time_samples.clear();
+
+  // auto start_time = std::chrono::high_resolution_clock::now();
+  double t = 0;
+  double prev_time = 0.0;
+  double traj_duration = trajectory.Duration();
+
+  if (traj_duration <= epsilon)
+  {
+    time_samples.push_back(0.0);
+    return true;
+  }
+
+  // Get the initial position
+  KDL::Frame prev_frame, current_frame = trajectory.Pos(t);
+  KDL::Frame last_frame = trajectory.Pos(traj_duration);
+
+  // Compute the total translation and rotation distance
+  double total_translation_distance = (last_frame.p - current_frame.p).Norm();
+  double total_rotation_distance = (last_frame.M * current_frame.M.Inverse()).GetRot().Norm();
+
+  bool has_translation = total_translation_distance > interpolation_params.min_translation_interpolation_distance;
+  bool has_rotation = total_rotation_distance > interpolation_params.min_rotation_interpolation_distance;
+
+  if (!has_translation && !has_rotation)
+  {
+    RCLCPP_ERROR(getLogger(), "Total translation and rotation distance are too small.");
+    return false;
+  }
+
+  // Compute the number of samples based on the maximum sample time, translation and rotation distance
+  int n_sample_duration = std::ceil(traj_duration / interpolation_params.max_sample_time);
+  int n_sample_translation =
+      std::ceil(total_translation_distance / (interpolation_params.max_translation_interpolation_distance));
+  int n_sample_rotation =
+      std::ceil(total_rotation_distance / (interpolation_params.max_rotation_interpolation_distance));
+
+  // Get the maximum number of samples
+  int n_samples = std::max({ n_sample_duration, n_sample_translation, n_sample_rotation });
+
+  // Compute the target translation and rotation distance based on the number of samples
+  double target_translation_distance = total_translation_distance / n_samples;
+  double target_rotation_distance = total_rotation_distance / n_samples;
+  double target_max_sample_time = traj_duration / n_samples;
+
+  if (target_translation_distance < interpolation_params.min_translation_interpolation_distance &&
+      target_rotation_distance < interpolation_params.min_rotation_interpolation_distance)
+  {
+    double min_feasible_max_sample_time = traj_duration / std::max(n_sample_translation, n_sample_rotation);
+    RCLCPP_ERROR_STREAM(getLogger(),
+                        "Translation and rotation distance are too small. The provided sampling time is too "
+                        "small min value is "
+                            << min_feasible_max_sample_time);
+    return false;
+  }
+
+  if (target_translation_distance < interpolation_params.min_translation_interpolation_distance && !has_translation)
+  {
+    RCLCPP_DEBUG(getLogger(), "Translation distance is too small, set to minimum value to ensure no numerical errors.");
+    target_translation_distance = epsilon;
+  }
+  if (target_rotation_distance < interpolation_params.min_rotation_interpolation_distance && !has_rotation)
+  {
+    RCLCPP_DEBUG(getLogger(), "Rotation distance is too small, set to minimum value to ensure no numerical errors.");
+    target_rotation_distance = epsilon;
+  }
+
+  RCLCPP_DEBUG_STREAM(getLogger(), "target translation distance: "
+                                       << target_translation_distance << ", target rotation distance: "
+                                       << target_rotation_distance << ", max sample time: " << target_max_sample_time
+                                       << ", max sampling time: " << interpolation_params.max_sample_time);
+
+  double translation_distance_from_previous, rotation_distance_from_previous;
+  double translation_distance_from_next, rotation_distance_from_next;
+  double translation_distance_from_last, rotation_distance_from_last;
+  // Set the time_step to a fraction of the interpolation adjusted max sample time
+  double time_step = std::max(std::min(0.001, target_max_sample_time / 10), epsilon);
+  time_samples.push_back(0.0);
+  while (t + time_step < traj_duration)
+  {
+    // Increment the time by the time step
+    t += time_step;
+
+    // Get the current position at time t
+    current_frame = trajectory.Pos(t);
+
+    // Compute the distance from the previous position
+    translation_distance_from_previous = (current_frame.p - prev_frame.p).Norm();
+    rotation_distance_from_previous = (prev_frame.M * current_frame.M.Inverse()).GetRot().Norm();
+
+    if (translation_distance_from_previous < interpolation_params.min_translation_interpolation_distance &&
+        rotation_distance_from_previous < interpolation_params.min_rotation_interpolation_distance)
+    {
+      continue;
+    }
+
+    KDL::Frame next_frame = trajectory.Pos(t + time_step);
+    translation_distance_from_next = (next_frame.p - prev_frame.p).Norm();
+    rotation_distance_from_next = (prev_frame.M * next_frame.M.Inverse()).GetRot().Norm();
+
+    if (translation_distance_from_previous >= target_translation_distance ||
+        rotation_distance_from_previous >= target_rotation_distance || (t - prev_time) >= target_max_sample_time ||
+        translation_distance_from_next > interpolation_params.max_translation_interpolation_distance ||
+        rotation_distance_from_next > interpolation_params.max_rotation_interpolation_distance)
+    {
+      translation_distance_from_last = (current_frame.p - last_frame.p).Norm();
+      rotation_distance_from_last = (last_frame.M * current_frame.M.Inverse()).GetRot().Norm();
+      // Ensures last point is valid
+      if ((has_translation &&
+           translation_distance_from_last < interpolation_params.min_translation_interpolation_distance) &&
+          (has_rotation && rotation_distance_from_last < interpolation_params.min_rotation_interpolation_distance))
+      {
+        break;
+      }
+
+      time_samples.push_back(t);  // Store the time
+      prev_time = t;
+      prev_frame = current_frame;
+    }
+  }
+
+  if (time_samples.empty() || time_samples.back() < traj_duration)
+  {
+    time_samples.push_back(traj_duration);
+  }
+  // std::chrono::duration<double, std::milli> execution_time = std::chrono::high_resolution_clock::now() - start_time;
+
+  return true;
+}
+
 bool pilz_industrial_motion_planner::generateJointTrajectory(
     const planning_scene::PlanningSceneConstPtr& scene,
     const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory,
     const std::string& group_name, const std::string& link_name,
-    const std::map<std::string, double>& initial_joint_position, double sampling_time,
+    const std::map<std::string, double>& initial_joint_position, const std::vector<double>& time_samples,
     trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code,
     bool check_self_collision)
 {
@@ -229,17 +364,6 @@ bool pilz_industrial_motion_planner::generateJointTrajectory(
   rclcpp::Clock clock;
   rclcpp::Time generation_begin = clock.now();
 
-  // generate the time samples
-  std::vector<double> time_samples;
-  int num_samples = std::floor(trajectory.Duration() / sampling_time);
-  sampling_time = trajectory.Duration() / num_samples;
-  time_samples.reserve(num_samples);
-  for (int i = 0; i < num_samples; ++i)
-  {
-    time_samples.push_back(i * sampling_time);
-  }
-  time_samples.push_back(trajectory.Duration());
-
   // sample the trajectory and solve the inverse kinematics
   Eigen::Isometry3d pose_sample;
   std::map<std::string, double> ik_solution_last, ik_solution, joint_velocity_last;
@@ -249,6 +373,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory(
     joint_velocity_last[item.first] = 0.0;
   }
 
+  double duration_current_sample, duration_last_sample;
   for (std::vector<double>::const_iterator time_iter = time_samples.begin(); time_iter != time_samples.end();
        ++time_iter)
   {
@@ -263,21 +388,22 @@ bool pilz_industrial_motion_planner::generateJointTrajectory(
       return false;
     }
 
-    // check the joint limits
-    double duration_current_sample = sampling_time;
-    // last interval can be shorter than the sampling time
-    if (time_iter == (time_samples.end() - 1) && time_samples.size() > 1)
+    if (time_iter != time_samples.begin())
     {
       duration_current_sample = *time_iter - *(time_iter - 1);
     }
+    if (*time_iter == time_samples[1])
+    {
+      duration_last_sample = duration_current_sample;
+    }
     if (time_samples.size() == 1)
     {
       duration_current_sample = *time_iter;
     }
 
     // skip the first sample with zero time from start for limits checking
     if (time_iter != time_samples.begin() &&
-        !verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, sampling_time,
+        !verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, duration_last_sample,
                                  duration_current_sample, joint_limits))
     {
       RCLCPP_ERROR_STREAM(getLogger(), "Inverse kinematics solution at "
@@ -309,7 +435,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory(
             (ik_solution.at(joint_name) - ik_solution_last.at(joint_name)) / duration_current_sample;
         point.velocities.push_back(joint_velocity);
         point.accelerations.push_back((joint_velocity - joint_velocity_last.at(joint_name)) /
-                                      (duration_current_sample + sampling_time) * 2);
+                                      (duration_current_sample + duration_last_sample) * 2);
         joint_velocity_last[joint_name] = joint_velocity;
       }
       else
@@ -323,6 +449,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory(
     // update joint trajectory
     joint_trajectory.points.push_back(point);
     ik_solution_last = ik_solution;
+    duration_last_sample = duration_current_sample;
   }
 
   error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
@@ -339,7 +466,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory(
     const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits,
     const pilz_industrial_motion_planner::CartesianTrajectory& trajectory, const std::string& group_name,
     const std::string& link_name, const std::map<std::string, double>& initial_joint_position,
-    const std::map<std::string, double>& initial_joint_velocity,
+    const std::map<std::string, double>& initial_joint_velocity, const double& last_sample_duration,
     trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code,
     bool check_self_collision)
 {
@@ -351,7 +478,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory(
 
   std::map<std::string, double> ik_solution_last = initial_joint_position;
   std::map<std::string, double> joint_velocity_last = initial_joint_velocity;
-  double duration_last = 0;
+  double duration_last = last_sample_duration;
   double duration_current = 0;
   joint_trajectory.joint_names.clear();
   for (const auto& joint_position : ik_solution_last)
@@ -376,8 +503,6 @@ 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/src/trajectory_generator_circ.cpp

@@ -214,17 +214,10 @@ void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr&
   KDL::Trajectory_Segment cart_trajectory(cart_path.get(), vel_profile.get(), false);
 
   moveit_msgs::msg::MoveItErrorCodes error_code;
-  // sample the Cartesian trajectory and compute joint trajectory using inverse
-  // kinematics
-  auto cartesian_limits = planner_limits_.getCartesianLimits();
-  auto sampling_time = std::min({ interpolation_params.max_sample_time,
-                                  interpolation_params.max_translation_interpolation_distance /
-                                      (cartesian_limits.max_trans_vel * req.max_velocity_scaling_factor),
-                                  interpolation_params.max_rotation_interpolation_distance /
-                                      (cartesian_limits.max_rot_vel * req.max_velocity_scaling_factor) });
-  RCLCPP_DEBUG(getLogger(), "Sampling time for CIR command: %f", sampling_time);
-  if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name,
-                               plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory,
+  std::vector<double> time_samples;
+  if (!computeTimeSamples(cart_trajectory, interpolation_params, time_samples) ||
+      !generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name,
+                               plan_info.link_name, plan_info.start_joint_position, time_samples, joint_trajectory,
                                error_code))
   {
     throw CircTrajectoryConversionFailure("Failed to generate valid joint trajectory from the Cartesian path",

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp

@@ -161,17 +161,10 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s
   KDL::Trajectory_Segment cart_trajectory(path.get(), vp.get(), false);
 
   moveit_msgs::msg::MoveItErrorCodes error_code;
-  // sample the Cartesian trajectory and compute joint trajectory using inverse
-  // kinematics
-  auto cartesian_limits = planner_limits_.getCartesianLimits();
-  auto sampling_time = std::min({ interpolation_params.max_sample_time,
-                                  interpolation_params.max_translation_interpolation_distance /
-                                      (cartesian_limits.max_trans_vel * req.max_velocity_scaling_factor),
-                                  interpolation_params.max_rotation_interpolation_distance /
-                                      (cartesian_limits.max_rot_vel * req.max_velocity_scaling_factor) });
-  RCLCPP_DEBUG(getLogger(), "Sampling time for LIN command: %f", sampling_time);
-  if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name,
-                               plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory,
+  std::vector<double> time_samples;
+  if (!computeTimeSamples(cart_trajectory, interpolation_params, time_samples) ||
+      !generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name,
+                               plan_info.link_name, plan_info.start_joint_position, time_samples, joint_trajectory,
                                error_code))
   {
     std::ostringstream os;

moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp

@@ -648,7 +648,7 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending)
     trajectory_msgs::msg::JointTrajectory joint_traj;
     const double duration{ lin_traj->getWayPointDurationFromStart(lin_traj->getWayPointCount()) };
     // time from start zero does not work
-    const double time_from_start_offset{ time_scaling_factor * lin_traj->getWayPointDurations().back() };
+    const double time_from_start_offset{ time_scaling_factor * lin_traj->getAverageSegmentDuration() };
 
     // generate modified cartesian trajectory
     for (size_t i = 0; i < lin_traj->getWayPointCount(); ++i)
@@ -692,10 +692,12 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending)
       }
     }
 
+    double duration_last_sample = cart_traj.points[1].time_from_start.seconds();
+
     moveit_msgs::msg::MoveItErrorCodes error_code;
     if (!generateJointTrajectory(planning_scene_, planner_limits_.getJointLimitContainer(), cart_traj, planning_group_,
-                                 target_link_, initial_joint_position, initial_joint_velocity, joint_traj, error_code,
-                                 true))
+                                 target_link_, initial_joint_position, initial_joint_velocity, duration_last_sample,
+                                 joint_traj, error_code, true))
     {
       std::runtime_error("Failed to generate trajectory.");
     }