API review

Proposer: Sachin Chitta/Ioan Sucan

Present at review:

• Gunter
• Gil
• Ioan
• Matei

Proposed API

Base level messages

JointState.msg - contains joint values, can contain multiple values to be able to represent multi-dof joints

float64[] values

JointLimit.msg - contains the joint limits for a joint

bool has_position_limits
bool angle_wraparound
float64 min_position
float64 max_position

bool has_velocity_limits
float64 max_velocity
# min_velocity is assumed to be -max_velocity

bool has_acceleration_limits
float64 max_acceleration
# min_acceleration is assumed to be -max_acceleration

Joint.msg - a full description of a joint including joint limits

Header header
string joint_name
uint8 joint_type

JointLimit limits

uint8 UNKNOWN=0    # unknown
uint8 REVOLUTE=1   # revolute joint with joint limits
uint8 CONTINUOUS=2 # revolute joint with continuous rotation capability
uint8 PRISMATIC=3  # prismatic joint
uint8 FLOATING=4   # free floating joint
uint8 PLANAR=5     # planar joint (x,y,yaw)
uint8 FIXED=5      # fixed joint - representing a rigid connection between links

JointState state

Review

• Take out JointState

• Waypoints will be shared between controllers and planners

• Take out model_id and start from the path message - path message will then be shared (instead of WaypointTraj) with the controllers.

• No longer need Joint.msg and JointLimits.msg here

Constraints

JointConstraint.msg - joint constraints

Header header

# Constrain the position of a joint to be within a certain bound
string joint_name

# the bound to be achieved is [value - tolerance_below, value + tolerance_above]
float64[] values
float64[] tolerance_above
float64[] tolerance_below

PoseConstraint - a pose constraint

# This message contains the definition of a motion planning constraint.
# Since there are multiple types of constraints, the 'type' member is used
# to identify the different constraints


# Constants that represent possible values for type. Each degree of freedom
# can be constrained individually

int32 POSITION_X=1   #  only x of position is considered
int32 POSITION_Y=2   #  only y of position is considered
int32 POSITION_Z=4   #  only z of position is considered

int32 ORIENTATION_R=8     # only roll of orientation is considered
int32 ORIENTATION_P=16    # only pitch of orientation is considered
int32 ORIENTATION_Y=32    # only yaw of orientation is considered

int32 type

# The robot link this constraint refers to
string link_name

# The desired pose of the robot link
geometry_msgs/PoseStamped pose

# The acceptable tolerance
geometry_msgs/Point position_tolerance_above
geometry_msgs/Point position_tolerance_below

# The acceptable tolerance (roll pitch yaw)
geometry_msgs/Point orientation_tolerance_above
geometry_msgs/Point orientation_tolerance_below

# The planner may internally compute a distance from current state to goal pose.
# This is done with a weighted sum such as:
# Distance_to_goal = position_distance + orientation_importance * orientation_distance;
# orientation_importance can be used to tell the planner which component is more important
# (this only makes a difference when approximate solutions are found)
# If you do not care about this value, simply set it to 1.0
# Planners should use square of 2-norm for position distance and shortest angular distance
# for orientations (roll, pitch yaw)
float64 orientation_importance

Constraint.msg - A full definition of constraints for all the joints

# This message contains a list of motion planning constraints.
JointConstraint[] joint_constraints
PoseConstraint[] pose_constraints

Specifying plans/paths

Waypoint.msg defines a single waypoint that represents the positions of all the joints at a particular time

JointState[] joints
float64 time

Path.msg defines a path - essentially a list of waypoints - that would be returned by the planners

# The definition of a kinematic path.
Header header

# The model for which this plan was computed
string model_id

# The joint names, in the same order as the values in the state
string[] joint_names

# A waypoint representing the start state of the robot at the start of the path
Waypoint start #TODO - should this be Joint[] start???

# A list of waypoints. Each waypoint consists of a set of joint states
# of dimension the number of joints that we are planning for
Waypoint[] points

Environment specification

AcceptableContact.msg - This message defines a region of space where contacts with certain links are allowed, up to a certain depth

# The shape of the region in the environment where contacts are allowed
mapping_msgs/Object region

# The pose of the space defining the region where contacts are allowed
geometry_msgs/PoseStamped pose

# The set of links that are allowed to touch obstacles
string[] link_names

# The maximum penetration depth
float64 penetration_depth

Environment.msg sets up the workspace that the planner will use for planning. Currently, the workspace defines a box within which all planned paths must stay - for 2D it would define a rectangular region on the ground within which all paths are constrained to stay.

