#include <ruckig_traj_smoothing.h>
Static Public Member Functions | |
static bool | applySmoothing (robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0, const bool mitigate_overshoot=false, const double overshoot_threshold=0.01) |
Apply jerk-limited smoothing to a trajectory. More... | |
static bool | applySmoothing (robot_trajectory::RobotTrajectory &trajectory, const std::unordered_map< std::string, double > &velocity_limits, const std::unordered_map< std::string, double > &acceleration_limits, const std::unordered_map< std::string, double > &jerk_limits, const bool mitigate_overshoot=false, const double overshoot_threshold=0.01) |
Apply jerk-limited smoothing to a trajectory. More... | |
Static Private Member Functions | |
static bool | checkOvershoot (ruckig::Trajectory< ruckig::DynamicDOFs, ruckig::StandardVector > &ruckig_trajectory, const size_t num_dof, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input, const double overshoot_threshold) |
Check if a trajectory out of Ruckig overshoots the target state. More... | |
static void | extendTrajectoryDuration (const double duration_extension_factor, size_t num_waypoints, const size_t num_dof, const std::vector< int > &move_group_idx, const robot_trajectory::RobotTrajectory &original_trajectory, robot_trajectory::RobotTrajectory &trajectory) |
Extend the duration of every trajectory segment. More... | |
static void | getNextRuckigInput (const moveit::core::RobotStateConstPtr ¤t_waypoint, const moveit::core::RobotStateConstPtr &next_waypoint, const moveit::core::JointModelGroup *joint_group, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input) |
Feed previous output back as input for next iteration. Get next target state from the next waypoint. More... | |
static bool | getRobotModelBounds (const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor, moveit::core::JointModelGroup const *const group, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input) |
A utility function to get bounds from a JointModelGroup and save them for Ruckig. More... | |
static void | initializeRuckigState (const moveit::core::RobotState &first_waypoint, const moveit::core::JointModelGroup *joint_group, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input) |
Initialize Ruckig position/vel/accel. This initializes ruckig_input and ruckig_output to the same values. More... | |
static bool | runRuckig (robot_trajectory::RobotTrajectory &trajectory, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input, const bool mitigate_overshoot=false, const double overshoot_threshold=0.01) |
A utility function to instantiate and run Ruckig for a series of waypoints. More... | |
static std::optional< robot_trajectory::RobotTrajectory > | runRuckigInBatches (const robot_trajectory::RobotTrajectory &trajectory, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input, size_t batch_size=100) |
Break the trajectory parameter into batches of reasonable size (~100), run Ruckig on each of them, then re-combine into a single trajectory again. runRuckig() stretches all input waypoints in time until all kinematic limits are obeyed. This works but it can slow the trajectory more than necessary. It's better to feed in just a few waypoints at once, so that only the waypoints needing it get stretched. Here, break the trajectory waypoints into batches so the output is closer to time-optimal. There is a trade-off between time-optimality of the output trajectory and runtime of the smoothing algorithm. More... | |
static bool | validateGroup (const robot_trajectory::RobotTrajectory &trajectory) |
A utility function to check if the group is defined. More... | |
Definition at line 77 of file ruckig_traj_smoothing.h.
|
static |
Apply jerk-limited smoothing to a trajectory.
max_velocity_scaling_factor | Scale all joint velocity limits by this factor. Usually 1.0. |
max_acceleration_scaling_factor | Scale all joint acceleration limits by this factor. Usually 1.0. |
mitigate_overshoot | If true, overshoot is mitigated by extending trajectory duration. |
overshoot_threshold | If an overshoot is greater than this, duration is extended (radians, for a single joint) |
Definition at line 88 of file ruckig_traj_smoothing.cpp.
|
static |
Apply jerk-limited smoothing to a trajectory.
velocity_limits | Joint names and velocity limits in rad/s |
acceleration_limits | Joint names and acceleration limits in rad/s^2 |
jerk_limits | Joint names and jerk limits in rad/s^3 |
mitigate_overshoot | If true, overshoot is mitigated by extending trajectory duration. |
overshoot_threshold | If an overshoot is greater than this, duration is extended (radians, for a single joint) |
Definition at line 119 of file ruckig_traj_smoothing.cpp.
|
staticprivate |
Check if a trajectory out of Ruckig overshoots the target state.
Definition at line 485 of file ruckig_traj_smoothing.cpp.
|
staticprivate |
Extend the duration of every trajectory segment.
[in] | duration_extension_factor | A number greater than 1. Extend every timestep by this much. |
[in] | num_waypoints | Number of waypoints in the trajectory. |
[in] | num_dof | Degrees of freedom in the manipulator. |
[in] | move_group_idx | For accessing the joints of interest out of the full RobotState. |
[in] | original_trajectory | Durations are extended based on the data in this original trajectory. |
[in,out] | trajectory | This trajectory will be returned with modified waypoint durations. |
Definition at line 397 of file ruckig_traj_smoothing.cpp.
|
staticprivate |
Feed previous output back as input for next iteration. Get next target state from the next waypoint.
current_waypoint | The nominal current state | |
next_waypoint | The nominal, desired state at the next waypoint | |
joint_group | The MoveIt JointModelGroup of interest | |
[out] | ruckig_input | The Rucking parameters for the next iteration |
Definition at line 450 of file ruckig_traj_smoothing.cpp.
|
staticprivate |
A utility function to get bounds from a JointModelGroup and save them for Ruckig.
max_velocity_scaling_factor | Scale all joint velocity limits by this factor. Usually 1.0. | |
max_acceleration_scaling_factor | Scale all joint acceleration limits by this factor. Usually 1.0. | |
group | The RobotModel and the limits are retrieved from this group. | |
[out] | ruckig_input | The limits are stored in this ruckig::InputParameter, for use in Ruckig. |
Definition at line 251 of file ruckig_traj_smoothing.cpp.
|
staticprivate |
Initialize Ruckig position/vel/accel. This initializes ruckig_input and ruckig_output to the same values.
first_waypoint | The Ruckig input/output parameters are initialized to the values at this waypoint | |
joint_group | The MoveIt JointModelGroup of interest | |
[out] | ruckig_input | Input parameters to Ruckig. Initialized here. |
[out] | ruckig_output | Output from the Ruckig algorithm. Initialized here. |
Definition at line 423 of file ruckig_traj_smoothing.cpp.
|
staticprivate |
A utility function to instantiate and run Ruckig for a series of waypoints.
[in,out] | trajectory | Trajectory to smooth. |
[in,out] | ruckig_input | Necessary input for Ruckig smoothing. Contains kinematic limits (vel, accel, jerk) |
mitigate_overshoot | If true, overshoot is mitigated by extending trajectory duration. | |
overshoot_threshold | If an overshoot is greater than this, duration is extended (radians, for a single joint) |
Definition at line 307 of file ruckig_traj_smoothing.cpp.
|
staticprivate |
Break the trajectory
parameter into batches of reasonable size (~100), run Ruckig on each of them, then re-combine into a single trajectory again. runRuckig() stretches all input waypoints in time until all kinematic limits are obeyed. This works but it can slow the trajectory more than necessary. It's better to feed in just a few waypoints at once, so that only the waypoints needing it get stretched. Here, break the trajectory waypoints into batches so the output is closer to time-optimal. There is a trade-off between time-optimality of the output trajectory and runtime of the smoothing algorithm.
[in,out] | trajectory | Trajectory to smooth. |
[in,out] | ruckig_input | Necessary input for Ruckig smoothing. Contains kinematic limits (vel, accel, jerk) |
[in] | batch_size | Minimum number of waypoints to include within a batch |
Definition at line 179 of file ruckig_traj_smoothing.cpp.
|
staticprivate |
A utility function to check if the group is defined.
trajectory | Trajectory to smooth. |
Definition at line 240 of file ruckig_traj_smoothing.cpp.