| Note: This tutorial assumes that you have completed the previous tutorials: Moving the arm to a joint 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. |
Moving the arm to a pose goal
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.Tutorial Level: INTERMEDIATE
Next Tutorial: Specifying complex pose goals
Contents
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
4 #include <arm_navigation_msgs/MoveArmAction.h>
5 #include <arm_navigation_msgs/utils.h>
6
7 int main(int argc, char **argv){
8 ros::init (argc, argv, "move_arm_pose_goal_test");
9 ros::NodeHandle nh;
10 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm",true);
11 move_arm.waitForServer();
12 ROS_INFO("Connected to server");
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.planner_id = std::string("");
18 goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
19 goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
20
21 arm_navigation_msgs::SimplePoseConstraint desired_pose;
22 desired_pose.header.frame_id = "torso_lift_link";
23 desired_pose.link_name = "r_wrist_roll_link";
24 desired_pose.pose.position.x = 0.75;
25 desired_pose.pose.position.y = -0.188;
26 desired_pose.pose.position.z = 0;
27
28 desired_pose.pose.orientation.x = 0.0;
29 desired_pose.pose.orientation.y = 0.0;
30 desired_pose.pose.orientation.z = 0.0;
31 desired_pose.pose.orientation.w = 1.0;
32
33 desired_pose.absolute_position_tolerance.x = 0.02;
34 desired_pose.absolute_position_tolerance.y = 0.02;
35 desired_pose.absolute_position_tolerance.z = 0.02;
36
37 desired_pose.absolute_roll_tolerance = 0.04;
38 desired_pose.absolute_pitch_tolerance = 0.04;
39 desired_pose.absolute_yaw_tolerance = 0.04;
40
41 arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
42
43 if (nh.ok())
44 {
45 bool finished_within_time = false;
46 move_arm.sendGoal(goalA);
47 finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
48 if (!finished_within_time)
49 {
50 move_arm.cancelGoal();
51 ROS_INFO("Timed out achieving goal A");
52 }
53 else
54 {
55 actionlib::SimpleClientGoalState state = move_arm.getState();
56 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
57 if(success)
58 ROS_INFO("Action finished: %s",state.toString().c_str());
59 else
60 ROS_INFO("Action failed: %s",state.toString().c_str());
61 }
62 }
63 ros::shutdown();
64 }
Now, we'll break the code down line by line:
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].
10 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.
11 move_arm.waitForServer();
This line waits 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.planner_id = std::string("");
18 goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
19 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.
- num_planning_attempts - this specifies the number of times move_arm will call the planner to try and get a successful plan
- 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.
- 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 pose constraint
21 arm_navigation_msgs::SimplePoseConstraint desired_pose;
22 desired_pose.header.frame_id = "torso_lift_link";
23 desired_pose.link_name = "r_wrist_roll_link";
24 desired_pose.pose.position.x = 0.75;
25 desired_pose.pose.position.y = -0.188;
26 desired_pose.pose.position.z = 0;
27
28 desired_pose.pose.orientation.x = 0.0;
29 desired_pose.pose.orientation.y = 0.0;
30 desired_pose.pose.orientation.z = 0.0;
31 desired_pose.pose.orientation.w = 1.0;
32
33 desired_pose.absolute_position_tolerance.x = 0.02;
34 desired_pose.absolute_position_tolerance.y = 0.02;
35 desired_pose.absolute_position_tolerance.z = 0.02;
36
37 desired_pose.absolute_roll_tolerance = 0.04;
38 desired_pose.absolute_pitch_tolerance = 0.04;
39 desired_pose.absolute_yaw_tolerance = 0.04;
40
41 arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
The aim of this tutorial is to move the arm to a pose goal. To do this, we will create a simple pose constraint for the end-effector that specifies the position and orientation where we want the arm to go and then add this constraint to the goal message for move arm.
Each simple pose constraint is specified with a header, a link name, the goal position and orientation that we want the link to reach and tolerance on the positions and orientations.
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. We will accept a final position within a cube of 0.02 m dimensions centered at the desired position. We will accept a final orientation that is within a tolerance roll, pitch, yaw of (0.04,0.04,0.04) radians around the desired orientation. Note also that we fill the header with the current time (ros::Time::now()).
Adding the pose constraint to move arm's goal
41 arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
This line adds the pose constraint that we created as a goal constraint into the MoveArmGoal message. The addGoalConstraintToMoveArmGoal function can be found in arm_navigation_msgs/utils.h and helps to convert the SimplePoseConstraint message into the more complicated form that move_arm actually uses. To see how to specify more complicated tolerance regions for the goal constraints, see the next tutorial.
46 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.
47 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.waitForResult(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
4 #include <move_arm_msgs/MoveArmAction.h>
5 #include <move_arm_msgs/utils.h>
6
7 int main(int argc, char **argv){
8 ros::init (argc, argv, "move_arm_pose_goal_test");
9 ros::NodeHandle nh;
10 actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm("move_right_arm",true);
11 move_arm.waitForServer();
12 ROS_INFO("Connected to server");
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.planner_id = std::string("");
18 goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
19 goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
20
21 motion_planning_msgs::SimplePoseConstraint desired_pose;
22 desired_pose.header.frame_id = "torso_lift_link";
23 desired_pose.link_name = "r_wrist_roll_link";
24 desired_pose.pose.position.x = 0.75;
25 desired_pose.pose.position.y = -0.188;
26 desired_pose.pose.position.z = 0;
27
28 desired_pose.pose.orientation.x = 0.0;
29 desired_pose.pose.orientation.y = 0.0;
30 desired_pose.pose.orientation.z = 0.0;
31 desired_pose.pose.orientation.w = 1.0;
32
33 desired_pose.absolute_position_tolerance.x = 0.02;
34 desired_pose.absolute_position_tolerance.y = 0.02;
35 desired_pose.absolute_position_tolerance.z = 0.02;
36
37 desired_pose.absolute_roll_tolerance = 0.04;
38 desired_pose.absolute_pitch_tolerance = 0.04;
39 desired_pose.absolute_yaw_tolerance = 0.04;
40
41 move_arm_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
42
43 if (nh.ok())
44 {
45 bool finished_within_time = false;
46 move_arm.sendGoal(goalA);
47 finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
48 if (!finished_within_time)
49 {
50 move_arm.cancelGoal();
51 ROS_INFO("Timed out achieving goal A");
52 }
53 else
54 {
55 actionlib::SimpleClientGoalState state = move_arm.getState();
56 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
57 if(success)
58 ROS_INFO("Action finished: %s",state.toString().c_str());
59 else
60 ROS_INFO("Action failed: %s",state.toString().c_str());
61 }
62 }
63 ros::shutdown();
64 }
Now, we'll break the code down line by line:
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].
10 actionlib::SimpleActionClient<move_arm_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.
11 move_arm.waitForServer();
This line waits 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.planner_id = std::string("");
18 goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
19 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.
- 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 pose constraint
21 motion_planning_msgs::SimplePoseConstraint desired_pose;
22 desired_pose.header.frame_id = "torso_lift_link";
23 desired_pose.link_name = "r_wrist_roll_link";
24 desired_pose.pose.position.x = 0.75;
25 desired_pose.pose.position.y = -0.188;
26 desired_pose.pose.position.z = 0;
27
28 desired_pose.pose.orientation.x = 0.0;
29 desired_pose.pose.orientation.y = 0.0;
30 desired_pose.pose.orientation.z = 0.0;
31 desired_pose.pose.orientation.w = 1.0;
32
33 desired_pose.absolute_position_tolerance.x = 0.02;
34 desired_pose.absolute_position_tolerance.y = 0.02;
35 desired_pose.absolute_position_tolerance.z = 0.02;
36
37 desired_pose.absolute_roll_tolerance = 0.04;
38 desired_pose.absolute_pitch_tolerance = 0.04;
39 desired_pose.absolute_yaw_tolerance = 0.04;
40
41 move_arm_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
The aim of this tutorial is to move the arm to a pose goal. To do this, we will create a simple pose constraint for the end-effector that specifies the position and orientation where we want the arm to go and then add this constraint to the goal message for move arm.
Each simple pose constraint is specified with a header, a link name, the goal position and orientation that we want the link to reach and tolerance on the positions and orientations.
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. We will accept a final position within a cube of 0.02 m dimensions centered at the desired position. We will accept a final orientation that is within a tolerance roll, pitch, yaw of (0.04,0.04,0.04) radians around the desired orientation. Note also that we fill the header with the current time (ros::Time::now()).
Adding the pose constraint to move arm's goal
41 move_arm_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
This line adds the pose constraint that we created as a goal constraint into the MoveArmGoal message. The addGoalConstraintToMoveArmGoal function can be found in move_arm_msgs/utils.h and helps to convert the SimplePoseConstraint message into the more complicated form that move_arm actually uses. To see how to specify more complicated tolerance regions for the goal constraints, see the next tutorial.
46 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.
47 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.waitForResult(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.
