Note: This tutorial assumes that you have completed the previous tutorials: Moving the arm to a pose goal.

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 complex pose goal constraints

Description: In this tutorial, we will use the action client to send a pose goal for the move_arm node to move the arm to. We will also learn how to specify a region of tolerance for the pose goal using a geometric shape.

Tutorial Level: ADVANCED

Next Tutorial: Specifying path constraints for motion planning

Contents

ROS Setup
Running in simulation
The code

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 in the simulator will look something like this

The code

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

This code is for the Electric 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(""));
  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.75;
  27   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.188;
  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 = arm_navigation_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   ros::shutdown();
  74 }

Now, we'll break the code down line by line:

   3 #include <arm_navigation_msgs/MoveArmAction.h>
   4

This line includes the action specification for move_arm which is a ROS action that exposes a high level interface to the arm planning stack. Essentially, the move_arm action accepts goals from clients and attempts to move the robot arm to the specified position/orientation in the world. For a detailed discussion of ROS actions see the [actionlib:actionlib documentation].

   8   actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm",true);

This line constructs an action client that we'll use to communicate with the action named "move_right_arm" that adheres to the MoveArmAction interface. It also tells the action client to start a thread to call ros::spin() so that ROS callbacks will be processed by passing "true" as the second argument of the MoveArmClient constructor.

  10   move_arm.waitForServer();

These lines wait for the action server to report that it has come up and is ready to begin processing goals.

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

Here we create a goal to send to move_arm using the arm_navigation_msgs::MoveArmGoal message type which is included automatically with the MoveArmAction.h header. The goal is specified within the motion_plan_request field of the move arm goal message and consists of multiple fields.

group_name : This is the group of joints that we are planning for. The group names are pre-defined on the parameter server using a yaml configuration files.
planner_id : This is the name of the planner that we are planning for.
planner_service_name: This is the service call name that move_arm will use to call the planner. This allows move_arm to address multiple planners at the same time. The planner_id is different from the planner_service_name since some planning libraries might offer different types of planners within the same service call.
goal_constraints : The goal is specified as a vector of constraints with tolerances. In this cases there is a single position constraint on the end-effector and a single orientation constraint on the end-effector.
num_planning_attempts - this specifies the number of times move_arm will call the planner to try and get a successful plan
allowed_planning_time - This parameter is passed in to the planners and specifies the maximum amount of time they are allowed to plan for.

Specifying a position constraint

  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.75;
  27   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.188;
  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 = arm_navigation_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;

Each position constraints is specified with a header, a link name, the goal position that we want the link to reach and a tolerance region specified using a geometric_shapes/Shape message. In this case we are trying to move the end-effector link (r_wrist_roll_link) to the position (0.75,-0.188,0) in the torso_lift_link frame. Note also that we fill the header with the current time (ros::Time::now()).

  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 = arm_navigation_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);

We also specify a tolerance region for the final planned goal to lie within a box of dimension 0.02 cm x 0.02 cm x 0.02 cm.

  33   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);

The box is oriented along the coordinate axes of the torso_lift_link frame.

  36   goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;

A weighting factor is attached to each constraint. The planners are free to interpret the weighting factor in any manner they like.

Specifying an orientation constraint

  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;

Each orientation constraints is specified with a header, a link name, the goal orientation that we want the link to reach and tolerance. The goal orientation is encoded in a geometry_msgs/Orientation message as a quaternion. In this case we are trying to move the end-effector link (r_wrist_roll_link) to the orientation (roll = 0.0,pitch = -0.0, yaw = 0.0) in the torso_lift_link frame. Note also that we fill the header with the current time (ros::Time::now()).

  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;

We also specify absolute tolerances for the roll, pitch and yaw as 0.04 radians, i.e. the final pose is acceptable if its absolute roll, pitch and yaw in the body fixed desired link frame is within 0.04 radians. Note that these error tolerances are by default set in the link frame, i.e. in the body fixed frame of the link we are planning for. To specify these tolerances in the header frame, i.e. in the frame specified in the header of the message, we specify the orientation constraint type appropriately:

   1 goalA.goal_constraints.orientation_constraints[0].type = goalA.goal_constraints.orientation_constraints[0].HEADER_FRAME;

  51   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;

An optional weighting factor is attached to each constraint (the weighting factors are currently reserved for future use).

  56     move_arm.sendGoal(goalA);

The call to move_arm.sendGoal will actually push the goal out over the wire to the move_arm node for processing.

  57     finished_within_time = move_arm.waitForResult(ros::Duration(200.0));

The only thing left to do now is to wait for the goal to finish using the move_arm.waitForGoalToFinish(ros::Duration(200.0)) call which will block until the move_arm action is done processing the goal we sent it or 200 seconds have passed, whichever happens earlier. After it finishes, we can check if the goal succeeded or failed and output a message to the user accordingly.

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.75;
  27   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.188;
  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   ros::shutdown();
  74 }

Now, we'll break the code down line by line:

   3 #include <move_arm_msgs/MoveArmAction.h>
   4

   8   actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm("move_right_arm",true);

  10   move_arm.waitForServer();

These lines wait for the action server to report that it has come up and is ready to begin processing goals.

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

Here we create a goal to send to move_arm using the move_arm_msgs::MoveArmGoal message type which is included automatically with the MoveArmAction.h header. The goal is specified within the motion_plan_request field of the move arm goal message and consists of multiple fields.

group_name : This is the group of joints that we are planning for. The group names are pre-defined on the parameter server using a yaml configuration files.
planner_id : This is the name of the planner that we are planning for.
planner_service_name: This is the service call name that move_arm will use to call the planner. This allows move_arm to address multiple planners at the same time. The planner_id is different from the planner_service_name since some planning libraries might offer different types of planners within the same service call.
goal_constraints : The goal is specified as a vector of constraints with tolerances. In this cases there is a single position constraint on the end-effector and a single orientation constraint on the end-effector.
num_planning_attempts - this specifies the number of times move_arm will call the planner to try and get a successful plan
allowed_planning_time - This parameter is passed in to the planners and specifies the maximum amount of time they are allowed to plan for.

Specifying a position constraint

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

We also specify a tolerance region for the final planned goal to lie within a box of dimension 0.02 cm x 0.02 cm x 0.02 cm.

  33   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);

The box is oriented along the coordinate axes of the torso_lift_link frame.

  36   goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;

A weighting factor is attached to each constraint. The planners are free to interpret the weighting factor in any manner they like.

Specifying an orientation constraint

  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;

  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;

   1 goalA.goal_constraints.orientation_constraints[0].type = goalA.goal_constraints.orientation_constraints[0].HEADER_FRAME;

  51   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;

An optional weighting factor is attached to each constraint (the weighting factors are currently reserved for future use).

  56     move_arm.sendGoal(goalA);

The call to move_arm.sendGoal will actually push the goal out over the wire to the move_arm node for processing.

  57     finished_within_time = move_arm.waitForResult(ros::Duration(200.0));

ROS 2 Documentation

Wiki

Page

User

Specifying complex pose goal constraints

ROS Setup

Running in simulation

The code

Specifying a position constraint

Specifying an orientation constraint

Specifying a position constraint

Specifying an orientation constraint