Namespaces
namespace	internal
Classes
struct	InitJointTrajectoryOptions
	Options used when initializing a joint trajectory from ROS message data. More...
class	JointTrajectoryController
	Controller for executing joint-space trajectories on a group of joints. More...
class	JointTrajectorySegment
	Class representing a multi-dimensional quintic spline segment with a start and end time. More...
struct	SegmentTolerances
	Trajectory segment tolerances. More...
struct	StateTolerances
	Trajectory state tolerances for position, velocity and acceleration variables. More...
Functions
template<class State >
bool	checkStateTolerance (const State &state_error, const std::vector< StateTolerances< typename State::Scalar > > &state_tolerance, bool show_errors=false)
std::vector < trajectory_msgs::JointTrajectoryPoint > ::const_iterator	findPoint (const trajectory_msgs::JointTrajectory &msg, const ros::Time &time)
	Find an iterator to the trajectory point with the greatest start time < `time`.
template<class Scalar >
SegmentTolerances< Scalar >	getSegmentTolerances (const ros::NodeHandle &nh, const std::vector< std::string > &joint_names)
	Populate trajectory segment tolerances from data in the ROS parameter server.
template<class Trajectory >
Trajectory	initJointTrajectory (const trajectory_msgs::JointTrajectory &msg, const ros::Time &time, const InitJointTrajectoryOptions< Trajectory > &options=InitJointTrajectoryOptions< Trajectory >())
	Initialize a joint trajectory from ROS message data.
bool	isTimeStrictlyIncreasing (const trajectory_msgs::JointTrajectory &msg)
bool	isValid (const trajectory_msgs::JointTrajectoryPoint &point, const unsigned int joint_dim)
bool	isValid (const trajectory_msgs::JointTrajectory &msg)
template<class Scalar >
void	updateSegmentTolerances (const control_msgs::FollowJointTrajectoryGoal &goal, const std::vector< std::string > &joint_names, SegmentTolerances< Scalar > &tols)
	Update data in `tols` from data in `goal`.
template<class Scalar >
void	updateStateTolerances (const control_msgs::JointTolerance &tol_msg, StateTolerances< Scalar > &tols)
	Update data in `tols` from data in `msg_tol`.
template<class Scalar >
std::vector< Scalar >	wraparoundOffset (const std::vector< Scalar > &prev_position, const std::vector< Scalar > &next_position, const std::vector< bool > &angle_wraparound)

Detailed Description

Author:: Adolfo Rodriguez Tsouroukdissian; Adolfo Rodriguez Tsouroukdissian, Stuart Glaser

Function Documentation

template<class State >

bool joint_trajectory_controller::checkStateTolerance	(	const State &	state_error,
		const std::vector< StateTolerances< typename State::Scalar > > &	state_tolerance,
		bool	show_errors = `false`
	)		`[inline]`

Parameters:

state_error	State error to check.
state_tolerance	State tolerances to check `state_error` against.
show_errors	If the joints that violate their tolerances should be output to console. NOT REALTIME if true

Returns:: True if state_error fulfills state_tolerance.

Definition at line 98 of file tolerances.h.

std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator joint_trajectory_controller::findPoint	(	const trajectory_msgs::JointTrajectory &	msg,
		const ros::Time &	time
	)		`[inline]`

Find an iterator to the trajectory point with the greatest start time < time.

Parameters:

msg	Trajectory message.
time	Time to search for in the range.

Returns:: Iterator to the trajectory point with the greatest start time < time. If all points occur at a time greater than time, then the points sequence end is returned.

Precondition:: The points in msg have monotonically increasing times.

Note:: On average, this method has logarithmic time complexity when used on containers with random-access iterators. On non-random-access iterators, iterator advances incur an additional linear time cost.

Definition at line 151 of file joint_trajectory_msg_utils.h.

template<class Scalar >

SegmentTolerances<Scalar> joint_trajectory_controller::getSegmentTolerances	(	const ros::NodeHandle &	nh,
		const std::vector< std::string > &	joint_names
	)

Populate trajectory segment tolerances from data in the ROS parameter server.

It is assumed that the following parameter structure is followed on the provided NodeHandle. Unspecified parameters will take the defaults shown in the comments:

 constraints:
  goal_time: 1.0                   # Defaults to zero
  stopped_velocity_tolerance: 0.02 # Defaults to 0.01
  foo_joint:
    trajectory: 0.05               # Defaults to zero (ie. the tolerance is not enforced)
    goal:       0.03               # Defaults to zero (ie. the tolerance is not enforced)
  bar_joint:
    goal: 0.01

Parameters:

nh	NodeHandle where the tolerances are specified.
joint_names	Names of joints to look for in the parameter server for a tolerance specification.

Returns:: Trajectory segment tolerances.

Definition at line 230 of file tolerances.h.

template<class Trajectory >

Trajectory joint_trajectory_controller::initJointTrajectory	(	const trajectory_msgs::JointTrajectory &	msg,
		const ros::Time &	time,
		const InitJointTrajectoryOptions< Trajectory > &	options = `InitJointTrajectoryOptions<Trajectory>()`
	)

Initialize a joint trajectory from ROS message data.

Parameters:

msg	Trajectory message.
time	Time from which data is to be extracted. All trajectory points in `msg` occurring after `time` will be extracted; or put otherwise, all points occurring at a time less or equal than `time` will be discarded. Set this value to zero to process all points in `msg`.
options	Options that change how the trajectory gets initialized.

The options parameter is optional. The meaning of its different members follows:

