add scaling when passing in vecotor of joint-limits

moveit · Jan 4, 2023 · b073888 · b073888
1 parent ac1776b
commit b073888
Show file tree

Hide file tree

Showing 5 changed files with 63 additions and 0 deletions.
diff --git a/...t_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h b/...t_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
@@ -56,6 +56,11 @@ class RuckigSmoothing
                              const double max_velocity_scaling_factor = 1.0,
                              const double max_acceleration_scaling_factor = 1.0);
 
+  static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
+                             const std::vector<moveit_msgs::msg::JointLimits>& joint_limits,
+                             const double max_velocity_scaling_factor = 1.0,
+                             const double max_acceleration_scaling_factor = 1.0);
+
 private:
   /**
    * \brief A utility function to check if the group is defined.

diff --git a/...tory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/...tory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h
@@ -206,6 +206,11 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization
                          const double max_velocity_scaling_factor = 1.0,
                          const double max_acceleration_scaling_factor = 1.0) const override;
 
+  bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
+                         const std::vector<moveit_msgs::msg::JointLimits>& joint_limits,
+                         const double max_velocity_scaling_factor = 1.0,
+                         const double max_acceleration_scaling_factor = 1.0) const override;
+
 private:
   bool doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory,
                                           const Eigen::VectorXd& max_velocity,

diff --git a/...t_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h b/...t_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h
@@ -20,5 +20,9 @@ class TimeParameterization
                                  const std::unordered_map<std::string, double>& acceleration_limits,
                                  const double max_velocity_scaling_factor = 1.0,
                                  const double max_acceleration_scaling_factor = 1.0) const = 0;
+  virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
+                                 const std::vector<moveit_msgs::msg::JointLimits>& joint_limits,
+                                 const double max_velocity_scaling_factor = 1.0,
+                                 const double max_acceleration_scaling_factor = 1.0) const = 0;
 };
 }  // namespace trajectory_processing
diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
@@ -213,6 +213,33 @@ RuckigSmoothing::runRuckigInBatches(const size_t num_waypoints, const robot_traj
   return std::make_optional<robot_trajectory::RobotTrajectory>(output_trajectory, true /* deep copy */);
 }
 
+bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
+                                     const std::vector<moveit_msgs::msg::JointLimits>& joint_limits,
+                                     const double max_velocity_scaling_factor,
+                                     const double max_acceleration_scaling_factor)
+{
+  std::unordered_map<std::string, double> velocity_limits;
+  std::unordered_map<std::string, double> acceleration_limits;
+  std::unordered_map<std::string, double> jerk_limits;
+  for (const auto& limit : joint_limits)
+  {
+    if (limit.has_velocity_limits)
+    {
+      velocity_limits[limit.joint_name] = limit.max_velocity;
+    }
+    if (limit.has_acceleration_limits)
+    {
+      acceleration_limits[limit.joint_name] = limit.max_acceleration;
+    }
+    if (limit.has_jerk_limits)
+    {
+      jerk_limits[limit.joint_name] = limit.max_jerk;
+    }
+  }
+  return applySmoothing(trajectory, velocity_limits, acceleration_limits, jerk_limits, max_velocity_scaling_factor,
+                        max_acceleration_scaling_factor);
+}
+
 bool RuckigSmoothing::validateGroup(const robot_trajectory::RobotTrajectory& trajectory)
 {
   moveit::core::JointModelGroup const* const group = trajectory.getGroup();

diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
@@ -967,6 +967,28 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
   return doTimeParameterizationCalculations(trajectory, max_velocity, max_acceleration);
 }
 
+bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
+                                                        const std::vector<moveit_msgs::msg::JointLimits>& joint_limits,
+                                                        const double max_velocity_scaling_factor,
+                                                        const double max_acceleration_scaling_factor) const
+{
+  std::unordered_map<std::string, double> velocity_limits;
+  std::unordered_map<std::string, double> acceleration_limits;
+  for (const auto& limit : joint_limits)
+  {
+    if (limit.has_velocity_limits)
+    {
+      velocity_limits[limit.joint_name] = limit.max_velocity;
+    }
+    if (limit.has_acceleration_limits)
+    {
+      acceleration_limits[limit.joint_name] = limit.max_acceleration;
+    }
+  }
+  return computeTimeStamps(trajectory, velocity_limits, acceleration_limits, max_velocity_scaling_factor,
+                           max_acceleration_scaling_factor);
+}
+
 bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
     robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map<std::string, double>& velocity_limits,
     const std::unordered_map<std::string, double>& acceleration_limits, const double max_velocity_scaling_factor,