Note: This tutorial assumes that you have completed the previous tutorials: Specifying complex pose goals.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Specifying path constraints for motion planning

Description: This tutorial will show you how to specify path constraints for move_arm. This is useful, e.g., if you want to execute tasks like moving a glass with water in it. You can use the path constraints to specify that the glass should stay approximately upright.

Tutorial Level: ADVANCED

In this tutorial, you will learn how to set path constraints for the arm navigation stack. Path constraints are spatial constraints that are applied all along the path that the motion planner needs to follow. They are not temporal constraints though.

Path constraints will allow you to, e.g., create constraints that can keep the gripper upright. This is useful if you are carrying a container with liquid in it. If you have already completed the previous few tutorials, you already know how to send goals to the move_arm action so we won't discuss that in detail. Instead, we will focus on adding path constraints.

ROS Setup

To use this tutorial make sure that you've run the following:

sudo apt-get install ros-electric-pr2-arm-navigation

Also make sure that in each terminal you use you've run:

export ROBOT=sim

Running in simulation

To use the arm planning stack, we first need to launch the simulator.

roslaunch pr2_gazebo pr2_empty_world.launch

The simulator should come up.

Now, launch the arm navigation stack:

roslaunch pr2_3dnav right_arm_navigation.launch

Next, run this executable from the pr2_arm_navigation_tutorials stack:

rosrun pr2_arm_navigation_tutorials move_arm_simple_pose_goal

If it succeeds, you should see the following output

[ INFO] 115.660000000: Connected to server
[ INFO] 121.854000000: Action finished: SUCCEEDED

The robot arm will go to the first arm position and then move to the next goal position while maintaining the gripper almost upright.

Here's a movie of the robot using path constraints. Note that the first part of the path had no constraints imposed on it while the second part required the gripper to stay upright.

Things to remember

Note that we first sent the arm to a position where the gripper was upright before imposing the path constraints. Did you wonder why?

The starting pose of the robot must also satisfy the path constraints since it is a part of the ultimate path for the robot. It is very important to remember that when you use path constraints, the first/initial/starting state that the robot is currently in MUST SATISFY THESE CONSTRAINTS.

Limitations

Dealing with path constraints is not yet mature. In particular, you may sometimes have to increase the tolerances to allow the planner to plan with path constraints.

The code

