## page was renamed from move_arm/Tutorials/Moving the arm to a pose goal ## For instruction on writing tutorials ## http://www.ros.org/wiki/WritingTutorials #################################### ##FILL ME IN #################################### ## for a custom note with links: ## note = ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0= [[move_arm/Tutorials/MoveArmJointGoal|Moving the arm to a joint goal]] ## descriptive title for the tutorial ## title = Moving the arm to a pose goal ## multi-line description to be displayed in search ## 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. ## the next tutorial description (optional) ## next = ## links to next tutorial (optional) ## next.0.link= [[move_arm/Tutorials/MoveArmPoseGoalComplex| Specifying complex pose goals]] ## next.1.link= ## what level user is this tutorial for ## level= IntermediateCategory ## keywords = #################################### <> <> == 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. {{attachment:gazebo_move_arm_snapshot.png}} 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 {{attachment:gazebo_pose_goal.png}} == 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: <> {{{{#!wiki seesaw electric This code is for the Electric distribution. {{{ #!cplusplus block=simple_arm_goals_pose_electric #include #include #include #include int main(int argc, char **argv){ ros::init (argc, argv, "move_arm_pose_goal_test"); ros::NodeHandle nh; actionlib::SimpleActionClient move_arm("move_right_arm",true); move_arm.waitForServer(); ROS_INFO("Connected to server"); arm_navigation_msgs::MoveArmGoal goalA; goalA.motion_plan_request.group_name = "right_arm"; goalA.motion_plan_request.num_planning_attempts = 1; goalA.motion_plan_request.planner_id = std::string(""); goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path"); goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0); arm_navigation_msgs::SimplePoseConstraint desired_pose; desired_pose.header.frame_id = "torso_lift_link"; desired_pose.link_name = "r_wrist_roll_link"; desired_pose.pose.position.x = 0.75; desired_pose.pose.position.y = -0.188; desired_pose.pose.position.z = 0; desired_pose.pose.orientation.x = 0.0; desired_pose.pose.orientation.y = 0.0; desired_pose.pose.orientation.z = 0.0; desired_pose.pose.orientation.w = 1.0; desired_pose.absolute_position_tolerance.x = 0.02; desired_pose.absolute_position_tolerance.y = 0.02; desired_pose.absolute_position_tolerance.z = 0.02; desired_pose.absolute_roll_tolerance = 0.04; desired_pose.absolute_pitch_tolerance = 0.04; desired_pose.absolute_yaw_tolerance = 0.04; arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA); if (nh.ok()) { bool finished_within_time = false; move_arm.sendGoal(goalA); finished_within_time = move_arm.waitForResult(ros::Duration(200.0)); if (!finished_within_time) { move_arm.cancelGoal(); ROS_INFO("Timed out achieving goal A"); } else { actionlib::SimpleClientGoalState state = move_arm.getState(); bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED); if(success) ROS_INFO("Action finished: %s",state.toString().c_str()); else ROS_INFO("Action failed: %s",state.toString().c_str()); } } ros::shutdown(); } }}} 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]. <> 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. <> This line waits for the action server to report that it has come up and is ready to begin processing goals. <> 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 === <> 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 === <> 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. <> The call to `move_arm.sendGoal` will actually push the goal out over the wire to the move_arm node for processing. <> 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. }}}} <> {{{{#!wiki seesaw diamondback This code is for the Diamondback distribution. {{{ #!cplusplus block=simple_arm_goals_pose_diamondback #include #include #include #include int main(int argc, char **argv){ ros::init (argc, argv, "move_arm_pose_goal_test"); ros::NodeHandle nh; actionlib::SimpleActionClient move_arm("move_right_arm",true); move_arm.waitForServer(); ROS_INFO("Connected to server"); move_arm_msgs::MoveArmGoal goalA; goalA.motion_plan_request.group_name = "right_arm"; goalA.motion_plan_request.num_planning_attempts = 1; goalA.motion_plan_request.planner_id = std::string(""); goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path"); goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0); motion_planning_msgs::SimplePoseConstraint desired_pose; desired_pose.header.frame_id = "torso_lift_link"; desired_pose.link_name = "r_wrist_roll_link"; desired_pose.pose.position.x = 0.75; desired_pose.pose.position.y = -0.188; desired_pose.pose.position.z = 0; desired_pose.pose.orientation.x = 0.0; desired_pose.pose.orientation.y = 0.0; desired_pose.pose.orientation.z = 0.0; desired_pose.pose.orientation.w = 1.0; desired_pose.absolute_position_tolerance.x = 0.02; desired_pose.absolute_position_tolerance.y = 0.02; desired_pose.absolute_position_tolerance.z = 0.02; desired_pose.absolute_roll_tolerance = 0.04; desired_pose.absolute_pitch_tolerance = 0.04; desired_pose.absolute_yaw_tolerance = 0.04; move_arm_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA); if (nh.ok()) { bool finished_within_time = false; move_arm.sendGoal(goalA); finished_within_time = move_arm.waitForResult(ros::Duration(200.0)); if (!finished_within_time) { move_arm.cancelGoal(); ROS_INFO("Timed out achieving goal A"); } else { actionlib::SimpleClientGoalState state = move_arm.getState(); bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED); if(success) ROS_INFO("Action finished: %s",state.toString().c_str()); else ROS_INFO("Action failed: %s",state.toString().c_str()); } } ros::shutdown(); } }}} 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]. <> 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. <> This line waits for the action server to report that it has come up and is ready to begin processing goals. <> 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 === <> 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 === <> 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. <> The call to `move_arm.sendGoal` will actually push the goal out over the wire to the move_arm node for processing. <> 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. }}}} ## AUTOGENERATED DO NOT DELETE ## TutorialCategory ## FILL IN THE STACK TUTORIAL CATEGORY HERE