# This message contains a set of parameters useful in
# setting up the workspace for planning

# The model (defined in pr2_defs/planning/planning.yaml)
# for which the motion planner should plan for
string model_id

# A string that identifies which distance metric the planner should use
string distance_metric

# Lower coordinate for a box defining the volume in the workspace the
# motion planner is active in.  If planning in 2D, only first 2 values
# (x, y) of the points are used.
geometry_primitives/Box   workspace_region
geometry_msgs/PoseStamped workspace_region_pose

# A list of allowed contacts
AcceptableContact[] contacts

Planning services

GetMotionPlan.srv

# This service contains the definition for a request to the motion
# planner and the output it provides

# Parameters for the workspace that the planner should work inside
motion_planning_msgs/Environment env

# Starting state updates. If certain joints should be considered
# at positions other than the current ones, these positions should
# be set here
motion_planning_msgs/Joint[] start

# The goal state for the model to plan for. The goal is achieved
# if all constraints are satisfied
motion_planning_msgs/Constraints goal_constraints

# No state in the produced motion plan will violate these constraints
motion_planning_msgs/Constraints path_constraints

# The name of the motion planner to use. If no name is specified,
# a default motion planner will be used
string planner_id

# The number of times this plan is to be computed. Shortest solution
# will be reported.
int32 num_tries

# The maximum amount of time the motion planner is allowed to plan for
float64 allowed_time

---

# A solution path, if found
motion_planning_msgs/Path path

# distance to goal as computed by internal planning heuristic
# this should be 0.0 if the solution was achieved exactly
float64 distance

# set to true if the computed path was approximate
bool approximate

Additional Messages

Additional messages required to complete this stack.

geometry_primitives/Box

float64 length
float64 width
float64 height

Question / concerns / comments

Enter your thoughts on the API and any questions / concerns you have here. Please sign your name. Anything you want to address in the API review should be marked down here before the start of the meeting.

Ioan

• It seems to me the planner does not need the Joint specification. The planner already has access to the robot model and the information about the joints is not needed (the joint name is sufficient)
• What is the Joint message intended to be? It does not contain all information about a joint (for instance, no axis). Furthermore, the same information is available in the robot description.
• The Box message can be contained by the Object message. Maybe the Object message should exist in geometric_primitives instead?
• Specification of orientation_importance is confusing. This parameter arises from the need to define a metric. The question is -- which metric should a planner use when looking at states? What makes a state closer to another? L2 is not always sufficient, so we have weighted sums (orientation_importance). Could it be beneficial to have a list of available metrics ? The choice of a metric depends on the group of joints we plan for. Maybe that is where the metric should be defined.

• GetMotionPlan should also specify what the acceptable contacts are, even if the planner chooses to ignore that information

Sachin

• The waypoints defined for the controller stack in manipulation_msgs are of the following type

  • # A trajectory waypoint. Contains positions, velocities, accelerations
    # for many dimensions (joints)
    
    float64[] positions
    float64[] velocities
    float64[] accelerations
    float64 time

Should we combine the two specifications?

Mrinal

• JointLimit.msg does not need the angle_wraparound boolean, since the Joint.msg already has joint_type = CONTINUOUS to represent this.

• In my opinion, allowing multi-dof joints just complicates the API. I can't think of any scenario where a multi-dof joint cannot be represented by a set of single-dof joints.

• If multi-dof joints are allowed, then the Joint message needs to have an array of JointLimits[], one for each dof.

• I would like to add a variable "preferred_duration" in the GetMotionPlan service - a hint to the planner about how long the motion should take. Obviously the planner can still do whatever it wants, but the user should be able to roughly control how fast/slow the movement is going to be.

Nikolay (TU Berlin)

(comments by email)

here are my few comments and questions about the motion planning messages:

-in path.msg and GetMotionPlan.srv: how is the desired count of waypoints specified, the sample resolution, maybe there could be a parameter nTimesteps, in addition to the allowed_time parameter

-in Environment.msg : why is the workspace constrained there, isn't it already constrained indirectly for every specific goal through

motion_planning_msgs/Constraints goal_constraints. So it seems that the workspace can be constrained in two ways,

• for every specific goal and more generally for all planning, are these much different?

-In general, I like the joint and pose constraints messages, these are very close to the control variables in our SOC library. Why is there only orientation_importance

weight parameter, it would be more natural to have a specific weight for every different pose constraint. Is there a way to request the robot Jacobean with respect to a specific pose constraint?

