[Documentation] [TitleIndex] [WordIndex

Note: This tutorial assumes that you have completed the previous tutorials: Collision free inverse kinematics for the PR2 arms and Moving the arm using the Joint Trajectory Action.
(!) 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 through a Cartesian pose trajectory using inverse kinematics and the joint trajectory action

Description: This tutorial teaches you how to move the arm through Cartesian pose trajectories using inverse kinematics and low-level joint controllers (the joint_trajectory_action).

Keywords: moving move arm ik inverse kinematics cartesian pose gripper low-level

Tutorial Level: INTERMEDIATE

If your goal is to move the arm along a pre-specified joint trajectory, without needing inverse kinematics, please see Moving the arm using the Joint Trajectory Action.

Overview

In this tutorial we will focus on moving the arm through sequences of Cartesian poses using inverse kinematics and an action interface to the low-level Joint Trajectory Controller. Note that, at this level, we are not concerned with many aspects of arm movement, such as:

Those components can be found in a higher level package called move_arm. However, if move_arm is not available, or you want to be using a lower-level interface, this tutorial shows the way.

If you just want to move the arm through a joint angle sequence (rather than a Cartesian pose sequence) using a lower-level interface than move_arm, see Moving the arm using the Joint Trajectory Action

Before you begin, bring up a robot, either on the hardware or in gazebo.

The Joint Trajectory Controller

The arm trajectory controller is brought up on robot start-up. In general, a controller can be in one of three states:

In general, you can use the pr2_controller_manager to check which controllers are available. After you bring up the robot, use the following command:

rosrun pr2_controller_manager pr2_controller_manager list

In the list that is printed, look for a line starting with r_arm_controller . If you find:

rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller

Note that this usage of the pr2_controller_manager also applies to other controllers, such as the head trajectory controller or the gripper controller.

Package setup

We will now create a service that takes in sequences of desired Cartesian poses for the PR2 gripper, uses inverse kinematics to convert those Cartesian poses to arm joint angles, and then asks the joint_trajectory_action to execute the resulting sequence of joint angles.

The first thing we'll need to do is create a package. To do this we'll use the handy roscreate-pkg command where we want to create the package directory:

roscreate-pkg ik_trajectory_tutorial actionlib roscpp joint_trajectory_action geometry_msgs kinematics_msgs pr2_controllers_msgs tf

After this is done we'll need to roscd to the package we created, since we'll be using it as our workspace.

roscd ik_trajectory_tutorial

Creating the service message

Now we need to define a service message that we can use to input sequences of Cartesian poses, and get back some feedback as to whether the trajectory succeeded. We will use a single header to tell us what frame our poses will be in, and an array of Pose messages to specify the position and orientation of each waypoint.

Put the following into srv/ExecuteCartesianIKTrajectory.srv:

Header header
geometry_msgs/Pose[] poses
---
uint32 success

Now uncomment the line

rosbuild_gensrv()

in your CMakeLists.txt and type 'make' in the ik_trajectory_tutorial directory to generate the service message files.

Creating the service node

Put the following into src/ik_trajectory_tutorial.cpp:

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <time.h>
   4 #include <ros/ros.h>
   5 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
   6 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
   7 #include <actionlib/client/simple_action_client.h>
   8 #include <kinematics_msgs/GetPositionIK.h>
   9 #include <kinematics_msgs/GetPositionFK.h>
  10 #include <ik_trajectory_tutorial/ExecuteCartesianIKTrajectory.h>
  11 #include <vector>
  12 
  13 #define MAX_JOINT_VEL 0.5  //in radians/sec
  14 
  15 static const std::string ARM_IK_NAME = "/pr2_right_arm_kinematics/get_ik";
  16 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::
  17                                       JointTrajectoryAction> TrajClient;
  18 
  19 class IKTrajectoryExecutor{
  20 
  21 private:
  22   ros::NodeHandle node;
  23   ros::ServiceClient ik_client;
  24   ros::ServiceServer service;
  25   pr2_controllers_msgs::JointTrajectoryGoal goal;
  26   kinematics_msgs::GetPositionIK::Request  ik_request;
  27   kinematics_msgs::GetPositionIK::Response ik_response;  
  28   TrajClient *action_client;
  29 
  30 public:
  31   IKTrajectoryExecutor(){
  32 
  33     //create a client function for the IK service
  34     ik_client = node.serviceClient<kinematics_msgs::GetPositionIK>
  35       (ARM_IK_NAME, true);    
  36 
  37     //wait for the various services to be ready
  38     ROS_INFO("Waiting for services to be ready");
  39     ros::service::waitForService(ARM_IK_NAME);
  40     ROS_INFO("Services ready");
  41 
  42     //tell the joint trajectory action client that we want 
  43     //to spin a thread by default
  44     action_client = new TrajClient("r_arm_controller/joint_trajectory_action", true);
  45 
  46     //wait for the action server to come up
  47     while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0))){
  48       ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
  49     }
  50 
  51     //register a service to input desired Cartesian trajectories
  52     service = node.advertiseService("execute_cartesian_ik_trajectory", 
  53         &IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);
  54 
  55     //have to specify the order of the joints we're sending in our 
  56     //joint trajectory goal, even if they're already on the param server
  57     goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
  58     goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
  59     goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
  60     goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
  61     goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
  62     goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
  63     goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
  64 
  65   }
  66 
  67   ~IKTrajectoryExecutor(){
  68     delete action_client;
  69   }
  70 
  71 
  72   //run inverse kinematics on a PoseStamped (7-dof pose 
  73   //(position + quaternion orientation) + header specifying the 
  74   //frame of the pose)
  75   //tries to stay close to double start_angles[7]
  76   //returns the solution angles in double solution[7] 
  77   bool run_ik(geometry_msgs::PoseStamped pose, double start_angles[7], 
  78               double solution[7], std::string link_name){
  79 
  80     kinematics_msgs::GetPositionIK::Request  ik_request;
  81     kinematics_msgs::GetPositionIK::Response ik_response;  
  82  
  83     ik_request.timeout = ros::Duration(5.0);
  84 ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_shoulder_pan_joint");
  85     ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_shoulder_lift_joint");
  86     ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_upper_arm_roll_joint");
  87     ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_elbow_flex_joint");
  88     ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_forearm_roll_joint");
  89     ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_wrist_flex_joint");
  90     ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_wrist_roll_joint");
  91 
  92     ik_request.ik_request.ik_link_name = link_name; 
  93 
  94     ik_request.ik_request.pose_stamped = pose;
  95     ik_request.ik_request.ik_seed_state.joint_state.position.resize(7);
  96 
  97     for(int i=0; i<7; i++) ik_request.ik_request.ik_seed_state.joint_state.position[i] = start_angles[i];
  98 
  99     ROS_INFO("request pose: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);
 100 
 101     bool ik_service_call = ik_client.call(ik_request,ik_response);
 102     if(!ik_service_call){
 103       ROS_ERROR("IK service call failed!");  
 104       return 0;
 105     }
 106     if(ik_response.error_code.val == ik_response.error_code.SUCCESS){
 107       for(int i=0; i<7; i++){
 108         solution[i] = ik_response.solution.joint_state.position[i];
 109       }
 110       ROS_INFO("solution angles: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", 
 111              solution[0],solution[1], solution[2],solution[3],
 112              solution[4],solution[5],solution[6]);
 113       ROS_INFO("IK service call succeeded");
 114       return 1;
 115     }
 116     ROS_INFO("IK service call error code: %d", ik_response.error_code.val);
 117     return 0;
 118   }
 119 
 120 
 121   //figure out where the arm is now  
 122   void get_current_joint_angles(double current_angles[7]){
 123     int i;
 124 
 125     //get a single message from the topic 'r_arm_controller/state'
 126     pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr state_msg = 
 127       ros::topic::waitForMessage<pr2_controllers_msgs::JointTrajectoryControllerState>
 128       ("r_arm_controller/state");
 129     
 130     //extract the joint angles from it
 131     for(i=0; i<7; i++){
 132       current_angles[i] = state_msg->actual.positions[i];
 133     }
 134   }
 135 
 136 
 137   //send a desired joint trajectory to the joint trajectory action
 138   //and wait for it to finish
 139   bool execute_joint_trajectory(std::vector<double *> joint_trajectory){
 140     int i, j; 
 141     int trajectorylength = joint_trajectory.size();
 142 
 143     //get the current joint angles
 144     double current_angles[7];    
 145     get_current_joint_angles(current_angles);
 146 
 147     //fill the goal message with the desired joint trajectory
 148     goal.trajectory.points.resize(trajectorylength+1);
 149 
 150     //set the first trajectory point to the current position
 151     goal.trajectory.points[0].positions.resize(7);
 152     goal.trajectory.points[0].velocities.resize(7);
 153     for(j=0; j<7; j++){
 154       goal.trajectory.points[0].positions[j] = current_angles[j];
 155       goal.trajectory.points[0].velocities[j] = 0.0; 
 156     }
 157 
 158     //make the first trajectory point start 0.25 seconds from when we run
 159     goal.trajectory.points[0].time_from_start = ros::Duration(0.25);     
 160 
 161     //fill in the rest of the trajectory
 162     double time_from_start = 0.25;
 163     for(i=0; i<trajectorylength; i++){
 164       goal.trajectory.points[i+1].positions.resize(7);
 165       goal.trajectory.points[i+1].velocities.resize(7);
 166 
 167       //fill in the joint positions (velocities of 0 mean that the arm
 168       //will try to stop briefly at each waypoint)
 169       for(j=0; j<7; j++){
 170         goal.trajectory.points[i+1].positions[j] = joint_trajectory[i][j];
 171         goal.trajectory.points[i+1].velocities[j] = 0.0;
 172       }
 173 
 174       //compute a desired time for this trajectory point using a max 
 175       //joint velocity
 176       double max_joint_move = 0;
 177       for(j=0; j<7; j++){
 178         double joint_move = fabs(goal.trajectory.points[i+1].positions[j] 
 179                                  - goal.trajectory.points[i].positions[j]);
 180         if(joint_move > max_joint_move) max_joint_move = joint_move;
 181       }
 182       double seconds = max_joint_move/MAX_JOINT_VEL;
 183       ROS_INFO("max_joint_move: %0.3f, seconds: %0.3f", max_joint_move, seconds);
 184       time_from_start += seconds;
 185       goal.trajectory.points[i+1].time_from_start = 
 186         ros::Duration(time_from_start);
 187     }
 188 
 189     //when to start the trajectory
 190     goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.25);
 191 
 192     ROS_INFO("Sending goal to joint_trajectory_action");
 193     action_client->sendGoal(goal);
 194 
 195     action_client->waitForResult();
 196 
 197     //get the current joint angles for debugging
 198     get_current_joint_angles(current_angles);
 199     ROS_INFO("joint angles after trajectory: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f\n",current_angles[0],current_angles[1],current_angles[2],current_angles[3],current_angles[4],current_angles[5],current_angles[6]);
 200 
 201     if(action_client->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
 202       ROS_INFO("Hooray, the arm finished the trajectory!");
 203       return 1;
 204     }
 205     ROS_INFO("The arm failed to execute the trajectory.");
 206     return 0;
 207   }
 208 
 209 
 210   //service function for execute_cartesian_ik_trajectory
 211   bool execute_cartesian_ik_trajectory(
 212          ik_trajectory_tutorial::ExecuteCartesianIKTrajectory::Request &req,
 213          ik_trajectory_tutorial::ExecuteCartesianIKTrajectory::Response &res){
 214 
 215     int trajectory_length = req.poses.size();
 216     int i, j;
 217     
 218     //IK takes in Cartesian poses stamped with the frame they belong to
 219     geometry_msgs::PoseStamped stamped_pose;
 220     stamped_pose.header = req.header;
 221     stamped_pose.header.stamp = ros::Time::now();
 222     bool success;
 223     std::vector<double *> joint_trajectory;
 224 
 225     //get the current joint angles (to find ik solutions close to)
 226     double last_angles[7];    
 227     get_current_joint_angles(last_angles);
 228 
 229     //find IK solutions for each point along the trajectory 
 230     //and stick them into joint_trajectory
 231     for(i=0; i<trajectory_length; i++){
 232       
 233       stamped_pose.pose = req.poses[i];
 234       double *trajectory_point = new double[7];
 235       success = run_ik(stamped_pose, last_angles, trajectory_point, "r_wrist_roll_link");
 236       joint_trajectory.push_back(trajectory_point);
 237 
 238       if(!success){
 239         ROS_ERROR("IK solution not found for trajectory point number %d!\n", i);
 240         return 0;
 241       }
 242       for(j=0; j<7; j++) last_angles[j] = trajectory_point[j];
 243     }        
 244 
 245     //run the resulting joint trajectory
 246     ROS_INFO("executing joint trajectory");
 247     success = execute_joint_trajectory(joint_trajectory);
 248     res.success = success;
 249     
 250     return success;
 251   }
 252 
 253 };
 254 
 255 
 256 int main(int argc, char** argv){
 257 
 258   //init ROS node
 259   ros::init(argc, argv, "cartesian_ik_trajectory_executor");
 260 
 261   IKTrajectoryExecutor ik_traj_exec = IKTrajectoryExecutor();
 262 
 263   ROS_INFO("Waiting for cartesian trajectories to execute");
 264   ros::spin();
 265   
 266   return 0;
 267 }