The code for this example can be found in the package pr2_arm_navigation_tutorials in the file src/move_arm_path_constraints.cpp. Select one of the buttons below to show the code and explanation for your distribution:

   1 #include <ros/ros.h>
   2 #include <actionlib/client/simple_action_client.h>
   3 #include <arm_navigation_msgs/MoveArmAction.h>
   4 
   5 int main(int argc, char **argv){
   6   ros::init (argc, argv, "move_arm_joint_goal_test");
   7   ros::NodeHandle nh;
   8   actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm",true);
   9 
  10   move_arm.waitForServer();
  11   ROS_INFO("Connected to server");
  12 
  13   arm_navigation_msgs::MoveArmGoal goalA;
  14 
  15   goalA.motion_plan_request.group_name = "right_arm";
  16   goalA.motion_plan_request.num_planning_attempts = 1;
  17   goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
  18 
  19   nh.param<std::string>("planner_id",goalA.motion_plan_request.planner_id,std::string(""));  nh.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("ompl_planning/
  20 plan_kinematic_path"));
  21   goalA.motion_plan_request.goal_constraints.set_position_constraints_size(1);
  22   goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();  goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_li
  23 nk";
  24     
  25   goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
  26   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.2;
  27   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.90;
  28   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0;
  29       goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = a
  30 rm_navigation_msgs::Shape::BOX;  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensio
  31 ns.push_back(0.02);  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensio
  32 ns.push_back(0.02);  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensio
  33 ns.push_back(0.02);
  34   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w 
  35 = 1.0;
  36   goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
  37 
  38   goalA.motion_plan_request.goal_constraints.set_orientation_constraints_size(1);
  39 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
  40   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_li
  41 nk";    
  42   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
  43   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
  44   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
  45   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = 0.0;
  46   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 1.0;
  47     
  48   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04;
  49   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04;
  50   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04;
  51 
  52   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
  53 
  54   if (nh.ok())
  55   {
  56     bool finished_within_time = false;
  57     move_arm.sendGoal(goalA);
  58     finished_within_time = move_arm.waitForResult(ros::Duration(200.0));    if (!finished_within_time)
  59     {
  60       move_arm.cancelGoal();
  61       ROS_INFO("Timed out achieving goal A");
  62     }
  63     {
  64       actionlib::SimpleClientGoalState state = move_arm.getState();
  65       bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
  66       if(success)
  67         ROS_INFO("Action finished: %s",state.toString().c_str());
  68       else
  69         ROS_INFO("Action failed: %s",state.toString().c_str());
  70     }
  71   }
  72 
  73   ros::Duration(1.0).sleep();
  74   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.60;
  75   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.288;
  76   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0;
  77 
  78   goalA.motion_plan_request.path_constraints.orientation_constraints.resize(1);  goalA.motion_plan_request.path_constraints.orientation_constraints[0].header.frame_id = "torso_lift_li
  79 nk";
  80   goalA.motion_plan_request.path_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
  81   goalA.motion_plan_request.path_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
  82    
  83   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.x = 0.0;
  84   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.y = 0.0;
  85   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.z = 0.0;
  86   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.w = 1.0;
  87   goalA.motion_plan_request.path_constraints.orientation_constraints[0].type = arm_navigation_msgs::Orie
  88 ntationConstraint::HEADER_FRAME;
  89   goalA.motion_plan_request.path_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.2;
  90   goalA.motion_plan_request.path_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.2;
  91   goalA.motion_plan_request.path_constraints.orientation_constraints[0].absolute_yaw_tolerance = M_PI;
  92 
  93   if (nh.ok())
  94   {
  95     bool finished_within_time = false;
  96     move_arm.sendGoal(goalA);
  97     finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
  98     if (!finished_within_time)
  99     {
 100       move_arm.cancelGoal();
 101       ROS_INFO("Timed out achieving goal A");
 102     }
 103     else
 104     {
 105       actionlib::SimpleClientGoalState state = move_arm.getState();
 106       bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
 107       if(success)
 108         ROS_INFO("Action finished: %s",state.toString().c_str());
 109       else
 110         ROS_INFO("Action failed: %s",state.toString().c_str());
 111     }
 112   }
 113   ros::shutdown();
 114 }

This code is pretty similar to the code used for specifying goals as a pose constraint. The major difference is in the use of path constraints to restrict the path taken by the arm. The intention of this code is to first move the robot arm to a position with the gripper upright using a request without path constraints and then move the right arm to another position while specifying a constraint that keeps the gripper upright.

Now, we'll break down the code for adding path constraints. A detailed description of the code before that can be found in the previous tutorial.

Specifying Path Constraints

We would like to specify a constraint that keeps the gripper pose almost upright. This corresponds to an orientation constraint where the yaw of the r_wrist_roll_link is unrestricted while the roll and pitch are constrained to lie within a small range of 0.0. We will specify this constraint by specifying two pieces of information

Specifying a nominal orientation

We first specify a nominal orientation for the r_wrist_roll_link in the torso_lift_link_frame. This orientation corresponds to keeping the gripper upright.

  79 nk";
  80   goalA.motion_plan_request.path_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
  81   goalA.motion_plan_request.path_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
  82    
  83   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.x = 0.0;
  84   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.y = 0.0;
  85   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.z = 0.0;
  86   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.w = 1.0;
  87   goalA.motion_plan_request.path_constraints.orientation_constraints[0].type = arm_navigation_msgs::Orie

Specifying an allowable constraint region

We will now specify an allowable constraint region using the tolerance fields of the message. In essence, we are specifying a tolerance region for the difference between a candidate gripper orientation and the nominal orientation, i.e. we are specifying a relative roll, pitch and yaw that the candidate gripper orientation can be off from the nominal orientation. In this case, since we want the roll and pitch to stay around 0.0, we specify a small error threshold for the roll and pitch while specifying a large threshold for the yaw (thus allowing it to be unrestricted).

  90   goalA.motion_plan_request.path_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.2;
  91   goalA.motion_plan_request.path_constraints.orientation_constraints[0].absolute_yaw_tolerance = M_PI;

Specifying the ''type'' of the constraint

There is an additional piece of information that we are specifying in the constraint specification that is important.

  89   goalA.motion_plan_request.path_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.2;

The orientation constraint type can be either HEADER_FRAME or LINK_FRAME (The types are specified within the message itself). The type specifies the frame in which the error roll, pitch and yaw is calculated while evaluating the constraint. type = HEADER_FRAME implies that these angles are computed assuming that the rotations are carried out in the frame specified in the header (torso_lift_link in this example). type=LINK_FRAME implies that the roll, pitch and yaw are computed in the frame of the target link frame (r_wrist_roll_link in this example) for which the constraint is specified.

This code is for the Diamondback distribution.

   1 #include <ros/ros.h>
   2 #include <actionlib/client/simple_action_client.h>
   3 #include <move_arm_msgs/MoveArmAction.h>
   4 
   5 int main(int argc, char **argv){
   6   ros::init (argc, argv, "move_arm_joint_goal_test");
   7   ros::NodeHandle nh;
   8   actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm("move_right_arm",true);
   9 
  10   move_arm.waitForServer();
  11   ROS_INFO("Connected to server");
  12 
  13   move_arm_msgs::MoveArmGoal goalA;
  14 
  15   goalA.motion_plan_request.group_name = "right_arm";
  16   goalA.motion_plan_request.num_planning_attempts = 1;
  17   goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
  18 
  19   nh.param<std::string>("planner_id",goalA.motion_plan_request.planner_id,std::string(""));
  20   nh.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("ompl_planning/plan_kinematic_path"));
  21   goalA.motion_plan_request.goal_constraints.set_position_constraints_size(1);
  22   goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
  23   goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link";
  24     
  25   goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
  26   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.2;
  27   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.90;
  28   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0;
  29     
  30   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = geometric_shapes_msgs::Shape::BOX;
  31   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
  32   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
  33   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
  34 
  35   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
  36   goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
  37 
  38   goalA.motion_plan_request.goal_constraints.set_orientation_constraints_size(1);
  39   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
  40   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";    
  41   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
  42   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
  43   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
  44   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = 0.0;
  45   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 1.0;
  46     
  47   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04;
  48   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04;
  49   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04;
  50 
  51   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
  52 
  53   if (nh.ok())
  54   {
  55     bool finished_within_time = false;
  56     move_arm.sendGoal(goalA);
  57     finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
  58     if (!finished_within_time)
  59     {
  60       move_arm.cancelGoal();
  61       ROS_INFO("Timed out achieving goal A");
  62     }
  63     else
  64     {
  65       actionlib::SimpleClientGoalState state = move_arm.getState();
  66       bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
  67       if(success)
  68         ROS_INFO("Action finished: %s",state.toString().c_str());
  69       else
  70         ROS_INFO("Action failed: %s",state.toString().c_str());
  71     }
  72   }
  73 
  74   ros::Duration(1.0).sleep();
  75   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.60;
  76   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.288;
  77   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0;
  78 
  79   goalA.motion_plan_request.path_constraints.orientation_constraints.resize(1);
  80   goalA.motion_plan_request.path_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";
  81   goalA.motion_plan_request.path_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
  82   goalA.motion_plan_request.path_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
  83    
  84   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.x = 0.0;
  85   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.y = 0.0;
  86   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.z = 0.0;
  87   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.w = 1.0;
  88 
  89   goalA.motion_plan_request.path_constraints.orientation_constraints[0].type = motion_planning_msgs::OrientationConstraint::HEADER_FRAME;
  90   goalA.motion_plan_request.path_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.2;
  91   goalA.motion_plan_request.path_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.2;
  92   goalA.motion_plan_request.path_constraints.orientation_constraints[0].absolute_yaw_tolerance = M_PI;
  93 
  94   if (nh.ok())
  95   {
  96     bool finished_within_time = false;
  97     move_arm.sendGoal(goalA);
  98     finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
  99     if (!finished_within_time)
 100     {
 101       move_arm.cancelGoal();
 102       ROS_INFO("Timed out achieving goal A");
 103     }
 104     else
 105     {
 106       actionlib::SimpleClientGoalState state = move_arm.getState();
 107       bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
 108       if(success)
 109         ROS_INFO("Action finished: %s",state.toString().c_str());
 110       else
 111         ROS_INFO("Action failed: %s",state.toString().c_str());
 112     }
 113   }
 114   ros::shutdown();
 115 }

