API review

Proposer: Stuart Glaser

Present at review:

• List reviewers

Revision 2

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

[CartesianTrajectoryGoal]
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

[CartesianTrajectoryResult]
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

[CartesianTrajectoryError]
int32 SUCCESSFUL = 0
int32 ROOT_TRANSFORM_FAILED
int32 TOOL_TRANSFORM_FAILED
int32 PATH_TOLERANCE_VIOLATED
int32 INVALID_POSTURE

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

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.

Queuing

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

Stu

• I messed up by putting in two fields for posture.

Agenda:

• Discuss weak timing constraints (stalling the desired) and queueing
• Decide where "posture" should go.

Sachin

• SUCCESS should always be a non-zero value. Default values for fields in messages are zero. I disagree. All unix programs return 0 on success. Also, considering that the name of the field is "error_code", I would expect there to be no error if you don't fill it out.

• The proposed message is very confusing. There is a field for posture but then there is another field for JointTrajectory. There is a field for posture_joint_names? There seems to be a lot of redundancy in this message. Mistake on my part

• I don't understand what "queue" is for.

• I don't like the idea of stalling the desired anymore - I think its better to just return an error and let the user deal with it. I think I agree. I'd like to review the consequences of rigid timing in the meeting just to double-check.

Melonee

• check to see if you have enough DOF between the root of the chain and the tool, if someone chooses the elbow flex link as their tool frame the trajectory can't be followed.

• what is the difference between JointTrajectory posture and the posture in the points []. Which posture is used when? Just a mistake. Only one should be there

• the controller should abort and terminate the trajectory. if someone wants to implement smarter behavior they should implement it on top of this controller
• maybe rename queue to append?

Wim

• Goal

  • Is the 'tool' pose relative to the frame_id specified in the header, or relative to the tip specified in the configuration of the controller? It has to be rigidly attached to the tip, though the final transform will be discovered using tf

  • Why are there two 'posture' fields in the goal message? Mistake

  • How is the 'orientation' error for the path tolerance computed? Angle between desired and actual

  • The 'queue' field is kind of ugly, as it is only used when the stamp in the header is 0. true

  • The behavior to use the previous posture when the array is empty seems wrong (for the float64[] posture, not for the JointTrajectory posture). If the posture is unspecified, the posture should be uncontrolled.

• Result
  • I don't understand ROOT_TRANSFORM_FAILED and TOOL_TRANSFORM_FAILED
  • Why is there no error for GOAL_TOLERANCE_VIOLATED?
  • Is INVALID_POSTURE triggered by:
    • invalid joint names?
    • arrays with incompatible sizes?
    • joint values outside the joint limits?
  • Why return the path tolerance in the result? This seems redundant.

Shaun Edwards

• One additional frame is required or typical in industrial applications. In industrial robotics, the trajectory is defined as the position(orientation) of the tool frame relative to an object frame. Neither the tool frame nor the object frame is required to be fixed. Although in most applications the tool frame does not change. The path tolerances would typically be specified in either the tool or the object frame. I would recommend that tolerances have their own frame, even if in most applications this will be the same as the other frames.

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.

Revision1 Revision1   

[feeling_controller/FeelGoal]:
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

CartesianTrajectory
  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

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

CartesianTrajectory
  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

• cartesian trajectory is generic enough that people may use it for something other than a manipulator arm. Consider moving posture outside that message and up to the top level of the action

• ManipulatorCartesianTrajectory

• It's cleaner to split up the cartesian traj and the redundancy, but it's more difficult to use
• As long as the controller throws an error when it gets a posture that doesn't match the cartesian trajectory, then the user can recover easily.

• PoseStampedWithCovariance sucks. Because they're layered and not combined?

• Draw up the code for filling in the message with them separated. If it's not so bad, then it's ok
• What if the times don't need to match up perfectly.
• It's more true to the joint trajectory message if it's just the trajectory to follow on the side.
• Success should not be 0?
• Feedback should indicate when the trajectory started / will finish
• Requesting interpolation of the controller
• stalling is gone. Amount of time to allow for reaching the goal tolerance (settling_time) will go into the goal message
• queue is gone. Feedback will contain information on timings that can be used
• Use case for tool: controlling the right finger tip (which moves in relation to the r_gripper_tool_frame).
• Separate controllers

• Don't make the tool a PoseStamped. Just a Pose

• It is pretty redundant (could just transform the trajectory yourself)
• Document carefully what the angular error is (axis-angle, magnitude of angle)
• Probably remove the path tolerance from the result.