Now we'll go through subsets of the code more carefully.

  23   ros::ServiceClient ik_client;
  24   ros::ServiceServer service;
  25   pr2_controllers_msgs::JointTrajectoryGoal goal;
  26   kinematics_msgs::GetPositionIK::Request  ik_request;
  27   kinematics_msgs::GetPositionIK::Response ik_response;  
  28   TrajClient *action_client;
  29 
  30 public:
  31   IKTrajectoryExecutor(){
  32 
  33     //create a client function for the IK service
  34     ik_client = node.serviceClient<kinematics_msgs::GetPositionIK>
  35       (ARM_IK_NAME, true);    
  36 
  37     //wait for the various services to be ready
  38     ROS_INFO("Waiting for services to be ready");
  39     ros::service::waitForService(ARM_IK_NAME);
  40     ROS_INFO("Services ready");
  41 
  42     //tell the joint trajectory action client that we want 
  43     //to spin a thread by default
  44     action_client = new TrajClient("r_arm_controller/joint_trajectory_action", true);
  45 
  46     //wait for the action server to come up
  47     while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0))){
  48       ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
  49     }
  50 
  51     //register a service to input desired Cartesian trajectories
  52     service = node.advertiseService("execute_cartesian_ik_trajectory", 
  53         &IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);

Here we are creating clients for the services and actions we'll be using: pr2_arm_navigation_kinematics's /pr2_right_arm_kinematics/get_ik for inverse kinematics, and the joint_trajectory_action's JointTrajectoryAction to execute joint trajectories. We also advertise our own service to take in desired Cartesian pose sequences, execute_cartesian_ik_trajectory.

  72   //run inverse kinematics on a PoseStamped (7-dof pose 
  73   //(position + quaternion orientation) + header specifying the 
  74   //frame of the pose)
  75   //tries to stay close to double start_angles[7]
  76   //returns the solution angles in double solution[7] 
  77   bool run_ik(geometry_msgs::PoseStamped pose, double start_angles[7], 
  78               double solution[7], std::string link_name){
  79 
  80     kinematics_msgs::GetPositionIK::Request  ik_request;
  81     kinematics_msgs::GetPositionIK::Response ik_response;  
  82  
  83     ik_request.timeout = ros::Duration(5.0);
  84 ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_shoulder_pan_joint");
  85     ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_shoulder_lift_joint");
  86     ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_upper_arm_roll_joint");
  87     ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_elbow_flex_joint");
  88     ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_forearm_roll_joint");
  89     ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_wrist_flex_joint");
  90     ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_wrist_roll_joint");
  91 
  92     ik_request.ik_request.ik_link_name = link_name; 
  93 
  94     ik_request.ik_request.pose_stamped = pose;
  95     ik_request.ik_request.ik_seed_state.joint_state.position.resize(7);
  96 
  97     for(int i=0; i<7; i++) ik_request.ik_request.ik_seed_state.joint_state.position[i] = start_angles[i];
  98 
  99     ROS_INFO("request pose: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);
 100 
 101     bool ik_service_call = ik_client.call(ik_request,ik_response);
 102     if(!ik_service_call){
 103       ROS_ERROR("IK service call failed!");  
 104       return 0;
 105     }
 106     if(ik_response.error_code.val == ik_response.error_code.SUCCESS){
 107       for(int i=0; i<7; i++){
 108         solution[i] = ik_response.solution.joint_state.position[i];
 109       }
 110       ROS_INFO("solution angles: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", 
 111              solution[0],solution[1], solution[2],solution[3],
 112              solution[4],solution[5],solution[6]);
 113       ROS_INFO("IK service call succeeded");
 114       return 1;
 115     }
 116     ROS_INFO("IK service call error code: %d", ik_response.error_code.val);
 117     return 0;
 118   }

A function to perform inverse kinematics, much as used/described in the Collision free inverse kinematics for the PR2 armstutorial.

 121   //figure out where the arm is now  
 122   void get_current_joint_angles(double current_angles[7]){
 123     int i;
 124 
 125     //get a single message from the topic 'r_arm_controller/state'
 126     pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr state_msg = 
 127       ros::topic::waitForMessage<pr2_controllers_msgs::JointTrajectoryControllerState>
 128       ("r_arm_controller/state");
 129     
 130     //extract the joint angles from it
 131     for(i=0; i<7; i++){
 132       current_angles[i] = state_msg->actual.positions[i];
 133     }
 134   }

This function gets the current right arm angles from the state being broadcast by the r_arm_controller, by listening for a single message.

 137   //send a desired joint trajectory to the joint trajectory action
 138   //and wait for it to finish
 139   bool execute_joint_trajectory(std::vector<double *> joint_trajectory){
 140     int i, j; 
 141     int trajectorylength = joint_trajectory.size();
 142 
 143     //get the current joint angles
 144     double current_angles[7];    
 145     get_current_joint_angles(current_angles);
 146 
 147     //fill the goal message with the desired joint trajectory
 148     goal.trajectory.points.resize(trajectorylength+1);
 149 
 150     //set the first trajectory point to the current position
 151     goal.trajectory.points[0].positions.resize(7);
 152     goal.trajectory.points[0].velocities.resize(7);
 153     for(j=0; j<7; j++){
 154       goal.trajectory.points[0].positions[j] = current_angles[j];
 155       goal.trajectory.points[0].velocities[j] = 0.0; 
 156     }
 157 
 158     //make the first trajectory point start 0.25 seconds from when we run
 159     goal.trajectory.points[0].time_from_start = ros::Duration(0.25);     
 160 
 161     //fill in the rest of the trajectory
 162     double time_from_start = 0.25;
 163     for(i=0; i<trajectorylength; i++){
 164       goal.trajectory.points[i+1].positions.resize(7);
 165       goal.trajectory.points[i+1].velocities.resize(7);
 166 
 167       //fill in the joint positions (velocities of 0 mean that the arm
 168       //will try to stop briefly at each waypoint)
 169       for(j=0; j<7; j++){
 170         goal.trajectory.points[i+1].positions[j] = joint_trajectory[i][j];
 171         goal.trajectory.points[i+1].velocities[j] = 0.0;
 172       }
 173 
 174       //compute a desired time for this trajectory point using a max 
 175       //joint velocity
 176       double max_joint_move = 0;
 177       for(j=0; j<7; j++){
 178         double joint_move = fabs(goal.trajectory.points[i+1].positions[j] 
 179                                  - goal.trajectory.points[i].positions[j]);
 180         if(joint_move > max_joint_move) max_joint_move = joint_move;
 181       }
 182       double seconds = max_joint_move/MAX_JOINT_VEL;
 183       ROS_INFO("max_joint_move: %0.3f, seconds: %0.3f", max_joint_move, seconds);
 184       time_from_start += seconds;
 185       goal.trajectory.points[i+1].time_from_start = 
 186         ros::Duration(time_from_start);
 187     }
 188 
 189     //when to start the trajectory
 190     goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.25);
 191 
 192     ROS_INFO("Sending goal to joint_trajectory_action");
 193     action_client->sendGoal(goal);
 194 
 195     action_client->waitForResult();
 196 
 197     //get the current joint angles for debugging
 198     get_current_joint_angles(current_angles);
 199     ROS_INFO("joint angles after trajectory: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f\n",current_angles[0],current_angles[1],current_angles[2],current_angles[3],current_angles[4],current_angles[5],current_angles[6]);
 200 
 201     if(action_client->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
 202       ROS_INFO("Hooray, the arm finished the trajectory!");
 203       return 1;
 204     }
 205     ROS_INFO("The arm failed to execute the trajectory.");
 206     return 0;
 207   }

