[Documentation] [TitleIndex] [WordIndex

API review

Proposer: Stuart Glaser

I've updated this proposal as suggestions have come in. The most recent revision is at the top. I've left older revisions here for reference

Present at review:

Revision 2

This is a proposal for the API for a cartesian trajectory controller.

CartesianTrajectory trajectory
  Header header  # A stamp of 0 means "execute now"
  PoseStamped tool  # The frame which is being controlled
  string[] posture_joint_names
  CartesianTrajectoryPoint[] points
    duration time_from_start
    Pose pose
    Twist twist
    float64[] posture
  JointTrajectory posture  # For determining the redundancy
CartesianTolerance path_tolerance  # Tolerance for aborting the path
  float64 position
  float64 orientation  # Permitted angular error
  float64 velocity
  float64 angular_velocity
CartesianTolerance goal_tolerance  # Tolerance for when reaching the goal is considered successful
bool queue

int32 error_code  # 0 if successful
CartesianTrajectoryPoint cartesian_state
JointTrajectoryPoint joint_state
Twist pose_error
Twist twist_error
CartesianTolerance path_tolerance  # Current tolerance used for the path

int32 SUCCESSFUL = 0

Control (Tool) Frame

The "tool" field describes the control frame for this trajectory. The poses and twists of the trajectory will be applied in this frame, and the tolerances will be measured in this frame. The tool frame should be rigidly attached to the "tip" frame given in the controller configuration; the transform between the two will only be computed once.

Redundancy Resolution

Each cartesian trajectory point contains a posture, which is an array of joint positions for the joints listed in posture_joint_names. The controller attempts to track the posture in the nullspace of the cartesian movement. The posture value for each point is either the given value, or the previous posture value if the array is empty. The posture is linearly interpolated between trajectory points. If the posture array is empty in every point, then the posture is uncontrolled.


Tolerances are specified for the entire trajectory (path_tolerance) and for the success conditions (goal_tolerance). In both, a tolerance of 0 is interpreted as "unspecified", and a default tolerance (such as a parameter to the controller) is used. A tolerace of -1 means "no tolerance" and the corresponding field is ignored when tolerances are checked.

There are two possible ways to handle the path tolerance:

  1. Abort if the path tolerance is violated
  2. Stall the desired and allow the controller to catch up if the path tolerance is violated.

Option 1 is the most straightforward to implement, but more difficult to use. I'm pretty sure I can implement option 2 by stalling the time used for computing the desired point. I'm considering making this choice a parameter of the controller so the user can choose either behavior.


If "queue" is true and the start time is zero, then the trajectory is added to the end of the current list of trajectories.

Question / concerns / comments






Shaun Edwards

Adolfo Rodriguez Tsouroukdissian

-1 for encoding the posture task as part of the message. It's an implementation detail of a very specific IK setup: a numeric IK solver with two prioritized tasks. Setups in which the posture task does not make sense can leave the field empty (I could live with that), but setups where redundancy resolution is more complex (think a whole-body controller with many priority levels) will also ignore this field as it's not expressive enough.

Konrad Banachowicz

I think that posture is still useful for whole-body control. It would be great if we could specify impedance behaviour along with each trajectory point.


Revision 1


This is a proposal for the API for a cartesian trajectory controller.

The closest controller to a full cartesian trajectory controller exists in the feeling_controller package. Its goal message looks like this:

feeling_controller/FeelTrajectory trajectory
  Header header
    uint32 seq
    time stamp
    string frame_id
  feeling_controller/FeelTrajectoryPoint[] points
    duration time_from_start
    geometry_msgs/Pose pose
    geometry_msgs/Twist twist

This gives a series of desired poses and twists and the time at which to achieve them. Some weaknesses are:

  1. The user must specify a twist for every point.
  2. The timing is rigid. The desired setpoint could easily outpace the actual pose.
  3. It has no information about tolerances while following.
  4. It contains no information about the redundancy.

Rigid timing

The current joint trajectory controller sticks rigidly to the given timings. It may be better to have a trajectory controller interpret time more loosely.

The controller could have a pose_windup, which constrains how far the desired may drift from the actual. When the pose exceeds pose_windup, time is stopped so the desired doesn't continue moving away.

Tolerance specification

There are three possibilities for specifying trajectory tolerances:

  • At the controller level
  • At the trajectory level
  • At the trajectory point level

Here's a possible command containing tolerances at the trajectory and trajectory point levels.

  Header header
  CartesianTrajectoryPoint[] points
    duration time_from_start
    geometry_msgs/Pose pose
    geometry_msgs/Twist twist
    bool has_tolerance
    CartesianTolerance tolerance  # Tolerance for this point only
  bool has_default_tolerance
  CartesianTolerance default_tolerance  # Default tolerance if no tolerance is given for a point
  bool has_goal_tolerance
  CartesianTolerance goal_tolerance  # Tolerance for when reaching the goal is considered successful

Where CartesianTolerance looks like this:

  Twist pose_tolerance  # Bounds on the pose error
  Twist twist_tolerance  # Bounds on the twist error

Each tolerance has a has_*_tolerance field to indicate if the tolerance is to be used.

Redundancy resolution

For starters, there are a multitude of ways of doing redundancy resolution. I think we should use a desired posutre. There are still several ways of specifying the desired posture.

Parameterized for the duration of the controller

Just put the desired posture on the parameter server. The posture will be constant for the duration of the controller.

As a topic/service into the controller

The controller itself can listen for the desired posture. The posture is changeable, but it's difficult to synchronize the posture changes with a running trajectory.

As part of the trajectory

The cartesian trajectory message could contain the desired posture as part of the trajectory point. This would allow the most fine-grained control over the posture, but it does make the trajectory message a bit more complex:

  Header header
  CartesianTrajectoryPoint[] points
    duration time_from_start
    geometry_msgs/Pose pose
    geometry_msgs/Twist twist
    # Possibly some tolerance stuff
    Posture posture
      string[] joint_names
      float64[] positions

Question / concerns / comments

Comments for Revision 1 are closed. Please comment on the #latest_revision


  • For timing

    • have the controller interpret time the same way TF does - ros::Time(0.0) in the header implies execute this "now". ok

  • message
    • Have a look at the RobotTrajectory message in motion_planning_msgs. It allows you to specify a trajectory for multiple rigid bodies. It has no twists right now but that could be a part of the eventual API.

      • RobotTrajectory does look good for multi-body trajectories. I'd like to make the first version of this controller only support single-body trajectories

    • If this trajectory is for a single rigid body - specify it as a vector of poses and twists - this solves the no twist specified problem
    • Have a frame id/link name for the frame you are operating on (similar to joint_names). Specify the tool frame to control to the trajectory poses? I like the idea.

  • Tolerances - have a goal tolerance and a path tolerance (no point tolerances). The goal tolerance region would have to be satisfied before goal convergence is declared. Path tolerance would be how much the actual trajectory can deviate from the desired trajectory at any given point of time. Two choices:
    • tolerance must always be specified, throw an error if its too small
    • tolerance is optional, use default tolerance that was specified in a launch file, add flag for when tolerance from message will be filled out, ignore tolerance in message if flag is not filled out. I like this option. I think it should also be possible to specify 'ignore tolerances' by giving negative numbers.

  • If you do abort the trajectory - abort gracefully by stopping nicely- the current joint trajectory controller aborts with a shudder. Ok. How about specifying a stopping_acceleration parameter, and generating a stopping trajectory on preemption/abortion?

  • Redundancy resolution
    • A joint trajectory
      • if it contains a single point, this is the constant posture that you should use for redundancy resolution. ok

      • if it contains as many points as the cartesian trajectory - move the desired posture along this joint trajectory like Peter was doing. ok

      • if it contains any other number of points - throw an error
  • Behavior
    • always, always, always return an error code when things go wrong. Sounds good

    • return message must also contain goal error/path errors that caused breakdown so people can check them and adjust their tolerances if needed. ok

    • have options using rosparam configuration flags for what to do if path errors exceed path tolerances.
      • stop and throw an error
      • wait until actual catches up to desired
    • Make sure gains can be changed on the fly
    • It's useful to be able to change the desired frame on the fly. You mean changing the tool frame? Like you proposed earlier?

2024-06-22 13:15