current_trajectory Currently executed trajectory. Use this parameter if you want to update an existing trajectory with the data in msg; that is, keep the useful parts of current_trajectory and msg. If specified, the output trajectory will not only contain data in msg occurring after time, but will also contain data from current_trajectory between time and the start time of msg (which might not be equal to time).

joint_names Joints expected to be found in msg. If specified, this function will return an empty trajectory when msg contains joints that differ in number or names to joint_names. If msg contains the same joints as joint_names, but in a different order, the resulting trajectory will be ordered according to joint_names (and not msg). If unspecified (empty), no checks will be performed against expected joints, and the resulting trajectory will preserve the joint ordering of msg.

angle_wraparound Vector of booleans where true values correspond to joints that wrap around (ie. are continuous). If specified, combining current_trajectory with msg will not result in joints performing multiple turns at the transition. This parameter requires current_trajectory to also be specified, otherwise it is ignored.

rt_goal_handle Action goal handle associated to the new trajectory. If specified, newly added segments will have a pointer to it, and to the trajectory tolerances it contains (if any).

default_tolerances Default trajectory tolerances. This option is only used when rt_goal_handle is also specified. It contains the default tolernaces to check when executing an action goal. If the action goal specifies tolerances (totally or partially), these values will take precedence over the defaults.

other_time_base When initializing a new trajectory, it might be the case that we desire the result expressed in a different time base than that contained in msg. If specified, the value of this variable should be the equivalent of the time parameter, but expressed in the desired time base. If the current_trajectory option is also specified, it must be expressed in other_time_base. The typical usecase for this variable is when the current_trajectory option is specified, and contains data in a different time base (eg. monotonically increasing) than msg (eg. system-clock synchronized).

Returns:: Trajectory container.

Template Parameters:

Trajectory

Trajectory type. Should be a sequence container sorted by segment start time. Additionally, the contained segment type must implement a constructor with the following signature:

 Segment(const ros::Time&                             traj_start_time,
         const trajectory_msgs::JointTrajectoryPoint& start_point,
         const trajectory_msgs::JointTrajectoryPoint& end_point,
         const std::vector<unsigned int>&             permutation,
         const std::vector<Scalar>&                   position_offset)

The following function must also be defined to properly handle continuous joints:

 std::vector<Scalar> wraparoundOffset(const typename Segment::State&  prev_state,
                                      const typename Segment::State&  next_state,
                                      const std::vector<bool>&        angle_wraparound)

Note:: This function does not throw any exceptions by itself, but the segment constructor might. In such a case, this method should be wrapped inside a try block.

Definition at line 182 of file init_joint_trajectory.h.

bool joint_trajectory_controller::isTimeStrictlyIncreasing ( const trajectory_msgs::JointTrajectory & msg ) [inline]

Parameters:

msg	Trajectory message.

Returns:: True if each trajectory waypoint is reached at a later time than its predecessor.

Definition at line 119 of file joint_trajectory_msg_utils.h.

bool joint_trajectory_controller::isValid	(	const trajectory_msgs::JointTrajectoryPoint &	point,
		const unsigned int	joint_dim
	)		`[inline]`

Parameters:

point	Trajectory point message.
joint_dim	Expected dimension of the position, velocity and acceleration fields.

Returns:: True if sizes of the position, velocity and acceleration fields are consistent. An empty field means that it's unspecified, so in this particular case its dimension must not coincide with joint_dim.

Definition at line 83 of file joint_trajectory_msg_utils.h.

bool joint_trajectory_controller::isValid ( const trajectory_msgs::JointTrajectory & msg ) [inline]

Parameters:

msg	Trajectory message.

Returns:: True if sizes of input message are consistent (joint names, position, velocity, acceleration).

Definition at line 95 of file joint_trajectory_msg_utils.h.

template<class Scalar >

void joint_trajectory_controller::updateSegmentTolerances	(	const control_msgs::FollowJointTrajectoryGoal &	goal,
		const std::vector< std::string > &	joint_names,
		SegmentTolerances< Scalar > &	tols
	)

Update data in tols from data in goal.

Parameters:

[in]	goal	Action goal data containing tolerance values `tols` will be updated with.
[in]	joint_names	Names of joints in `tols`, with the same ordering.
[out]	tols	Tolerances values to update.

Definition at line 171 of file tolerances.h.

template<class Scalar >

void joint_trajectory_controller::updateStateTolerances	(	const control_msgs::JointTolerance &	tol_msg,
		StateTolerances< Scalar > &	tols
	)

Update data in tols from data in msg_tol.

If a value in tol_msg is positive, the corresponding values in tols will be overritten.
If a value in tol_msg is negative, the corresponding values in tols will be reset.
If a value in tol_msg is zero, the corresponding values in tols is unchanged.

Parameters:

[in]	tol_msg	Message containing tolerance values `tols` will be updated with.
[out]	tols	Tolerances values to update.

Definition at line 151 of file tolerances.h.

template<class Scalar >

std::vector<Scalar> joint_trajectory_controller::wraparoundOffset	(	const std::vector< Scalar > &	prev_position,
		const std::vector< Scalar > &	next_position,
		const std::vector< bool > &	angle_wraparound
	)

Parameters:

prev_position	Previous position from which to compute the wraparound offset.
next_position	Next position from which to compute the wraparound offset.
angle_wraparound	Vector of booleans where true values correspond to joints that wrap around (ie. are continuous). Offsets will be computed only for these joints, otherwise they are set to zero.

Returns:: Wraparound offsets that should be applied to next_position such that no multi-turns are performed when transitioning from prev_position.

Template Parameters:

Scalar Scalar type.

Definition at line 250 of file joint_trajectory_segment.h.

Namespaces

Classes

Functions

Detailed Description

Function Documentation