Because the joint_trajectory_action assumes that we will input not only our desired Cartesian poses but also the time at which we would like the arm to be at each waypoint, we need to compute reasonable times for each waypoint. In this code we do so by enforcing a maximum joint speed. For waypoints after the first, we can compute the maximum change in joint angles (after IK is run) on the two segment endpoints. However, because the first waypoint we ask for could be arbitrarily far away from our current arm position, we need to check where the arm is now to decide how long is a reasonable time to use to get there.

 210   //service function for execute_cartesian_ik_trajectory
 211   bool execute_cartesian_ik_trajectory(
 212          ik_trajectory_tutorial::ExecuteCartesianIKTrajectory::Request &req,
 213          ik_trajectory_tutorial::ExecuteCartesianIKTrajectory::Response &res){
 214 
 215     int trajectory_length = req.poses.size();
 216     int i, j;
 217     
 218     //IK takes in Cartesian poses stamped with the frame they belong to
 219     geometry_msgs::PoseStamped stamped_pose;
 220     stamped_pose.header = req.header;
 221     stamped_pose.header.stamp = ros::Time::now();
 222     bool success;
 223     std::vector<double *> joint_trajectory;
 224 
 225     //get the current joint angles (to find ik solutions close to)
 226     double last_angles[7];    
 227     get_current_joint_angles(last_angles);
 228 
 229     //find IK solutions for each point along the trajectory 
 230     //and stick them into joint_trajectory
 231     for(i=0; i<trajectory_length; i++){
 232       
 233       stamped_pose.pose = req.poses[i];
 234       double *trajectory_point = new double[7];
 235       success = run_ik(stamped_pose, last_angles, trajectory_point, "r_wrist_roll_link");
 236       joint_trajectory.push_back(trajectory_point);
 237 
 238       if(!success){
 239         ROS_ERROR("IK solution not found for trajectory point number %d!\n", i);
 240         return 0;
 241       }
 242       for(j=0; j<7; j++) last_angles[j] = trajectory_point[j];
 243     }        
 244 
 245     //run the resulting joint trajectory
 246     ROS_INFO("executing joint trajectory");
 247     success = execute_joint_trajectory(joint_trajectory);
 248     res.success = success;
 249     
 250     return success;
 251   }

