Note: This tutorial assumes that you have completed the previous tutorials: Inverse kinematics for the PR2 arms.
(!) 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.

Collision free inverse kinematics for the PR2 arms

Description: This tutorial will show you how to use a kinematics node to get collision free inverse kinematics solutions for a desired cartesian position of the PR2 arms.

Keywords: collision inverse kinematics cartesian

Tutorial Level: INTERMEDIATE

Contents

  1. ROS Setup
  2. Running in simulation
  3. The code

In this tutorial, you will learn how to use the PR2 kinematics node to compute collision free inverse kinematics for the tip link of the PR2 arm.

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 get_collision_free_ik

If it succeeds, you should see the following output 

[ INFO] 0.000000000: Joint: r_shoulder_pan_joint -0.330230
[ INFO] 0.000000000: Joint: r_shoulder_lift_joint 0.008300
[ INFO] 0.000000000: Joint: r_upper_arm_roll_joint -1.550000
[ INFO] 0.000000000: Joint: r_elbow_flex_joint -0.859908
[ INFO] 0.000000000: Joint: r_forearm_roll_joint 3.139403
[ INFO] 0.000000000: Joint: r_wrist_flex_joint -0.529580
[ INFO] 0.000000000: Joint: r_wrist_roll_joint -1.591270

The code

The code for this example can be found in the package pr2_arm_navigation_tutorials in the file src/get_collision_free_ik.cpp.

The setup and code for this node are almost the same as the setup and code for using the base inverse kinematics solution. The key differences are the following:

  • Service call type: Instead of the GetPositionIK service from the kinematics_msgs package, we will use the GetConstraintAwarePositionIK service.

  • Service call name: The service call name is now get_collision_free_ik

  • Nodes: An additional set of nodes will have to be launched to setup the collision checking.

  • Kinematics solver: The IK solver will now need to be launched from a different package (pr2_arm_kinematics_constraint_aware).

Select one of the buttons below to show the code and explanation for your distribution:

This code is for the Electric distribution.

The code

   1 #include <ros/ros.h>
   2 #include <kinematics_msgs/GetKinematicSolverInfo.h>
   3 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
   4 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
   5 
   6 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
   7 
   8 int main(int argc, char **argv){
   9   ros::init (argc, argv, "get_fk");
  10   ros::NodeHandle rh;
  11 
  12   ros::service::waitForService("pr2_right_arm_kinematics/get_ik_solver_info");
  13   ros::service::waitForService("pr2_right_arm_kinematics/get_constraint_aware_ik");
  14 
  15   ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("pr2_right_arm_kinematics/get_ik_solver_info");
  16   ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK>("pr2_right_arm_kinematics/get_constraint_aware_ik");
  17   ros::ServiceClient set_planning_scene_diff_client = rh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);
  18 
  19   arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
  20   arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
  21 
  22   if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res)) {
  23     ROS_WARN("Can't get planning scene");
  24     return -1;
  25   }
  26 
  27   // define the service messages
  28   kinematics_msgs::GetKinematicSolverInfo::Request request;
  29   kinematics_msgs::GetKinematicSolverInfo::Response response;
  30 
  31   if(query_client.call(request,response))
  32   {
  33     for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
  34     {
  35       ROS_DEBUG("Joint: %d %s",i,response.kinematic_solver_info.joint_names[i].c_str());
  36     }
  37   }
  38   else
  39   {
  40     ROS_ERROR("Could not call query service");
  41     ros::shutdown();
  42     exit(-1);
  43   }
  44 
  45   // define the service messages
  46   kinematics_msgs::GetConstraintAwarePositionIK::Request  gpik_req;
  47   kinematics_msgs::GetConstraintAwarePositionIK::Response gpik_res;
  48 
  49   gpik_req.timeout = ros::Duration(5.0);
  50   gpik_req.ik_request.ik_link_name = "r_wrist_roll_link";
  51 
  52   gpik_req.ik_request.pose_stamped.header.frame_id = "torso_lift_link";
  53   gpik_req.ik_request.pose_stamped.pose.position.x = 0.75;
  54   gpik_req.ik_request.pose_stamped.pose.position.y = -0.188;
  55   gpik_req.ik_request.pose_stamped.pose.position.z = 0.0;
  56 
  57   gpik_req.ik_request.pose_stamped.pose.orientation.x = 0.0;
  58   gpik_req.ik_request.pose_stamped.pose.orientation.y = 0.0;
  59   gpik_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
  60   gpik_req.ik_request.pose_stamped.pose.orientation.w = 1.0;
  61   gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_nam
  62 es.size());
  63   gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;
  64 
  65   for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
  66   {    gpik_req.ik_request.ik_seed_state.joint_state.position[i] = (response.kinematic_solver_info.limits[i
  67 ].min_position + response.kinematic_solver_info.limits[i].max_position)/2.0;
  68   }
  69   if(ik_client.call(gpik_req, gpik_res))
  70   {
  71     if(gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
  72     {
  73       for(unsigned int i=0; i < gpik_res.solution.joint_state.name.size(); i ++)
  74       {        ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_st
  75 ate.position[i]);
  76       } 
  77     }
  78     else
  79     {
  80       ROS_ERROR("Inverse kinematics failed");
  81     }
  82   }
  83   else
  84   {
  85     ROS_ERROR("Inverse kinematics service call failed");
  86   }
  87   ros::shutdown();
  88 }

