diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h index a59be0a2b7..e0bb63bbb3 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h +++ b/moveit_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& 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/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index 57549b3673..705e04b5cf 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_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& 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/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h index e35b8f30c0..9bc6dbbb2b 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h @@ -20,5 +20,9 @@ class TimeParameterization const std::unordered_map& 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& 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 index 40d10c5db0..82d7a70641 100644 --- 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(output_trajectory, true /* deep copy */); } +bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory, + const std::vector& joint_limits, + const double max_velocity_scaling_factor, + const double max_acceleration_scaling_factor) +{ + std::unordered_map velocity_limits; + std::unordered_map acceleration_limits; + std::unordered_map 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 index d86fcba198..fff78d05fb 100644 --- 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& joint_limits, + const double max_velocity_scaling_factor, + const double max_acceleration_scaling_factor) const +{ + std::unordered_map velocity_limits; + std::unordered_map 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& velocity_limits, const std::unordered_map& acceleration_limits, const double max_velocity_scaling_factor,