PILZ adjust cartesian interpolation

moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt

@@ -63,8 +63,14 @@ generate_parameter_library(
   src/cartesian_limits_parameters.yaml # path to input yaml file
 )
 
+generate_parameter_library(
+  interpolation_parameters # cmake target name for the parameter library
+  src/interpolation_parameters.yaml # path to input yaml file
+)
+
 add_library(planning_context_loader_base SHARED src/planning_context_loader.cpp)
-target_link_libraries(planning_context_loader_base cartesian_limits_parameters)
+target_link_libraries(planning_context_loader_base cartesian_limits_parameters
+                      interpolation_parameters)
 ament_target_dependencies(planning_context_loader_base
                           ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
@@ -148,6 +154,7 @@ pluginlib_export_plugin_description_file(
 install(
   TARGETS pilz_industrial_motion_planner
           cartesian_limits_parameters
+          interpolation_parameters
           joint_limits_common
           planning_context_loader_base
           planning_context_loader_ptp

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.hpp

@@ -46,6 +46,7 @@
 #include <memory>
 
 #include <pilz_industrial_motion_planner/cartesian_limits_parameters.hpp>
+#include <pilz_industrial_motion_planner/interpolation_parameters.hpp>
 
 namespace pilz_industrial_motion_planner
 {
@@ -136,6 +137,9 @@ class CommandPlanner : public planning_interface::PlannerManager
   /// cartesian limit
   std::shared_ptr<cartesian_limits::ParamListener> param_listener_;
   cartesian_limits::Params params_;
+
+  // interpolation params
+  std::shared_ptr<interpolation::ParamListener> interpolation_param_listener_;
 };
 
 MOVEIT_CLASS_FORWARD(CommandPlanner);

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.hpp

@@ -58,11 +58,13 @@ class PlanningContextBase : public planning_interface::PlanningContext
 public:
   PlanningContextBase<GeneratorT>(const std::string& name, const std::string& group,
                                   const moveit::core::RobotModelConstPtr& model,
-                                  const pilz_industrial_motion_planner::LimitsContainer& limits)
+                                  const pilz_industrial_motion_planner::LimitsContainer& limits,
+                                  const std::shared_ptr<interpolation::ParamListener>& interpolation_param_listener)
     : planning_interface::PlanningContext(name, group)
     , terminated_(false)
     , model_(model)
     , limits_(limits)
+    , interpolation_param_listener_(interpolation_param_listener)
     , generator_(model, limits_, group)
   {
   }
@@ -112,6 +114,9 @@ class PlanningContextBase : public planning_interface::PlanningContext
   /// Joint limits to be used during planning
   pilz_industrial_motion_planner::LimitsContainer limits_;
 
+  /// Listener for interpolation parameters
+  std::shared_ptr<interpolation::ParamListener> interpolation_param_listener_;
+
 protected:
   GeneratorT generator_;
 };
@@ -126,7 +131,7 @@ void pilz_industrial_motion_planner::PlanningContextBase<GeneratorT>::solve(plan
     res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
     return;
   }
-  generator_.generate(getPlanningScene(), request_, res);
+  generator_.generate(getPlanningScene(), request_, res, interpolation_param_listener_->get_params());
 }
 
 template <typename GeneratorT>

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.hpp

@@ -58,8 +58,10 @@ class PlanningContextCIRC : public pilz_industrial_motion_planner::PlanningConte
 {
 public:
   PlanningContextCIRC(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model,
-                      const pilz_industrial_motion_planner::LimitsContainer& limits)
-    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorCIRC>(name, group, model, limits)
+                      const pilz_industrial_motion_planner::LimitsContainer& limits,
+                      const std::shared_ptr<interpolation::ParamListener>& interpolation_param_listener)
+    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorCIRC>(name, group, model, limits,
+                                                                                   interpolation_param_listener)
   {
   }
 };

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.hpp

@@ -58,8 +58,10 @@ class PlanningContextLIN : public pilz_industrial_motion_planner::PlanningContex
 {
 public:
   PlanningContextLIN(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model,
-                     const pilz_industrial_motion_planner::LimitsContainer& limits)
-    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorLIN>(name, group, model, limits)
+                     const pilz_industrial_motion_planner::LimitsContainer& limits,
+                     const std::shared_ptr<interpolation::ParamListener>& interpolation_param_listener)
+    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorLIN>(name, group, model, limits,
+                                                                                  interpolation_param_listener)
   {
   }
 };

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.hpp

@@ -42,6 +42,7 @@
 #include <moveit/planning_interface/planning_interface.hpp>
 
 #include <pilz_industrial_motion_planner/limits_container.hpp>
