Static Public Member Functions | Static Private Member Functions | List of all members
trajectory_processing::RuckigSmoothing Class Reference

#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 &current_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::RobotTrajectoryrunRuckigInBatches (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...
 

Detailed Description

Definition at line 77 of file ruckig_traj_smoothing.h.

Member Function Documentation

◆ applySmoothing() [1/2]

bool trajectory_processing::RuckigSmoothing::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 
)
static

Apply jerk-limited smoothing to a trajectory.

Parameters
max_velocity_scaling_factorScale all joint velocity limits by this factor. Usually 1.0.
max_acceleration_scaling_factorScale all joint acceleration limits by this factor. Usually 1.0.
mitigate_overshootIf true, overshoot is mitigated by extending trajectory duration.
overshoot_thresholdIf an overshoot is greater than this, duration is extended (radians, for a single joint)
Returns
true if successful.

Definition at line 88 of file ruckig_traj_smoothing.cpp.

◆ applySmoothing() [2/2]

bool trajectory_processing::RuckigSmoothing::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 
)
static

Apply jerk-limited smoothing to a trajectory.

Parameters
velocity_limitsJoint names and velocity limits in rad/s
acceleration_limitsJoint names and acceleration limits in rad/s^2
jerk_limitsJoint names and jerk limits in rad/s^3
mitigate_overshootIf true, overshoot is mitigated by extending trajectory duration.
overshoot_thresholdIf an overshoot is greater than this, duration is extended (radians, for a single joint)
Returns
true if successful.

Definition at line 119 of file ruckig_traj_smoothing.cpp.

◆ checkOvershoot()

bool trajectory_processing::RuckigSmoothing::checkOvershoot ( ruckig::Trajectory< ruckig::DynamicDOFs, ruckig::StandardVector > &  ruckig_trajectory,
const size_t  num_dof,
ruckig::InputParameter< ruckig::DynamicDOFs > &  ruckig_input,
const double  overshoot_threshold 
)
staticprivate

Check if a trajectory out of Ruckig overshoots the target state.

Definition at line 485 of file ruckig_traj_smoothing.cpp.

◆ extendTrajectoryDuration()

void trajectory_processing::RuckigSmoothing::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 
)
staticprivate

Extend the duration of every trajectory segment.

Parameters
[in]duration_extension_factorA number greater than 1. Extend every timestep by this much.
[in]num_waypointsNumber of waypoints in the trajectory.
[in]num_dofDegrees of freedom in the manipulator.
[in]move_group_idxFor accessing the joints of interest out of the full RobotState.
[in]original_trajectoryDurations are extended based on the data in this original trajectory.
[in,out]trajectoryThis trajectory will be returned with modified waypoint durations.

Definition at line 397 of file ruckig_traj_smoothing.cpp.

◆ getNextRuckigInput()

void trajectory_processing::RuckigSmoothing::getNextRuckigInput ( const moveit::core::RobotStateConstPtr &  current_waypoint,
const moveit::core::RobotStateConstPtr &  next_waypoint,
const moveit::core::JointModelGroup joint_group,
ruckig::InputParameter< ruckig::DynamicDOFs > &  ruckig_input 
)
staticprivate

Feed previous output back as input for next iteration. Get next target state from the next waypoint.

Parameters
current_waypointThe nominal current state
next_waypointThe nominal, desired state at the next waypoint
joint_groupThe MoveIt JointModelGroup of interest
[out]ruckig_inputThe Rucking parameters for the next iteration

Definition at line 450 of file ruckig_traj_smoothing.cpp.

◆ getRobotModelBounds()

bool trajectory_processing::RuckigSmoothing::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 
)
staticprivate

A utility function to get bounds from a JointModelGroup and save them for Ruckig.

Parameters
max_velocity_scaling_factorScale all joint velocity limits by this factor. Usually 1.0.
max_acceleration_scaling_factorScale all joint acceleration limits by this factor. Usually 1.0.
groupThe RobotModel and the limits are retrieved from this group.
[out]ruckig_inputThe limits are stored in this ruckig::InputParameter, for use in Ruckig.

Definition at line 251 of file ruckig_traj_smoothing.cpp.

◆ initializeRuckigState()

void trajectory_processing::RuckigSmoothing::initializeRuckigState ( const moveit::core::RobotState first_waypoint,
const moveit::core::JointModelGroup joint_group,
ruckig::InputParameter< ruckig::DynamicDOFs > &  ruckig_input 
)
staticprivate

Initialize Ruckig position/vel/accel. This initializes ruckig_input and ruckig_output to the same values.

Parameters
first_waypointThe Ruckig input/output parameters are initialized to the values at this waypoint
joint_groupThe MoveIt JointModelGroup of interest
[out]ruckig_inputInput parameters to Ruckig. Initialized here.
[out]ruckig_outputOutput from the Ruckig algorithm. Initialized here.

Definition at line 423 of file ruckig_traj_smoothing.cpp.

◆ runRuckig()

bool trajectory_processing::RuckigSmoothing::runRuckig ( robot_trajectory::RobotTrajectory trajectory,
ruckig::InputParameter< ruckig::DynamicDOFs > &  ruckig_input,
const bool  mitigate_overshoot = false,
const double  overshoot_threshold = 0.01 
)
staticprivate

A utility function to instantiate and run Ruckig for a series of waypoints.

Parameters
[in,out]trajectoryTrajectory to smooth.
[in,out]ruckig_inputNecessary input for Ruckig smoothing. Contains kinematic limits (vel, accel, jerk)
mitigate_overshootIf true, overshoot is mitigated by extending trajectory duration.
overshoot_thresholdIf an overshoot is greater than this, duration is extended (radians, for a single joint)

Definition at line 307 of file ruckig_traj_smoothing.cpp.

◆ runRuckigInBatches()

std::optional< robot_trajectory::RobotTrajectory > trajectory_processing::RuckigSmoothing::runRuckigInBatches ( const robot_trajectory::RobotTrajectory trajectory,
ruckig::InputParameter< ruckig::DynamicDOFs > &  ruckig_input,
size_t  batch_size = 100 
)
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.

Parameters
[in,out]trajectoryTrajectory to smooth.
[in,out]ruckig_inputNecessary input for Ruckig smoothing. Contains kinematic limits (vel, accel, jerk)
[in]batch_sizeMinimum number of waypoints to include within a batch

Definition at line 179 of file ruckig_traj_smoothing.cpp.

◆ validateGroup()

bool trajectory_processing::RuckigSmoothing::validateGroup ( const robot_trajectory::RobotTrajectory trajectory)
staticprivate

A utility function to check if the group is defined.

Parameters
trajectoryTrajectory to smooth.

Definition at line 240 of file ruckig_traj_smoothing.cpp.


The documentation for this class was generated from the following files:


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Nov 21 2024 03:23:43