This code is pretty similar to the code used for specifying goals as a pose constraint. The major difference is in the use of path constraints to restrict the path taken by the arm. The intention of this code is to first move the robot arm to a position with the gripper upright using a request without path constraints and then move the right arm to another position while specifying a constraint that keeps the gripper upright.

Now, we'll break down the code for adding path constraints. A detailed description of the code before that can be found in the previous tutorial.

Specifying Path Constraints

We would like to specify a constraint that keeps the gripper pose almost upright. This corresponds to an orientation constraint where the yaw of the r_wrist_roll_link is unrestricted while the roll and pitch are constrained to lie within a small range of 0.0. We will specify this constraint by specifying two pieces of information

Specifying a nominal orientation

We first specify a nominal orientation for the r_wrist_roll_link in the torso_lift_link_frame. This orientation corresponds to keeping the gripper upright.

  79   goalA.motion_plan_request.path_constraints.orientation_constraints.resize(1);
  80   goalA.motion_plan_request.path_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";
  81   goalA.motion_plan_request.path_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
  82   goalA.motion_plan_request.path_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
  83    
  84   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.x = 0.0;
  85   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.y = 0.0;
  86   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.z = 0.0;
  87   goalA.motion_plan_request.path_constraints.orientation_constraints[0].orientation.w = 1.0;