- I understand that AcceptableContact.msg treats collisions as properties of the environment. Once the contacts are specified, how can they be used to generate movements under the constraint of no collisions? Is it possible to add a new motion planning constraint type, a no collision constraint. Then it would be more straightforward to combine motion and collision constraints with their weights and gradients in a planner.

-It will be great if you make a small tutorial for this new API showing how to ask a planner to generate a collision free movement moving the robot hand somewhere

Gil

Two areas of concern:

1. I think we should merge the controller waypoint definition with the planner waypoint definition - not only is one message type better than two, but planners should have the option to include velocities and accelerations. Otherwise we will have to revise the message types in order to include dynamic planners.

2. We need to think hard about the sorts of feedback in GetMotionPlan that might be useful both in improving the performance of the move_arm action and eventually in giving a caller of the move_arm action more feedback about planning failure than a simple "we failed". Lack of useful feedback in case of failure was one of Nate Koenig's - an internal user of move_arm - primary complaints. So the main question is what sort of feedback about plan failure could be useful?

   1. The nature of the failure: ik fails, goal in collision, can find a path but can't satisfy constraints, which constraints are particularly problematic, everything ok but just ran out of time, goal ok but all paths in collision, others?
   2. If the constraints can't be met or a non-colliding path found, the best guess at a reasonable path? Maybe this could be useful in deciding what to do next.

Gunter

Not sure how to respond - I think I'm missing some of the underlying assumptions to fully appreciate this stuff. So forgive me if this is off the mark.... My thoughts are basically: a) Where does the planner get it's information about the robot (kinematics and/or dynamics?) Some of it is included in the joint type and limits. But lots must be pulled from some other model? How/why is this information distributed/collected?

b) Constaints to me would be goals? I think of constraints as environment places not to touch, etc. Regardless of naming, specifying tolerance in 3D points as above/below seems strange. What is above? This defines a asymmetric box. Why directions up/down/x/y/z? Why not a sphere? Tolerance in orientation is also tricky. Why switch to roll/pitch/yaw (which I hate anyway ;)? It's seems inconsistent with the actual goal spec. And ambiguous if multiple orientation types are specified.

c) Orientation_importance seems to be a more of a cost function than a goal. Are there other aspects? Seems this is just a small piece of a bigger behavior of an algorithm - probably separate from goals? Or incomplete? Anyway, as given it is in units of length - does that mean anything?

d) How does the planner get actual environment information? I probably not understanding the environment message. Penetration distance seems strange to me. e) Path definitions. Is the planner kinematic only (quasi-static) or dynamic. I distinguish a path without temporal information vs. a trajectory with. If the planner is quasi-static, why have times with way points? Is there an assumption that paths are straight lines between waypoints? Is there a notion of smoothness, in which case there is a notion of path length and curvature as a function of path length, or some parameterization. Which would be analagous to time in a trajectory? In general, I agree that the path or trajectory messages should be shared across the system, so that the output of a planner could be directly sent to a controller.

Wim

• A lot of the information that is in the urdf is duplicated in the joint messages. However, not the whole joint representation is present in the message. Can't the planner just read the urdf?

• The JointState message does not contain names, and therefore has little meaning by itself, especially form trees. Why not use sensor_msgs/JointState?

• In the PoseContraint:

  • The tolerances on xyz make a square region. why not an ellipse?
  • How do you deal with singularities in the RPY orientation?
  • With the "type" you constrain each dof individually. Why then do you specify 6 constraints in the message instead of 1?
  • The orientation_importance seems very specific to one planner. It only allows you to weigh between position and orientation, not between different dof of e.g. the position.
• The Waypoint message has no link to joint names, and therefore the Path message needs to specify these separately.
• How is the environment known to the different components in the system? The robot model comes from the urdf, but where does the environment model come from?

• The AcceptableContact seems like a hack for situations where the planner resolution is not sufficient. Should this not be part of the planner configuration instead of en environment property?

• The "num_tries" seems very specific to some/one planner(s). The maximum allowed time seems more general.
• For the "approximate" bool. Isn't a path always an approximation? What does this actually mean?

Meeting agenda

• Review Joint messages - decide if we can use the existing JointState messages in sensor_msgs or pr2_mechanism_msgs:

• [sensor_msgs/JointState]:
  Header header
    uint32 seq
    time stamp
    string frame_id
  string[] name
  float64[] position
  float64[] velocity
  float64[] effort
  
  [pr2_mechanism_msgs/JointState]:
  string name
  float64 position
  float64 velocity
  float64 applied_effort
  float64 commanded_effort
  byte is_calibrated
  byte saturated
  float64 odometer
  float64 min_position
  float64 max_position
  float64 max_abs_velocity
  float64 max_abs_effort

  The amended proposal is to not use the JointState message at all and instead use the same Waypoint message used for the controllers:

