| Note: This tutorial assumes that you have completed the previous tutorials: Getting started with move arm. |
 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 joint goal
Description: In this tutorial, we will use a simple action client to get the move_arm node to move the arm to a joint goal.Tutorial Level: INTERMEDIATE
Next Tutorial: Moving the arm to a pose goal move_arm/Tutorials/MoveArmPoseGoal
Versions
- This tutorial should work in Diamondback or Electric
In this tutorial, we will create a simple action client to move the arm to a joint goal in a Gazebo simulation.
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 the tutorial in simulation
To use the arm planning stack, we first need to launch the simulator. Do this by running the following:
roslaunch pr2_gazebo pr2_empty_world.launch
The simulator should come up. 
Now, in a new terminal launch the arm navigation stack:
roscd pr2_3dnav roslaunch right_arm_navigation.launch
Next, run this executable from the pr2_arm_navigation_tutorials stack:
rosrun pr2_arm_navigation_tutorials move_arm_joint_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_joint_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 goalB;
14 std::vector<std::string> names(7);
15 names[0] = "r_shoulder_pan_joint";
16 names[1] = "r_shoulder_lift_joint";
17 names[2] = "r_upper_arm_roll_joint";
18 names[3] = "r_elbow_flex_joint";
19 names[4] = "r_forearm_roll_joint";
20 names[5] = "r_wrist_flex_joint";
21 names[6] = "r_wrist_roll_joint";
22
23 goalB.motion_plan_request.group_name = "right_arm";
24 goalB.motion_plan_request.num_planning_attempts = 1;
25 goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
26
27 goalB.motion_plan_request.planner_id= std::string("");
28 goalB.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
29 goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
30
31 for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
32 {
33 goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
34 goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
35 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
36 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
37 }
38
39 goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
40 goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
41 goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.15;
42
43 if (nh.ok())
44 {
45 bool finished_within_time = false;
46 move_arm.sendGoal(goalB);
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. Note that we don't need to set the planning scene as the MoveArm action will do this automatically after we send the goal.
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 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 goalB;
14 std::vector<std::string> names(7);
15 names[0] = "r_shoulder_pan_joint";
16 names[1] = "r_shoulder_lift_joint";
17 names[2] = "r_upper_arm_roll_joint";
18 names[3] = "r_elbow_flex_joint";
19 names[4] = "r_forearm_roll_joint";
20 names[5] = "r_wrist_flex_joint";
21 names[6] = "r_wrist_roll_joint";
22
23 goalB.motion_plan_request.group_name = "right_arm";
24 goalB.motion_plan_request.num_planning_attempts = 1;
25 goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
26
27 goalB.motion_plan_request.planner_id= std::string("");
28 goalB.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
29 goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
30
31 for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
32 {
33 goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
34 goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
35 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
36 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
37 }
38
39 goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
40 goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
41 goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.15;
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 name that we are planning for. The model names are pre-defined on the parameter server using a yaml configuration files. Each group essential consists of a set of joints and links (see planning environment configuration.
- 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 are 7 joint constraints (for the seven joints).
joint_constraints : Each joint constraints is specified with a joint_name, the goal position that we want the joint to reach and a tolerance above and below this position that we are willing to accept. Thus, for the r_shoulder_pan_joint, the position we would like to get to is -2.0 radians but we are willing to accept a joint position between -2.1 and -1.9. Thus, the accepted range of tolerances is [position-tolerance_below,position+tolerance_above].
- 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.
46 move_arm.sendGoal(goalB);
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.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 goalB;
14 std::vector<std::string> names(7);
15 names[0] = "r_shoulder_pan_joint";
16 names[1] = "r_shoulder_lift_joint";
17 names[2] = "r_upper_arm_roll_joint";
18 names[3] = "r_elbow_flex_joint";
19 names[4] = "r_forearm_roll_joint";
20 names[5] = "r_wrist_flex_joint";
21 names[6] = "r_wrist_roll_joint";
22
23 goalB.motion_plan_request.group_name = "right_arm";
24 goalB.motion_plan_request.num_planning_attempts = 1;
25 goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
26
27 goalB.motion_plan_request.planner_id= std::string("");
28 goalB.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
29 goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
30
31 for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
32 {
33 goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
34 goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
35 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
36 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
37 }
38
39 goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
40 goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
41 goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.15;
42
43 if (nh.ok())
44 {
45 bool finished_within_time = false;
46 move_arm.sendGoal(goalB);
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 documentation.
8 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.
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 goalB;
14 std::vector<std::string> names(7);
15 names[0] = "r_shoulder_pan_joint";
16 names[1] = "r_shoulder_lift_joint";
17 names[2] = "r_upper_arm_roll_joint";
18 names[3] = "r_elbow_flex_joint";
19 names[4] = "r_forearm_roll_joint";
20 names[5] = "r_wrist_flex_joint";
21 names[6] = "r_wrist_roll_joint";
22
23 goalB.motion_plan_request.group_name = "right_arm";
24 goalB.motion_plan_request.num_planning_attempts = 1;
25 goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
26
27 goalB.motion_plan_request.planner_id= std::string("");
28 goalB.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
29 goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
30
31 for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
32 {
33 goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
34 goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
35 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
36 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
37 }
38
39 goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
40 goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
41 goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.15;
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 name that we are planning for. The model names are pre-defined on the parameter server using a yaml configuration files. Each group essential consists of a set of joints and links (see planning environment configuration.
- 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 are 7 joint constraints (for the seven joints).
joint_constraints : Each joint constraints is specified with a joint_name, the goal position that we want the joint to reach and a tolerance above and below this position that we are willing to accept. Thus, for the r_shoulder_pan_joint, the position we would like to get to is -2.0 radians but we are willing to accept a joint position between -2.1 and -1.9. Thus, the accepted range of tolerances is [position-tolerance_below,position+tolerance_above].
- 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.
46 move_arm.sendGoal(goalB);
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.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.
