| 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
Contents
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:
- generation of trajectories with smooth velocity and acceleration profiles
- obstacle avoidance or motion planning
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:
- not loaded
- loaded, but not running (stopped)
- running
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:
r_arm_controller (running) : the controller is running; you can skip to the next section of this tutorial
r_arm_controller (stopped) : the controller is loaded, but not running. To start it, use again the pr2_controller_manager :
rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller
no mention of r_arm_controller : the controller has not been loaded; this probably means that something went wrong during robot start-up. Abort this tutorial and investigate the cause for that.
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
Bring up a robot, either on the hardware or in gazebo.
Run rxconsole, to see any errors that occur.
- Run the launch file:
roslaunch launch/ik_trajectory_tutorial.launch
- Run the ik_trajectory_tutorial service:
bin/ik_trajectory_tutorial
- Run the test program:
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]}]}"