joint_trajectory_controller Namespace Reference

Classes
class	JointTrajectoryController
struct	SegmentTolerances
	Trajectory segment tolerances. More...
struct	StateTolerances
	Trajectory state tolerances for position, velocity and acceleration variables. More...
class	Trajectory

Typedefs
using	TrajectoryPointIter = std::vector< trajectory_msgs::msg::JointTrajectoryPoint >::iterator
using	TrajectoryPointConstIter = std::vector< trajectory_msgs::msg::JointTrajectoryPoint >::const_iterator

Functions
SegmentTolerances	get_segment_tolerances (rclcpp::Logger &jtc_logger, const Params &params)
	Populate trajectory segment tolerances using data from the ROS node.
double	resolve_tolerance_source (const double default_value, const double goal_value)
SegmentTolerances	get_segment_tolerances (rclcpp::Logger &jtc_logger, const SegmentTolerances &default_tolerances, const control_msgs::action::FollowJointTrajectory::Goal &goal, const std::vector< std::string > &joints)
	Populate trajectory segment tolerances using data from an action goal.
bool	check_state_tolerance_per_joint (const trajectory_msgs::msg::JointTrajectoryPoint &state_error, size_t joint_idx, const StateTolerances &state_tolerance, bool show_errors=false)
template<class T >
std::vector< size_t >	mapping (const T &t1, const T &t2)
void	wraparound_joint (std::vector< double > &current_position, const std::vector< double > next_position, const std::vector< bool > &joints_angle_wraparound)
bool	fill_cubic_spline_velocities (trajectory_msgs::msg::JointTrajectory &traj)
	Fill the knot velocities of a global cubic spline (C2) in place.
tl::expected< void, std::string >	command_interface_type_combinations (rclcpp::Parameter const &parameter)
tl::expected< void, std::string >	state_interface_type_combinations (rclcpp::Parameter const &parameter)

Variables
auto	interface_has_values

Detailed Description

Author

Adolfo Rodriguez Tsouroukdissian

Function Documentation

check_state_tolerance_per_joint()

bool joint_trajectory_controller::check_state_tolerance_per_joint ( const trajectory_msgs::msg::JointTrajectoryPoint &  state_error, size_t  joint_idx, const StateTolerances &  state_tolerance, bool  show_errors = false  )

inline

Parameters

state_error	State error to check.
joint_idx	Joint index for the state error
state_tolerance	State tolerance of joint to check state_error against.
show_errors	If the joint that violate its tolerance should be output to console. NOT REALTIME if true

Returns

True if state_error fulfills state_tolerance.

fill_cubic_spline_velocities()

bool joint_trajectory_controller::fill_cubic_spline_velocities

(

trajectory_msgs::msg::JointTrajectory &

traj

)

Fill the knot velocities of a global cubic spline (C2) in place.

For each joint independently, solves the knot velocities that make the acceleration continuous across every interior knot – an O(n) tridiagonal (Thomas) solve over the waypoint positions and their time_from_start – using rest boundary conditions v[0] = v[n-1] = 0, and writes the result into points[i].velocities. Once velocities are present, the per-segment cubic-Hermite sampling in Trajectory::sample reproduces the global cubic spline, so a positions-only trajectory is upgraded from linear (C0) to C2.

Parameters

[in,out]

traj

Trajectory whose points carry positions; velocities are filled.

Returns

true if velocities were written; false (trajectory left untouched) if it has fewer than two points, inconsistent widths, or non-strictly-increasing timing.

trajectory_msgs::msg::JointTrajectory msg;  // positions-only waypoints, with time_from_start
fill_cubic_spline_velocities(msg);          // msg now has C2 knot velocities
Trajectory traj(time_now, current_point, std::make_shared<...>(msg));
traj.sample(t, interpolation_methods::DEFAULT_INTERPOLATION, out, start, end);

get_segment_tolerances() [1/2]

SegmentTolerances joint_trajectory_controller::get_segment_tolerances	(	rclcpp::Logger &	jtc_logger,
		const Params &	params
	)

Populate trajectory segment tolerances using data from the ROS node.

It is assumed that the following parameter structure is followed on the provided LifecycleNode. 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

jtc_logger	The logger to use for output
params	The ROS Parameters

Returns

Trajectory segment tolerances.

get_segment_tolerances() [2/2]

SegmentTolerances joint_trajectory_controller::get_segment_tolerances	(	rclcpp::Logger &	jtc_logger,
		const SegmentTolerances &	default_tolerances,
		const control_msgs::action::FollowJointTrajectory::Goal &	goal,
		const std::vector< std::string > &	joints
	)

Populate trajectory segment tolerances using data from an action goal.

Parameters

jtc_logger	The logger to use for output
default_tolerances	The default tolerances to use if the action goal does not specify any.
goal	The new action goal
joints	The joints configured by ROS parameters

Returns

Trajectory segment tolerances.

mapping()

template<class T >

std::vector< size_t > joint_trajectory_controller::mapping ( const T &  t1, const T &  t2  )

inline

Returns

The map between t1 indices (implicitly encoded in return vector indices) to t2 indices. If t1 is "{C, B}" and t2 is "{A, B, C, D}", the associated mapping vector is "{2, 1}". return empty vector if t1 is not a subset of t2.

wraparound_joint()

void joint_trajectory_controller::wraparound_joint	(	std::vector< double > &	current_position,
		const std::vector< double >	next_position,
		const std::vector< bool > &	joints_angle_wraparound
	)

Parameters

current_position	The current position given from the controller, which will be adapted.
next_position	Next position from which to compute the wraparound offset, i.e., the first trajectory point
joints_angle_wraparound	Vector of boolean where true value corresponds to a joint that wrap around (ie. is continuous).

Variable Documentation

interface_has_values

auto joint_trajectory_controller::interface_has_values

Initial value:

= [](const auto & joint_interface) -> bool
{
  return std::find_if(
           joint_interface.begin(), joint_interface.end(),
           [](const auto & interface)
           {
             auto interface_op = interface.get().get_optional();
             return !interface_op.has_value() || !std::isfinite(interface_op.value());
           }) == joint_interface.end();
}