Description/Walkthrough of Tolerances in move_arm

edit

John,

There are two sets of constraints in the MotionPlanRequest that gets included in the MoveArmGoal - goal constraints and path constraints. The goal constraints govern what regions will be considered acceptable goals, and the path constraints govern what states will be considered valid on a path between the start and the goal states. So assuming you are starting out with your gripper down, you'll want to set not just position constraints for your goal but also orientation constraints. You'll note that the orientation constraints contain fields for absolute_roll_tolerence as well as for pitch and yaw - these are the values you'll want to set to allow some play in what constitutes straight down to make it more likely that a solution can be found.

You'll also want to set path constraints of a similar variety - just on orientation, set to the current rotation of the gripper, with some small tolerances in pitch and yaw and a large tolerance in roll, as you don't want to constraint it and make the problem harder.

A big caveat - when you pass this kind of constraint into OMPL (which is what move_arm will do) it will attempt to sample in Cartesian space and use whatever kinematics plug-in you've configured to solve IK and generate a trajectory for your arm. If you use the generic IK provided in arm_navigation you will generally fail to find a valid plan. We can do this kind of planning on the PR2, though it's more computationally expensive, because we have a very good analytic kinematics solver, and it can still take seconds to find a feasible path in some cases. For situations where there's no IK solution as configured the numeric solver is around 1000 times slower than the PR2 analytic solver, and performs especially poorly when there isn't a good seed state, as is often the case in sampling. Also worth noting that unless you are using an arm with at least 7-DOF and can exploit redundancy it's unlikely that you'll be able to maintain this kind of constraint. If you want to try this out and don't have a custom analytic ik solver for your arm I suggest using the PR2 - you can look in pr2_arm_navigation/pr2_arm_navigation_planning/config/ompl_planning.yaml to see how to specify that OMPL should use the PR2-specific solver.

Here's a function from the planning components visualizer that sets the orientation constraints for path and goal with pitch and roll constraints while leaving yaw unconstrained:

void determinePitchRollConstraintsGivenState(const PlanningComponentsVisualizer::GroupCollection& gc,
                                               const planning_models::KinematicState& state,
                                               arm_navigation_msgs::OrientationConstraint& goal_constraint,
                                               arm_navigation_msgs::OrientationConstraint& path_constraint) const
  {
    btTransform cur = state.getLinkState(gc.ik_link_name_)->getGlobalLinkTransform();
    goal_constraint.header.frame_id = cm_->getWorldFrameId();
    goal_constraint.header.stamp = ros::Time::now();
    goal_constraint.link_name = gc.ik_link_name_;
    tf::quaternionTFToMsg(cur.getRotation(), goal_constraint.orientation);
    goal_constraint.absolute_roll_tolerance = 0.04;
    goal_constraint.absolute_pitch_tolerance = 0.04;
    goal_constraint.absolute_yaw_tolerance = M_PI;
    path_constraint.header.frame_id = cm_->getWorldFrameId();
    path_constraint.header.stamp = ros::Time::now();
    path_constraint.link_name = gc.ik_link_name_;
    tf::quaternionTFToMsg(cur.getRotation(), path_constraint.orientation ...