Specifying an allowable constraint region

We will now specify an allowable constraint region using the tolerance fields of the message. In essence, we are specifying a tolerance region for the difference between a candidate gripper orientation and the nominal orientation, i.e. we are specifying a relative roll, pitch and yaw that the candidate gripper orientation can be off from the nominal orientation. In this case, since we want the roll and pitch to stay around 0.0, we specify a small error threshold for the roll and pitch while specifying a large threshold for the yaw (thus allowing it to be unrestricted).

  90   goalA.motion_plan_request.path_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.2;
  91   goalA.motion_plan_request.path_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.2;
  92   goalA.motion_plan_request.path_constraints.orientation_constraints[0].absolute_yaw_tolerance = M_PI;

Specifying the ''type'' of the constraint

There is an additional piece of information that we are specifying in the constraint specification that is important.

  89   goalA.motion_plan_request.path_constraints.orientation_constraints[0].type = motion_planning_msgs::OrientationConstraint::HEADER_FRAME;

The orientation constraint type can be either HEADER_FRAME or LINK_FRAME (The types are specified within the message itself). The type specifies the frame in which the error roll, pitch and yaw is calculated while evaluating the constraint. type = HEADER_FRAME implies that these angles are computed assuming that the rotations are carried out in the frame specified in the header (torso_lift_link in this example). type=LINK_FRAME implies that the roll, pitch and yaw are computed in the frame of the target link frame (r_wrist_roll_link in this example) for which the constraint is specified.

Wiki: move_arm/Tutorials/Tutorial 5 (last edited 2017-10-22 07:43:01 by TullyFoote)