Our service function. Takes in a sequence of desired Cartesian poses in the reference frame of the header's frame_id, finds joint angles that bring the arm there using IK, and then feeds the resulting joint angle sequence to the joint_trajectory_action.

Building

Add the following line to the CMakeLists.txt:

rosbuild_add_executable(ik_trajectory_tutorial src/ik_trajectory_tutorial.cpp)

and make the binary by typing 'make' in the ik_tutorial directory.

Test program

Because our ik_trajectory_tutorial program runs a service that waits for service calls to come in, we need to write a client program to send it Cartesian trajectories.

Put the following in scripts/test_ik_trajectory_tutorial.py:

   1 #test client for ik_trajectory_tutorial
   2 
   3 import roslib
   4 roslib.load_manifest('ik_trajectory_tutorial')
   5 import rospy
   6 from ik_trajectory_tutorial.srv import ExecuteCartesianIKTrajectory
   7 from geometry_msgs.msg import Pose
   8 import time
   9 import sys
  10 import pdb
  11 import tf
  12 
  13 #execute a Cartesian trajectory defined by a root frame, a list of 
  14 #positions (x,y,z), and a list of orientations (quaternions: x,y,z,w)
  15 def call_execute_cartesian_ik_trajectory(frame, positions, orientations):
  16     rospy.wait_for_service("execute_cartesian_ik_trajectory")
  17 
  18     #fill in the header (don't need seq)
  19     header = rospy.Header()
  20     header.frame_id = frame
  21     header.stamp = rospy.get_rostime()
  22 
  23     #fill in the poses
  24     poses = []
  25     for (position, orientation) in zip(positions, orientations):
  26         pose = Pose()
  27         pose.position.x = position[0]
  28         pose.position.y = position[1]
  29         pose.position.z = position[2]
  30         pose.orientation.x = orientation[0]
  31         pose.orientation.y = orientation[1]
  32         pose.orientation.z = orientation[2]
  33         pose.orientation.w = orientation[3]
  34         poses.append(pose)
  35 
  36     #call the service to execute the trajectory
  37     print "calling execute_cartesian_ik_trajectory"
  38     try:
  39         s = rospy.ServiceProxy("execute_cartesian_ik_trajectory", \
  40                                    ExecuteCartesianIKTrajectory)
  41         resp = s(header, poses)
  42     except rospy.ServiceException, e:
  43         print "error when calling execute_cartesian_ik_trajectory: %s"%e
  44         return 0
  45     return resp.success
  46 
  47 #pretty-print list to string
  48 def pplist(list):
  49     return ' '.join(['%2.3f'%x for x in list])
  50 
  51 #print out the positions, velocities, and efforts of the right arm joints
  52 if __name__ == "__main__":
  53     rospy.init_node("test_cartesian_ik_trajectory_executer")
  54     tf_listener = tf.TransformListener()
  55     time.sleep(.5) #give the transform listener time to get some frames
  56 
  57     # not needed, fix tutorial
  58     joint_names = ["r_shoulder_pan_joint",
  59                    "r_shoulder_lift_joint",
  60                    "r_upper_arm_roll_joint",
  61                    "r_elbow_flex_joint",
  62                    "r_forearm_roll_joint",
  63                    "r_wrist_flex_joint",
  64                    "r_wrist_roll_joint"]
  65 
  66     positions = [[.76, -.19, .83], [0.59, -0.36, 0.93]]
  67     orientations = [[.02, -.09, 0.0, 1.0], [0.65, -0.21, .38, .62]]
  68 
  69     success = call_execute_cartesian_ik_trajectory("/base_link", \
  70             positions, orientations)
  71 
  72     #check the final pose
  73     (trans, rot) = tf_listener.lookupTransform('base_link', 'r_wrist_roll_link', rospy.Time(0))
  74     print "end Cartesian pose: trans", pplist(trans), "rot", pplist(rot)
  75 
  76     if success:
  77         print "trajectory succeeded!"
  78     else:
  79         print "trajectory failed."

Launching

Before we can run, we need to launch a couple things: the IK services and the joint_states_listener.

Put the following into launch/ik_trajectory_tutorial.launch:

<launch>
  <!-- load ik -->
  <include file="$(find pr2_arm_navigation_kinematics)/launch/pr2_ik_rarm_node.launch"/>
</launch>

Running

roslaunch launch/ik_trajectory_tutorial.launch

bin/ik_trajectory_tutorial

python scripts/test_ik_trajectory_tutorial.py

If the test succeeds, you should see "Hooray, the arm finished the trajectory!" and the end pose should be close to the desired Cartesian position for our second waypoint.

Instead of writing the Python test client test_ik_trajectory_tutorial.py, you may as well use the command line for the same effect:

rosservice call /execute_cartesian_ik_trajectory -- "{header: { frame_id: /base_link}, poses: [{position: [0.76, -0.19, 0.83], orientation:[0.02, -0.09, 0.0, 1.0]}, {position: [0.59, -0.36, 0.93], orientation: [0.65, -0.21, .38, .62]}]}"


2019-08-17 13:04