+#include <pilz_industrial_motion_planner/interpolation_parameters.hpp>
 
 namespace pilz_industrial_motion_planner
 {
@@ -75,6 +76,13 @@ class PlanningContextLoader
    */
   virtual bool setLimits(const pilz_industrial_motion_planner::LimitsContainer& limits);
 
+  /**
+   * @brief Set the listener for interpolation parameters
+   * @param param_listener
+   * @return true on success, false otherwise
+   */
+  virtual bool setInterpolationParamListener(const std::shared_ptr<interpolation::ParamListener>& param_listener);
+
   /**
    * @brief Return the planning context
    * @param planning_context
@@ -112,6 +120,9 @@ class PlanningContextLoader
 
   /// The robot model
   moveit::core::RobotModelConstPtr model_;
+
+  /// Listener for interpolation parameters
+  std::shared_ptr<interpolation::ParamListener> interpolation_param_listener_;
 };
 
 typedef std::shared_ptr<PlanningContextLoader> PlanningContextLoaderPtr;
@@ -123,7 +134,7 @@ bool PlanningContextLoader::loadContext(planning_interface::PlanningContextPtr&
 {
   if (limits_set_ && model_set_)
   {
-    planning_context = std::make_shared<T>(name, group, model_, limits_);
+    planning_context = std::make_shared<T>(name, group, model_, limits_, interpolation_param_listener_);
     return true;
   }
   else

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.hpp

@@ -58,8 +58,10 @@ class PlanningContextPTP : public pilz_industrial_motion_planner::PlanningContex
 {
 public:
   PlanningContextPTP(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model,
-                     const pilz_industrial_motion_planner::LimitsContainer& limits)
-    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorPTP>(name, group, model, limits)
+                     const pilz_industrial_motion_planner::LimitsContainer& limits,
+                     const std::shared_ptr<interpolation::ParamListener>& interpolation_param_listener)
+    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorPTP>(name, group, model, limits,
+                                                                                  interpolation_param_listener)
   {
   }
 };

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.hpp

@@ -101,12 +101,10 @@ class TrajectoryBlenderTransitionWindow : public TrajectoryBlender
   /**
    * @brief validate trajectory blend request
    * @param req
-   * @param sampling_time: get the same sampling time of the two input
-   * trajectories
    * @param error_code
    * @return
    */
-  bool validateRequest(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, double& sampling_time,
+  bool validateRequest(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req,
                        moveit_msgs::msg::MoveItErrorCodes& error_code) const;
   /**
    * @brief searchBlendPoint
@@ -164,13 +162,12 @@ class TrajectoryBlenderTransitionWindow : public TrajectoryBlender
    * @param first_interse_index
    * @param second_interse_index
    * @param blend_begin_index
-   * @param sampling_time
    * @param trajectory: the resulting blend trajectory inside the blending
    * sphere
    */
   void blendTrajectoryCartesian(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req,
                                 const std::size_t first_interse_index, const std::size_t second_interse_index,
-                                const std::size_t blend_align_index, double sampling_time,
+                                const std::size_t blend_align_index,
                                 pilz_industrial_motion_planner::CartesianTrajectory& trajectory) const;
 
 private:  // static members

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp

@@ -112,6 +112,20 @@ 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 Interpolates between two poses.
+ *
+ * This function computes an interpolated pose between a start pose and an end pose based on the given interpolation factor.
+ *
+ * @param start_pose The starting pose as an Eigen::Isometry3d.
+ * @param end_pose The ending pose as an Eigen::Isometry3d.
+ * @param interpolation_factor A double value between 0 and 1 that determines the interpolation factor.
+ *                             A value of 0 returns the start_pose, and a value of 1 returns the end_pose.
+ * @param interpolated_pose The resulting interpolated pose as an Eigen::Isometry3d.
+ */
+void interpolate(const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& end_pose,
+                 const double& interpolation_factor, Eigen::Isometry3d& interpolated_pose);
+
 /**
  * @brief Generate joint trajectory from a KDL Cartesian trajectory
  * @param scene: planning scene

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp

@@ -48,6 +48,7 @@
 #include <pilz_industrial_motion_planner/limits_container.hpp>
 #include <pilz_industrial_motion_planner/trajectory_functions.hpp>
 #include <pilz_industrial_motion_planner/trajectory_generation_exceptions.hpp>
+#include <pilz_industrial_motion_planner/interpolation_parameters.hpp>
 
 namespace pilz_industrial_motion_planner
 {
@@ -107,7 +108,8 @@ class TrajectoryGenerator
    * @param sampling_time: sampling time of the generate trajectory
    */
   void generate(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
-                planning_interface::MotionPlanResponse& res, double sampling_time = 0.1);
+                planning_interface::MotionPlanResponse& res,
+                const interpolation::Params& interpolation_params = interpolation::Params());
 
 protected:
   /**
@@ -157,7 +159,8 @@ class TrajectoryGenerator
 
   virtual void plan(const planning_scene::PlanningSceneConstPtr& scene,
                     const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
-                    double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0;
+                    const interpolation::Params& interpolation_params,
+                    trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0;
 
 private:
   /**

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.hpp

@@ -89,7 +89,7 @@ class TrajectoryGeneratorCIRC : public TrajectoryGenerator
                              const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final;
 
   void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
-            const MotionPlanInfo& plan_info, double sampling_time,
+            const MotionPlanInfo& plan_info, const interpolation::Params& interpolation_params,
             trajectory_msgs::msg::JointTrajectory& joint_trajectory) override;
 
   /**

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.hpp

@@ -73,7 +73,7 @@ class TrajectoryGeneratorLIN : public TrajectoryGenerator
                              const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final;
 
   void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
-            const MotionPlanInfo& plan_info, double sampling_time,
+            const MotionPlanInfo& plan_info, const interpolation::Params& interpolation_params,
             trajectory_msgs::msg::JointTrajectory& joint_trajectory) override;
 
   /**

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.hpp

@@ -83,7 +83,7 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator
                double acceleration_scaling_factor, double sampling_time);
 
   void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
-            const MotionPlanInfo& plan_info, double sampling_time,
+            const MotionPlanInfo& plan_info, const interpolation::Params& interpolation_params,
             trajectory_msgs::msg::JointTrajectory& joint_trajectory) override;
 
 private:

moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml

@@ -0,0 +1,41 @@
+interpolation:
+  max_sample_time: {
+    type: double,
+    default_value: 0.1,
+    description: "Max time interval between two consecutive samples",
+    validation: {
+      gt: [ 0.0 ]
+    }
+  }
+  max_translation_interpolation_distance: {
+    type: double,
+    default_value: 10000000.0,
+    description: "Max translation distance between two consecutive samples",
+    validation: {
+      gt: [ 0.0 ]
+    }
+  }
+  max_rotation_interpolation_distance: {
+    type: double,
+    default_value: 10000000.0,
+    description: "Max rotation distance between two consecutive samples",
+    validation: {
+      gt: [ 0.0 ]
+    }
+  }
+  min_rotation_interpolation_distance: {
+    type: double,
+    default_value: 0.000001, # 1e-6
+    description: "Min rotation distance between two consecutive samples",
+    validation: {
+      gt_eq: [ 0.000001 ]
+    }
+  }
+  min_translation_interpolation_distance: {
+    type: double,
+    default_value: 0.000001, # 1e-6
+    description: "Min translation distance between two consecutive samples",
+    validation: {
+      gt_eq: [ 0.000001 ]
+    }
+  }

moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp

@@ -80,6 +80,9 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c
       std::make_shared<cartesian_limits::ParamListener>(node, PARAM_NAMESPACE_LIMITS + ".cartesian_limits");
   params_ = param_listener_->get_params();
 
+  interpolation_param_listener_ =
+      std::make_shared<interpolation::ParamListener>(node, PARAM_NAMESPACE_LIMITS + ".interpolation");
+
   // Load the planning context loader
   planner_context_loader_ = std::make_unique<pluginlib::ClassLoader<PlanningContextLoader>>(
       "pilz_industrial_motion_planner", "pilz_industrial_motion_planner::PlanningContextLoader");
@@ -106,6 +109,7 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c
 
     loader_pointer->setLimits(limits);
     loader_pointer->setModel(model_);
+    loader_pointer->setInterpolationParamListener(interpolation_param_listener_);
 
     registerContextLoader(loader_pointer);
   }

moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp

@@ -57,6 +57,13 @@ bool pilz_industrial_motion_planner::PlanningContextLoader::setLimits(
   return true;
 }
 
+bool pilz_industrial_motion_planner::PlanningContextLoader::setInterpolationParamListener(
+    const std::shared_ptr<interpolation::ParamListener>& param_listener)
+{
+  interpolation_param_listener_ = param_listener;
+  return true;
+}
+
 std::string pilz_industrial_motion_planner::PlanningContextLoader::getAlgorithm() const
 {
   return alg_;

moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp

@@ -65,7 +65,8 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderCIRC::loadContext(
 {
   if (limits_set_ && model_set_)
   {
-    planning_context = std::make_shared<PlanningContextCIRC>(name, group, model_, limits_);
+    planning_context =
+        std::make_shared<PlanningContextCIRC>(name, group, model_, limits_, interpolation_param_listener_);
     return true;
   }
   else

moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp

@@ -62,7 +62,8 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderLIN::loadContext(
 {
   if (limits_set_ && model_set_)
   {
-    planning_context = std::make_shared<PlanningContextLIN>(name, group, model_, limits_);
+    planning_context =
+        std::make_shared<PlanningContextLIN>(name, group, model_, limits_, interpolation_param_listener_);
     return true;
   }
   else

moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp

@@ -61,7 +61,8 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderPTP::loadContext(
 {
   if (limits_set_ && model_set_)
   {
-    planning_context = std::make_shared<PlanningContextPTP>(name, group, model_, limits_);
+    planning_context =
+        std::make_shared<PlanningContextPTP>(name, group, model_, limits_, interpolation_param_listener_);
     return true;
   }
   else