API review

Proposer: E. Gil Jones, Sachin Chitta

Present at review:

• List reviewers

Proposed API

• MoveArm.action

• #Acceptable contacts for objects in the environment
  AcceptableContact[] contacts
  
  # 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
  ---
  # How close did the planner get to the goal, in joint space or cartesian?
  double[] distance_to_goal
  
  # Some way to communicate resulting status back to client, so they can decide
  # what to do next - success, what kind of failure
  string status
  ---
  # Some way of communicating back what move arm is doing
  # i.e. no contact planning, contact planning, no contact execution, 
  # searching for valid configuration, no contact execution, 
  # contact execution
  string current_mode

Gil

Open questions:

• Do we report any arm configuration in either feedback or goal, or do we just expect consumers of move arm to be watching that?
• Do we allow control over the function of move arm in the action request - i.e. which planner to use, or other aspects of the internal move arm flow? Or everything on configuration/launch?

• Do we include the full workspace parameters message, or is just AcceptableContacts sufficient?

• Do we give any control over things like planner smoothing? I.e. take more planning time but produce smoother paths?
• Any more planner configuration? Like time allowed for planning, time allowed for execution?

Gil's thoughts on tests, use cases, and move_arm internal functionality

I'm trying to figure out several things - how to comprehensively test move_arm, how to set up move_arm so that it supports application use cases, and how the internal functionality should work both to pass tests and implement desired use cases.

Test 1: Simple joint goal test

Test function: this test (currently implemented as regression_test_joint_goal) sets desired goal poses for several joints in the absence of obstacles (other than the robot itself).

Status during: no self collisions

Status afterward: move_arm reports success, and the arm ends up within the tolerances of the goal configuration

Current move_arm internal approach: Use non-contact planning. Joint goal should represent a valid state, so jump straight to planning.

Whether move_arm currently does this correctly: yes, and the internal functionality is correct

Test 2: Simple pose goal test

Test function: this test (currently implemented as regression_test_pose_goal) sets a desired goal pose (position and orientation) for the right wrist roll link.

Status during: no self collisions

Status afterward: move_arm reports success, and the right wrist roll link ends up within the tolerances specified in the request

Current move_arm internal approach: calls IK first with the exact goal pose, and then up to 9 more times with sampled points in the goal pose region (as defined by the tolerances). If IK returns a valid configuration at any point pass to long range planner. Otherwise call complexGoals (should not be called in this test)

Whether move_arm currently does this correctly: the test passes, though whether this is the right general functionality is a question worth discussing

Test 3: End pose with path constraints

Test function: this test (currently implemented as regression_test_cylinder_constraint) sets two goals. The first has no path constraint and is just a right wrist position that brings the robot closer to the robot. The second goal point is directly beneath the first, with a path constraint limiting the acceptable paths to be within .05 meters in x and y, the z constraint to constrain the path to not go too far beneath the goal, with orientation constraints in roll and pitch (though no constraint in yaw)

Status during: the test (or move_arm) needs to make sure that the constraints are not violated during the move

Status afterward: move_arm returns success and the goal configuration is reached within tolerances

Current move_arm internal approach: calls IK first with the exact goal pose, and then up to 9 more times with sampled points in the goal pose region (as defined by the tolerances). If IK returns a valid configuration at any point pass to long range planner. Otherwise call complexGoals (should not be called in this test).

Whether move_arm currently does this correctly: mostly yes, as long as IK returns a valid state, though I haven't yet implemented the strict monitoring of the trajectory. If IK doesn't return as valid, and long range is set to false, meaning that GA IK doesn't get evoked, then KPIECE or RRT get run, and they don't seem to always obey constraints.

Test 4: Non-contact planner shouldn't do anything if valid state not found

Test function: test that something reasonable happens when not using short range planner and an invalid state (due to self collision) is passed into move_arm.

Status during: Intermediate failures to find working state if that takes a while

Status afterward: Failure reported, with representative constraint.

Current move_arm approach: Move_arm will attempt to do IK with the goal point and then with sampled points. If none of these work the pose goal is passed directly into the long range planner. The long range planner can only produce approximate solutions. If the approximate solutions are reported as .1 in distance they are sent as trajectories. This process seems to continue indefinitely unless an outside preempt is sent.

Whether move_arm currently does this correctly: the correct behavior is a matter for discussion, but the current way needs to be altered.

Test 5: Correct behavior when self colliding

Test function: If somehow following the plan results in unacceptable/unexpected collision, reasonable things should happen. If we are running fake planning for the short range it can hit things. This test sets a point inside or near the base. When the arm hits the action should fail and report status.

Status during: Intermediate state has been reached

Status afterward: move_arm returns failure of self-collision

Current move_arm approach: GA_IK used to generate a near valid state. If the state isn't in the goal region (which it shouldn't be if the goal is actually inside the base) the near valid goal becomes and intermediate joint goal that is passed to the non-contact planner. After the intermediate state is reached the contact planner will be invoked, and if using the fake planner will just try to directly command the joints, which results in collision. The failure and status should be supported.

Whether move_arm currently does this correctly: Once collision has occurred the contact planner is repeatedly invoked and can't do anything. Move_arm should probably figure this out and fail. Also, whether this is the right thing for move_arm to do (the intermediate state) is an open question.

Test 6: Basic obstacle avoidance with joints

Test function: Make sure that not just self-collisions but real obstacles can be avoided. Move_arm moves the arm from one position on one side of a pole to the other side.

Status during: any collisions

Status afterward: arm in desired joint configuration without hitting anything.

Current move_arm approach: just like the other joint command

Whether move_arm currently does this correctly: haven't tried it yet, but I bet it will work.

Other random thoughts

* Move_arm needs checks that keep planners from violating allowed_time. * Grasping support needs to be thought about more

Meeting agenda

To be filled out by proposer based on comments gathered during API review period

Conclusion

Package status change mark change manifest)

• Action items that need to be taken.

• Major issues that need to be resolved