#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::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...

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_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)

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_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)

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_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.

◆ 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_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.

◆ 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_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.

◆ 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_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.

◆ 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]	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.

◆ 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]	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.

◆ validateGroup()

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

staticprivate

A utility function to check if the group is defined.

Parameters

trajectory Trajectory to smooth.

Definition at line 240 of file ruckig_traj_smoothing.cpp.

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

Static Public Member Functions

Static Private Member Functions

Detailed Description

Member Function Documentation

◆ applySmoothing() [1/2]

◆ applySmoothing() [2/2]

◆ checkOvershoot()

◆ extendTrajectoryDuration()

◆ getNextRuckigInput()

◆ getRobotModelBounds()

◆ initializeRuckigState()

◆ runRuckig()

◆ runRuckigInBatches()

◆ validateGroup()