This code is for the Diamondback distribution. 

   1 #include <ros/ros.h>
   2 #include <kinematics_msgs/GetKinematicSolverInfo.h>
   3 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
   4 
   5 int main(int argc, char **argv){
   6   ros::init (argc, argv, "get_fk");
   7   ros::NodeHandle rh;
   8 
   9   ros::service::waitForService("pr2_right_arm_kinematics/get_ik_solver_info");
  10   ros::service::waitForService("pr2_right_arm_kinematics/get_constraint_aware_ik");
  11 
  12   ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("pr2_right_arm_kinematics/get_ik_solver_info");
  13   ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK>("pr2_right_arm_kinematics/get_constraint_aware_ik");
  14 
  15   // define the service messages
  16   kinematics_msgs::GetKinematicSolverInfo::Request request;
  17   kinematics_msgs::GetKinematicSolverInfo::Response response;
  18 
  19   if(query_client.call(request,response))
  20   {
  21     for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
  22     {
  23       ROS_DEBUG("Joint: %d %s",i,response.kinematic_solver_info.joint_names[i].c_str());
  24     }
  25   }
  26   else
  27   {
  28     ROS_ERROR("Could not call query service");
  29     ros::shutdown();
  30     exit(-1);
  31   }
  32 
  33   // define the service messages
  34   kinematics_msgs::GetConstraintAwarePositionIK::Request  gpik_req;
  35   kinematics_msgs::GetConstraintAwarePositionIK::Response gpik_res;
  36 
  37   gpik_req.timeout = ros::Duration(5.0);
  38   gpik_req.ik_request.ik_link_name = "r_wrist_roll_link";
  39 
  40   gpik_req.ik_request.pose_stamped.header.frame_id = "torso_lift_link";
  41   gpik_req.ik_request.pose_stamped.pose.position.x = 0.75;
  42   gpik_req.ik_request.pose_stamped.pose.position.y = -0.188;
  43   gpik_req.ik_request.pose_stamped.pose.position.z = 0.0;
  44 
  45   gpik_req.ik_request.pose_stamped.pose.orientation.x = 0.0;
  46   gpik_req.ik_request.pose_stamped.pose.orientation.y = 0.0;
  47   gpik_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
  48   gpik_req.ik_request.pose_stamped.pose.orientation.w = 1.0;
  49 
  50   gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
  51   gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;
  52 
  53   for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
  54   {
  55     gpik_req.ik_request.ik_seed_state.joint_state.position[i] = (response.kinematic_solver_info.limits[i].min_position + response.kinematic_solver_info.limits[i].max_position)/2.0;
  56   }
  57   if(ik_client.call(gpik_req, gpik_res))
  58   {
  59     if(gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
  60     {
  61       for(unsigned int i=0; i < gpik_res.solution.joint_state.name.size(); i ++)
  62       {
  63         ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_state.position[i]);
  64       } 
  65     }
  66     else
  67     {
  68       ROS_ERROR("Inverse kinematics failed");
  69     }
  70   }
  71   else
  72   {
  73     ROS_ERROR("Inverse kinematics service call failed");
  74   }
  75   ros::shutdown();
  76 }

Wiki: pr2_kinematics/Tutorials/Tutorial 5 (last edited 2011-11-08 03:46:12 by EGilJones)

Except where otherwise noted, the ROS wiki is licensed under the
Creative Commons Attribution 3.0