•   float64[] positions
    float64[] velocities
    float64[] accelerations
    float64 time

• Review Waypoint messages for changes to Joint messages above
• Review Path message - does the path message need a start in it?
• Review Constraint messages
  • add weights for every constraints - Is this more of a planner dependent configuration?
  • review constraint messages for changes in Joint and Waypoint messages above
  • constraint specifications can be used by KDL as well?
• Review Environment messages
  • create geometric primitives package?
  • different message for each primitive or one type of message (mapping_msgs/Object)?

  • Review AcceptableContacts message

    • should always be optional
    • confusion about how this is being used
  • Review Environment message
    • why is this specification needed, is it used?
    • how should it be specified - geometric_primitives/Object?

• Review GetMotionPlan.srv

  • more feedback - reason for failure is important feedback - timed out?, failed, goal not valid?
  • No need for Joint[] specification of start position - replace with Waypoint
  • Should model_id be here and not inside the environment message
  • orientation_importance - specify as set of weights on each constraint? - should these weights be in the constraint message then?
  • defining a metric for the planner - should be planner specific configuration parameter
  • Add some specification for (a) expected duration of the plan and (b) expected discretization in time that the returned plan should have

Conclusion

• Review

• Both joint and pose constraint should have additional "weight" field
• Take out orientation importance from the constraint messages.
• Constraints
  • Current specification implies axis-alignment of the box specifying the constraint
  • Specify weights for each constraint separately
  • Separate out pose constraint into two parts - position constraints and orientation constraints and allow each constraint to be specified as a "region" in space

• Rename Environment with WorkspaceParameters

  • Take out model id from the environment and goes into the planner request
  • Take out distance metric from the environment (should be inside the planners themselves)

Package status change mark change manifest)

• Action items that need to be taken.

• Joint level messages

  • Take out JointState.msg, Joint.msg and JointLimits.msg

  • Joints are now only single-dof
• Waypoint/Path messages
  • Waypoints will be shared between controllers and planners

  • Take out model_id and start from the path message - path message will then be shared (instead of WaypointTraj) with the controllers.

  • Final version of messages:

• Path.msg defines a path - essentially a list of waypoints - that would be returned by the planners

• # The definition of a path.
  Header header
  
  # The joint names, in the same order as the values in the waypoints
  string[] joint_names
  
  # A list of waypoints. Each waypoint consists of a set of joint states
  # of dimension the number of joints that we are planning for
  Waypoint[] points

• The Waypoint.msg message

• # A trajectory waypoint. Contains positions, velocities, accelerations
  # for many dimensions (joints)
  
  float64[] positions
  float64[] velocities
  float64[] accelerations
  float64 time

• Constraints
  • Current specification implies axis-alignment of the box specifying the constraint
  • Specify weights for each constraint separately
  • Separate out pose constraint into two parts - position constraints and orientation constraints and allow each constraint to be specified as a "region" in space
  • The final version of the constraint messages:

  • JointConstraint.msg - joint constraints

• Header header
  
  # Constrain the position of a joint to be within a certain bound
  string joint_name
  
  # the bound to be achieved is [position - tolerance_below, position + tolerance_above]
  float64 position
  float64 tolerance_above
  float64 tolerance_below
  
  float64 weight

• PositionConstraint - a position constraint

• # This message contains the definition of a position constraint.
  
  # The robot link this constraint refers to
  string link_name
  
  # The desired pose of the robot link
  geometry_msgs/Point position
  
  # The constraint region shape- The shape of the bounded region that constrains the position of the end-effector
  geometry_primitives/Object constraint_region_shape
  
  # The constraint region pose - The pose of the bounded region that constrains the position of the end-effector. This allows the specification of non-axis aligned constraints
  geometry_msgs/Pose constraint_region_pose
  
  # Constraint weighting factor - a weight for this constraint
  float64 weight

• OrientationConstraint - an orientation constraint

• # This message contains the definition of an orientation constraint.
  
  # The robot link this constraint refers to
  string link_name
  
  # The desired orientation of the robot link specified as a roll pitch and yaw.
  geometry_msgs/Point orientation
  
  # The constraint region shape- The shape of the bounded region that constrains the orientation of the link. This is a shape in 3D roll pitch yaw shape.
  geometry_primitives/Object constraint_region_shape
  
  # The constraint region pose - The pose of the bounded region that constrains the position of the link. This allows the specification of non-axis aligned constraints
  geometry_msgs/Pose constraint_region_pose
  
  # Constraint weighting factor - a weight for this constraint
  float64 weight

