| 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).
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.