• Constraints.msg

• motion_planning_msgs/JointConstraint[] joint_constraints
  motion_planning_msgs/PositionConstraint[] position_constraints
  motion_planning_msgs/OrientationConstraint[] orientation_constraints

• ConstraintErrors.msg

• float64[] joint_constraint_errors
  float64[] position_constraint_errors
  float64[] orientation_constraint_errors

• Environment.msg

• AcceptableContact.msg - This message defines a region of space where contacts with certain links are allowed, up to a certain depth

• # The shape of the region in the environment where contacts are allowed
  mapping_msgs/Object contact_region_shape
  
  # The pose of the space defining the region where contacts are allowed
  geometry_msgs/PoseStamped contact_region_pose
  
  # The set of links that are allowed to touch obstacles
  string[] link_names
  
  # The maximum penetration depth allowed
  float64 penetration_depth

• WorkspaceParameters.msg sets up the workspace that the planner will use for planning and allows for specification of contacts. The acceptable contacts message allows the specification of contact regions in the environment that the robot is allowed to touch, e.g. any object that has to be grasped must be touched by the robot. The workspace defines a shape within which all planned paths must stay - for 2D it would define a region on the ground within which all paths are constrained to stay.

• # This message contains a set of parameters useful in
  # setting up the workspace for planning
  
  geometry_primitives/Object   workspace_region_shape
  geometry_msgs/PoseStamped    workspace_region_pose
  
  # A list of allowed contacts
  AcceptableContact[] contacts

• Motion planning service

• GetMotionPlan.srv

• # This service contains the definition for a request to the motion
  # planner and the output it provides
  
  # Parameters for the workspace that the planner should work inside
  motion_planning_msgs/WorkspaceParameters workspace_parameters
  
  # Starting state updates. If certain joints should be considered
  # at positions other than the current ones, these positions should
  # be set here
  motion_planning_msgs/Waypoint start
  
  # The goal state for the model to plan for. The goal is achieved
  # if all constraints are satisfied
  motion_planning_msgs/Constraints goal_constraints
  
  # No state at any point along the path in the produced motion plan will violate these constraints
  motion_planning_msgs/Constraints path_constraints
  
  # The name of the motion planner to use. If no name is specified,
  # a default motion planner will be used
  string planner_id
  
  # The name of the model on which this planner is operating
  string model_id
  
  # The number of times this plan is to be computed. Shortest solution
  # will be reported.
  int32 num_planning_attempts
  
  # The maximum amount of time the motion planner is allowed to plan for
  float64 planning_timeout
  
  # An expected path duration (in seconds) along with an expected discretization of the path allows the planner to determine the discretization of the trajectory that it returns
  
  float64 expected_path_duration
  float64 expected_path_dt
  
  ---
  
  # A solution path, if found
  motion_planning_msgs/Path path
  
  # Planning time
  float64 planning_time
  
  # Constraint errors
  motion_planning_msgs/ConstraintErrors goal_constraint_errors
  motion_planning_msgs/ConstraintErrors path_constraint_errors
  
  # Error code - encodes the reason for failure
  int32 error_code
  
  int32 SUCCESS = 1
  int32 FAILURE  = 2
  int32 TIMEOUT = 3
  int32 GOAL_IN_COLLISION = 4
  int32 CONSTRAINT_VIOLATION = 5
  int32 IK_FAILURE = 6

Stu

• "Path" is generally used to refer to things that don't involve time.

• If the Path messages has "joint_names" as a field, then it should be called JointPath

• PositionConstraint:

  • There's a field named "pose" which is actually a point
  • What happens if both the desired link pose and the constraint region are specified? Can they conflict with each other?

• The "_region_pose" in AcceptableContact is stamped. Should the poses in the *Constraint messages also be stamped?

• Major issues that need to be resolved

Nate

• Should "joint_name" be in the JointState msg? It seems this would reduce the number of times "joint_names" are in messages.

• What is the difference between a "joint" and a "link"? This should be clearly specified somewhere.
• In the Path message:
  • The "start" item is confusing. It seems like the first element in the array should be the start.

  • Why do we need a list of joint_names? This info should be included at a lower level, probably in JointState.

• GetMotionPlan.srv

  • Will this return a indication of success or failure?

Gil

• In GetMotionPlan.srv error code should be an error string. There's no way we can anticipate all the errors that may need to be passed.