pr2_arm_ik.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Sachin Chitta */
36 
37 #include <angles/angles.h>
38 #include <moveit/pr2_arm_kinematics/pr2_arm_ik.h>
39 
40 /**** List of angles (for reference) *******
41  th1 = shoulder/turret pan
42  th2 = shoulder/turret lift/pitch
43  th3 = shoulder/turret roll
44  th4 = elbow pitch
45  th5 = elbow roll
46  th6 = wrist pitch
47  th7 = wrist roll
48 *****/
49 using namespace angles;
50 using namespace pr2_arm_kinematics;
51 
52 PR2ArmIK::PR2ArmIK()
53 {
54 }
55 
56 bool PR2ArmIK::init(const urdf::Model &robot_model, const std::string &root_name, const std::string &tip_name)
57 {
58  std::vector<urdf::Pose> link_offset;
59  int num_joints = 0;
60  urdf::LinkConstSharedPtr link = robot_model.getLink(tip_name);
61  while(link && num_joints < 7)
62  {
63  urdf::JointConstSharedPtr joint;
64  if (link->parent_joint)
65  joint = robot_model.getJoint(link->parent_joint->name);
66  if(!joint)
67  {
68  if (link->parent_joint)
69  ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
70  else
71  ROS_ERROR("Link %s has no parent joint",link->name.c_str());
72  return false;
73  }
74  if(joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
75  {
76  link_offset.push_back(link->parent_joint->parent_to_joint_origin_transform);
77  angle_multipliers_.push_back(joint->axis.x*fabs(joint->axis.x) + joint->axis.y*fabs(joint->axis.y) + joint->axis.z*fabs(joint->axis.z));
78  ROS_DEBUG("Joint axis: %d, %f, %f, %f",6-num_joints,joint->axis.x,joint->axis.y,joint->axis.z);
79  if(joint->type != urdf::Joint::CONTINUOUS)
80  {
81  if (joint->safety)
82  {
83  min_angles_.push_back(joint->safety->soft_lower_limit);
84  max_angles_.push_back(joint->safety->soft_upper_limit);
85  }
86  else
87  {
88  if (joint->limits)
89  {
90  min_angles_.push_back(joint->limits->lower);
91  max_angles_.push_back(joint->limits->upper);
92  }
93  else
94  {
95  min_angles_.push_back(0.0);
96  max_angles_.push_back(0.0);
97  ROS_WARN("No joint limits or joint '%s'",joint->name.c_str());
98  }
99  }
100  continuous_joint_.push_back(false);
101  }
102  else
103  {
104  min_angles_.push_back(-M_PI);
105  max_angles_.push_back(M_PI);
106  continuous_joint_.push_back(true);
107  }
108  addJointToChainInfo(link->parent_joint,solver_info_);
109  num_joints++;
110  }
111  link = robot_model.getLink(link->getParent()->name);
112  }
113 
114  solver_info_.link_names.push_back(tip_name);
115 
116  // solver_info_.link_names.push_back(tip_name);
117  // We expect order from root to tip, so reverse the order
118  std::reverse(angle_multipliers_.begin(),angle_multipliers_.end());
119  std::reverse(min_angles_.begin(),min_angles_.end());
120  std::reverse(max_angles_.begin(),max_angles_.end());
121  std::reverse(link_offset.begin(),link_offset.end());
122  std::reverse(solver_info_.limits.begin(),solver_info_.limits.end());
123  std::reverse(solver_info_.joint_names.begin(),solver_info_.joint_names.end());
124  std::reverse(solver_info_.link_names.begin(),solver_info_.link_names.end());
125  std::reverse(continuous_joint_.begin(),continuous_joint_.end());
126 
127  if(num_joints != 7)
128  {
129  ROS_FATAL("PR2ArmIK:: Chain from %s to %s does not have 7 joints",root_name.c_str(),tip_name.c_str());
130  return false;
131  }
132 
133  torso_shoulder_offset_x_ = link_offset[0].position.x;
134  torso_shoulder_offset_y_ = link_offset[0].position.y;
135  torso_shoulder_offset_z_ = link_offset[0].position.z;
136  shoulder_upperarm_offset_ = distance(link_offset[1]);
137  upperarm_elbow_offset_ = distance(link_offset[3]);
138  elbow_wrist_offset_ = distance(link_offset[5]);
139  shoulder_elbow_offset_ = shoulder_upperarm_offset_ + upperarm_elbow_offset_;
140  shoulder_wrist_offset_ = shoulder_upperarm_offset_+upperarm_elbow_offset_+elbow_wrist_offset_;
141 
142  Eigen::Matrix4f home = Eigen::Matrix4f::Identity();
143  home(0,3) = shoulder_upperarm_offset_ + upperarm_elbow_offset_ + elbow_wrist_offset_;
144  home_inv_ = home.inverse();
145  grhs_ = home;
146  gf_ = home_inv_;
147  solution_.resize( NUM_JOINTS_ARM7DOF);
148  return true;
149 }
150 
151 void PR2ArmIK::addJointToChainInfo(urdf::JointConstSharedPtr joint, moveit_msgs::KinematicSolverInfo &info)
152 {
153  moveit_msgs::JointLimits limit;
154  info.joint_names.push_back(joint->name);//Joints are coming in reverse order
155 
156  if(joint->type != urdf::Joint::CONTINUOUS)
157  {
158  if (joint->safety)
159  {
160  limit.min_position = joint->safety->soft_lower_limit;
161  limit.max_position = joint->safety->soft_upper_limit;
162  limit.has_position_limits = true;
163  }
164  else
165  if (joint->limits)
166  {
167  limit.min_position = joint->limits->lower;
168  limit.max_position = joint->limits->upper;
169  limit.has_position_limits = true;
170  }
171  else
172  limit.has_position_limits = false;
173  }
174  else
175  {
176  limit.min_position = -M_PI;
177  limit.max_position = M_PI;
178  limit.has_position_limits = false;
179  }
180  if (joint->limits)
181  {
182  limit.max_velocity = joint->limits->velocity;
183  limit.has_velocity_limits = 1;
184  }
185  else
186  limit.has_velocity_limits = 0;
187  info.limits.push_back(limit);
188 }
189 
190 void PR2ArmIK::getSolverInfo(moveit_msgs::KinematicSolverInfo &info)
191 {
192  info = solver_info_;
193 }
194 
195 
196 void PR2ArmIK::computeIKShoulderPan(const Eigen::Matrix4f &g_in, const double &t1_in, std::vector<std::vector<double> > &solution) const
197 {
198 //t1 = shoulder/turret pan is specified
199 // solution_ik_.resize(0);
200  std::vector<double> solution_ik(NUM_JOINTS_ARM7DOF,0.0);
201  Eigen::Matrix4f g = g_in;
202  Eigen::Matrix4f gf_local = home_inv_;
203  Eigen::Matrix4f grhs_local = home_inv_;
204 //First bring everything into the arm frame
205  g(0,3) = g_in(0,3) - torso_shoulder_offset_x_;
206  g(1,3) = g_in(1,3) - torso_shoulder_offset_y_;
207  g(2,3) = g_in(2,3) - torso_shoulder_offset_z_;
208 
209  double t1 = angles::normalize_angle(t1_in);
210  if(!checkJointLimits(t1,0))
211  return;
212 
213 
214  double cost1, cost2, cost3, cost4;
215  double sint1, sint2, sint3, sint4;
216 
217  gf_local = g*home_inv_;
218 
219  cost1 = cos(t1);
220  sint1 = sin(t1);
221 
222  double t2(0), t3(0), t4(0), t5(0), t6(0), t7(0);
223 
224  double at(0), bt(0), ct(0);
225 
226  double theta2[2],theta3[2],theta4[2],theta5[2],theta6[4],theta7[2];
227 
228  double sopx = shoulder_upperarm_offset_*cost1;
229  double sopy = shoulder_upperarm_offset_*sint1;
230  double sopz = 0;
231 
232  double x = g(0,3);
233  double y = g(1,3);
234  double z = g(2,3);
235 
236  double dx = x - sopx;
237  double dy = y - sopy;
238  double dz = z - sopz;
239 
240  double dd = dx*dx + dy*dy + dz*dz;
241 
242  double numerator = dd-shoulder_upperarm_offset_*shoulder_upperarm_offset_+2*shoulder_upperarm_offset_*shoulder_elbow_offset_-2*shoulder_elbow_offset_*shoulder_elbow_offset_+2*shoulder_elbow_offset_*shoulder_wrist_offset_-shoulder_wrist_offset_*shoulder_wrist_offset_;
243  double denominator = 2*(shoulder_upperarm_offset_-shoulder_elbow_offset_)*(shoulder_elbow_offset_-shoulder_wrist_offset_);
244 
245  double acosTerm = numerator/denominator;
246 
247  if (acosTerm > 1.0 || acosTerm < -1.0)
248  return;
249 
250  double acos_angle = acos(acosTerm);
251 
252  theta4[0] = acos_angle;
253  theta4[1] = -acos_angle;
254 
255 #ifdef DEBUG
256  std::cout << "ComputeIK::theta3:" << numerator << "," << denominator << "," << std::endl << theta4[0] << std::endl;
257 #endif
258 
259  for(int jj =0; jj < 2; jj++)
260  {
261  t4 = theta4[jj];
262  cost4 = cos(t4);
263  sint4 = sin(t4);
264 
265 #ifdef DEBUG
266  std::cout << "t4 " << t4 << std::endl;
267 #endif
268  if(std::isnan(t4))
269  continue;
270 
271  if(!checkJointLimits(t4,3))
272  continue;
273 
274  at = x*cost1+y*sint1-shoulder_upperarm_offset_;
275  bt = -z;
276  ct = -shoulder_upperarm_offset_ + shoulder_elbow_offset_ + (shoulder_wrist_offset_-shoulder_elbow_offset_)*cos(t4);
277 
278  if(!solveCosineEqn(at,bt,ct,theta2[0],theta2[1]))
279  continue;
280 
281  for(int ii=0; ii < 2; ii++)
282  {
283  t2 = theta2[ii];
284  if(!checkJointLimits(t2,1))
285  continue;
286 
287 
288 #ifdef DEBUG
289  std::cout << "t2 " << t2 << std::endl;
290 #endif
291  sint2 = sin(t2);
292  cost2 = cos(t2);
293 
294  at = sint1*(shoulder_elbow_offset_ - shoulder_wrist_offset_)*sint2*sint4;
295  bt = (-shoulder_elbow_offset_+shoulder_wrist_offset_)*cost1*sint4;
296  ct = y - (shoulder_upperarm_offset_+cost2*(-shoulder_upperarm_offset_+shoulder_elbow_offset_+(-shoulder_elbow_offset_+shoulder_wrist_offset_)*cos(t4)))*sint1;
297  if(!solveCosineEqn(at,bt,ct,theta3[0],theta3[1]))
298  continue;
299 
300  for(int kk =0; kk < 2; kk++)
301  {
302  t3 = theta3[kk];
303 
304  if(!checkJointLimits(angles::normalize_angle(t3),2))
305  continue;
306 
307  sint3 = sin(t3);
308  cost3 = cos(t3);
309 #ifdef DEBUG
310  std::cout << "t3 " << t3 << std::endl;
311 #endif
312  if(fabs((shoulder_upperarm_offset_-shoulder_elbow_offset_+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost4)*sint2+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost2*cost3*sint4-z) > IK_EPS )
313  continue;
314 
315  if(fabs((shoulder_elbow_offset_-shoulder_wrist_offset_)*sint1*sint3*sint4+cost1*(shoulder_upperarm_offset_+cost2*(-shoulder_upperarm_offset_+shoulder_elbow_offset_+(-shoulder_elbow_offset_+shoulder_wrist_offset_)*cost4)+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost3*sint2*sint4) - x) > IK_EPS)
316  continue;
317 
318  grhs_local(0,0) = cost4*(gf_local(0,0)*cost1*cost2+gf_local(1,0)*cost2*sint1-gf_local(2,0)*sint2)-(gf_local(2,0)*cost2*cost3 + cost3*(gf_local(0,0)*cost1 + gf_local(1,0)*sint1)*sint2 + (-(gf_local(1,0)*cost1) + gf_local(0,0)*sint1)*sint3)*sint4;
319 
320  grhs_local(0,1) = cost4*(gf_local(0,1)*cost1*cost2 + gf_local(1,1)*cost2*sint1 - gf_local(2,1)*sint2) - (gf_local(2,1)*cost2*cost3 + cost3*(gf_local(0,1)*cost1 + gf_local(1,1)*sint1)*sint2 + (-(gf_local(1,1)*cost1) + gf_local(0,1)*sint1)*sint3)*sint4;
321 
322  grhs_local(0,2) = cost4*(gf_local(0,2)*cost1*cost2 + gf_local(1,2)*cost2*sint1 - gf_local(2,2)*sint2) - (gf_local(2,2)*cost2*cost3 + cost3*(gf_local(0,2)*cost1 + gf_local(1,2)*sint1)*sint2 + (-(gf_local(1,2)*cost1) + gf_local(0,2)*sint1)*sint3)*sint4;
323 
324  grhs_local(1,0) = cost3*(gf_local(1,0)*cost1 - gf_local(0,0)*sint1) + gf_local(2,0)*cost2*sint3 + (gf_local(0,0)*cost1 + gf_local(1,0)*sint1)*sint2*sint3;
325 
326  grhs_local(1,1) = cost3*(gf_local(1,1)*cost1 - gf_local(0,1)*sint1) + gf_local(2,1)*cost2*sint3 + (gf_local(0,1)*cost1 + gf_local(1,1)*sint1)*sint2*sint3;
327 
328  grhs_local(1,2) = cost3*(gf_local(1,2)*cost1 - gf_local(0,2)*sint1) + gf_local(2,2)*cost2*sint3 + (gf_local(0,2)*cost1 + gf_local(1,2)*sint1)*sint2*sint3;
329 
330  grhs_local(2,0) = cost4*(gf_local(2,0)*cost2*cost3 + cost3*(gf_local(0,0)*cost1 + gf_local(1,0)*sint1)*sint2 + (-(gf_local(1,0)*cost1) + gf_local(0,0)*sint1)*sint3) + (gf_local(0,0)*cost1*cost2 + gf_local(1,0)*cost2*sint1 - gf_local(2,0)*sint2)*sint4;
331 
332  grhs_local(2,1) = cost4*(gf_local(2,1)*cost2*cost3 + cost3*(gf_local(0,1)*cost1 + gf_local(1,1)*sint1)*sint2 + (-(gf_local(1,1)*cost1) + gf_local(0,1)*sint1)*sint3) + (gf_local(0,1)*cost1*cost2 + gf_local(1,1)*cost2*sint1 - gf_local(2,1)*sint2)*sint4;
333 
334  grhs_local(2,2) = cost4*(gf_local(2,2)*cost2*cost3 + cost3*(gf_local(0,2)*cost1 + gf_local(1,2)*sint1)*sint2 + (-(gf_local(1,2)*cost1) + gf_local(0,2)*sint1)*sint3) + (gf_local(0,2)*cost1*cost2 + gf_local(1,2)*cost2*sint1 - gf_local(2,2)*sint2)*sint4;
335 
336 
337  double val1 = sqrt(grhs_local(0,1)*grhs_local(0,1)+grhs_local(0,2)*grhs_local(0,2));
338  double val2 = grhs_local(0,0);
339 
340  theta6[0] = atan2(val1,val2);
341  theta6[1] = atan2(-val1,val2);
342 
343 // theta6[3] = M_PI + theta6[0];
344 // theta6[4] = M_PI + theta6[1];
345 
346  for(int mm = 0; mm < 2; mm++)
347  {
348  t6 = theta6[mm];
349  if(!checkJointLimits(angles::normalize_angle(t6),5))
350  continue;
351 
352 #ifdef DEBUG
353  std::cout << "t6 " << t6 << std::endl;
354 #endif
355  if(fabs(cos(t6) - grhs_local(0,0)) > IK_EPS)
356  continue;
357 
358  if(fabs(sin(t6)) < IK_EPS)
359  {
360  // std::cout << "Singularity" << std::endl;
361  theta5[0] = acos(grhs_local(1,1))/2.0;
362  theta7[0] = theta7[0];
363  theta7[1] = M_PI+theta7[0];
364  theta5[1] = theta7[1];
365  }
366  else
367  {
368  theta7[0] = atan2(grhs_local(0,1),grhs_local(0,2));
369  theta5[0] = atan2(grhs_local(1,0),-grhs_local(2,0));
370  theta7[1] = M_PI+theta7[0];
371  theta5[1] = M_PI+theta5[0];
372  }
373 #ifdef DEBUG
374  std::cout << "theta1: " << t1 << std::endl;
375  std::cout << "theta2: " << t2 << std::endl;
376  std::cout << "theta3: " << t3 << std::endl;
377  std::cout << "theta4: " << t4 << std::endl;
378  std::cout << "theta5: " << t5 << std::endl;
379  std::cout << "theta6: " << t6 << std::endl;
380  std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
381 #endif
382  for(int lll =0; lll < 2; lll++)
383  {
384  t5 = theta5[lll];
385  t7 = theta7[lll];
386  if(!checkJointLimits(t5,4))
387  continue;
388  if(!checkJointLimits(t7,6))
389  continue;
390 
391 #ifdef DEBUG
392  std::cout << "t5" << t5 << std::endl;
393  std::cout << "t7" << t7 << std::endl;
394 #endif
395  if(fabs(sin(t6)*sin(t7)-grhs_local(0,1)) > IK_EPS || fabs(cos(t7)*sin(t6)-grhs_local(0,2)) > IK_EPS)
396  continue;
397 
398  solution_ik[0] = normalize_angle(t1)*angle_multipliers_[0];
399  solution_ik[1] = normalize_angle(t2)*angle_multipliers_[1];
400  solution_ik[2] = normalize_angle(t3)*angle_multipliers_[2];
401  solution_ik[3] = normalize_angle(t4)*angle_multipliers_[3];
402  solution_ik[4] = normalize_angle(t5)*angle_multipliers_[4];
403  solution_ik[5] = normalize_angle(t6)*angle_multipliers_[5];
404  solution_ik[6] = normalize_angle(t7)*angle_multipliers_[6];
405  solution.push_back(solution_ik);
406 
407 #ifdef DEBUG
408  std::cout << "SOLN " << solution_ik[0] << " " << solution_ik[1] << " " << solution_ik[2] << " " << solution_ik[3] << " " << solution_ik[4] << " " << solution_ik[5] << " " << solution_ik[6] << std::endl << std::endl;
409 #endif
410  }
411  }
412  }
413  }
414  }
415 }
416 
417 
418 void PR2ArmIK::computeIKShoulderRoll(const Eigen::Matrix4f &g_in, const double &t3, std::vector<std::vector<double> > &solution) const
419 {
420  std::vector<double> solution_ik(NUM_JOINTS_ARM7DOF,0.0);
421  // ROS_INFO(" ");
422  // solution_ik_.clear();
423  // ROS_INFO("Solution IK size: %d",solution_ik_.size());
424  // for(unsigned int i=0; i < solution_ik_.size(); i++)
425  // {
426  // solution_ik_[i].clear();
427  // }
428  // if(!solution_ik_.empty())
429  // solution_ik_.resize(0);
430 //t3 = shoulder/turret roll is specified
431  Eigen::Matrix4f g = g_in;
432  Eigen::Matrix4f gf_local = home_inv_;
433  Eigen::Matrix4f grhs_local = home_inv_;
434 //First bring everything into the arm frame
435  g(0,3) = g_in(0,3) - torso_shoulder_offset_x_;
436  g(1,3) = g_in(1,3) - torso_shoulder_offset_y_;
437  g(2,3) = g_in(2,3) - torso_shoulder_offset_z_;
438 
439  if(!checkJointLimits(t3,2))
440  {
441  return;
442  }
443  double x = g(0,3);
444  double y = g(1,3);
445  double z = g(2,3);
446  double cost1, cost2, cost3, cost4;
447  double sint1, sint2, sint3, sint4;
448 
449  gf_local = g*home_inv_;
450 
451  cost3 = cos(t3);
452  sint3 = sin(t3);
453 
454  double t1(0), t2(0), t4(0), t5(0), t6(0), t7(0);
455 
456  double at(0), bt(0), ct(0);
457 
458  double theta1[2],theta2[2],theta4[4],theta5[2],theta6[4],theta7[2];
459 
460  double c0 = -sin(-t3)*elbow_wrist_offset_;
461  double c1 = -cos(-t3)*elbow_wrist_offset_;
462 
463  double d0 = 4*shoulder_upperarm_offset_*shoulder_upperarm_offset_*(upperarm_elbow_offset_*upperarm_elbow_offset_+c1*c1-z*z);
464  double d1 = 8*shoulder_upperarm_offset_*shoulder_upperarm_offset_*upperarm_elbow_offset_*elbow_wrist_offset_;
465  double d2 = 4*shoulder_upperarm_offset_*shoulder_upperarm_offset_*(elbow_wrist_offset_*elbow_wrist_offset_-c1*c1);
466 
467  double b0 = x*x+y*y+z*z-shoulder_upperarm_offset_*shoulder_upperarm_offset_-upperarm_elbow_offset_*upperarm_elbow_offset_-c0*c0-c1*c1;
468  double b1 = -2*upperarm_elbow_offset_*elbow_wrist_offset_;
469 
470  if(!solveQuadratic(b1*b1-d2,2*b0*b1-d1,b0*b0-d0,&theta4[0],&theta4[1]))
471  {
472 #ifdef DEBUG
473  printf("No solution to quadratic eqn\n");
474 #endif
475  return;
476  }
477  theta4[0] = acos(theta4[0]);
478  theta4[2] = acos(theta4[1]);
479  theta4[1] = -theta4[0];
480  theta4[3] = -theta4[2];
481 
482  for(int jj = 0; jj < 4; jj++)
483  {
484  t4 = theta4[jj];
485 
486  if(!checkJointLimits(t4,3))
487  {
488  continue;
489  }
490  cost4 = cos(t4);
491  sint4 = sin(t4);
492 #ifdef DEBUG
493  std::cout << "t4 " << t4 << std::endl;
494 #endif
495  if(std::isnan(t4))
496  continue;
497  at = cos(t3)*sin(t4)*(shoulder_elbow_offset_-shoulder_wrist_offset_);
498  bt = (shoulder_upperarm_offset_-shoulder_elbow_offset_+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cos(t4));
499  ct = z;
500 
501  if(!solveCosineEqn(at,bt,ct,theta2[0],theta2[1]))
502  continue;
503 
504  for(int ii=0; ii < 2; ii++)
505  {
506  t2 = theta2[ii];
507 #ifdef DEBUG
508  std::cout << "t2 " << t2 << std::endl;
509 #endif
510  if(!checkJointLimits(t2,1))
511  {
512  continue;
513  }
514 
515 
516  sint2 = sin(t2);
517  cost2 = cos(t2);
518 
519  at = -y;
520  bt = x;
521  ct = (shoulder_elbow_offset_-shoulder_wrist_offset_)*sin(t3)*sin(t4);
522  if(!solveCosineEqn(at,bt,ct,theta1[0],theta1[1]))
523  {
524 #ifdef DEBUG
525  std::cout << "could not solve cosine equation for t1" << std::endl;
526 #endif
527  continue;
528  }
529 
530  for(int kk =0; kk < 2; kk++)
531  {
532  t1 = theta1[kk];
533 #ifdef DEBUG
534  std::cout << "t1 " << t1 << std::endl;
535 #endif
536  if(!checkJointLimits(t1,0))
537  {
538  continue;
539  }
540  sint1 = sin(t1);
541  cost1 = cos(t1);
542  if(fabs((shoulder_upperarm_offset_-shoulder_elbow_offset_+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost4)*sint2+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost2*cost3*sint4-z) > IK_EPS )
543  {
544 #ifdef DEBUG
545  printf("z value not matched %f\n",fabs((shoulder_upperarm_offset_-shoulder_elbow_offset_+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost4)*sint2+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost2*cost3*sint4-z));
546 #endif
547  continue;
548  }
549  if(fabs((shoulder_elbow_offset_-shoulder_wrist_offset_)*sint1*sint3*sint4+cost1*(shoulder_upperarm_offset_+cost2*(-shoulder_upperarm_offset_+shoulder_elbow_offset_+(-shoulder_elbow_offset_+shoulder_wrist_offset_)*cost4)+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost3*sint2*sint4) - x) > IK_EPS)
550  {
551 #ifdef DEBUG
552  printf("x value not matched by %f\n",fabs((shoulder_elbow_offset_-shoulder_wrist_offset_)*sint1*sint3*sint4+cost1*(shoulder_upperarm_offset_+cost2*(-shoulder_upperarm_offset_+shoulder_elbow_offset_+(-shoulder_elbow_offset_+shoulder_wrist_offset_)*cost4)+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost3*sint2*sint4) - x));
553 #endif
554  continue;
555  }
556  if(fabs(-(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost1*sint3*sint4+sint1*(shoulder_upperarm_offset_+cost2*(-shoulder_upperarm_offset_+shoulder_elbow_offset_+(-shoulder_elbow_offset_+shoulder_wrist_offset_)*cost4)+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost3*sint2*sint4) - y) > IK_EPS)
557  {
558 #ifdef DEBUG
559  printf("y value not matched\n");
560 #endif
561  continue;
562  }
563  grhs_local(0,0) = cost4*(gf_local(0,0)*cost1*cost2+gf_local(1,0)*cost2*sint1-gf_local(2,0)*sint2)-(gf_local(2,0)*cost2*cost3 + cost3*(gf_local(0,0)*cost1 + gf_local(1,0)*sint1)*sint2 + (-(gf_local(1,0)*cost1) + gf_local(0,0)*sint1)*sint3)*sint4;
564 
565  grhs_local(0,1) = cost4*(gf_local(0,1)*cost1*cost2 + gf_local(1,1)*cost2*sint1 - gf_local(2,1)*sint2) - (gf_local(2,1)*cost2*cost3 + cost3*(gf_local(0,1)*cost1 + gf_local(1,1)*sint1)*sint2 + (-(gf_local(1,1)*cost1) + gf_local(0,1)*sint1)*sint3)*sint4;
566 
567  grhs_local(0,2) = cost4*(gf_local(0,2)*cost1*cost2 + gf_local(1,2)*cost2*sint1 - gf_local(2,2)*sint2) - (gf_local(2,2)*cost2*cost3 + cost3*(gf_local(0,2)*cost1 + gf_local(1,2)*sint1)*sint2 + (-(gf_local(1,2)*cost1) + gf_local(0,2)*sint1)*sint3)*sint4;
568 
569  grhs_local(1,0) = cost3*(gf_local(1,0)*cost1 - gf_local(0,0)*sint1) + gf_local(2,0)*cost2*sint3 + (gf_local(0,0)*cost1 + gf_local(1,0)*sint1)*sint2*sint3;
570 
571  grhs_local(1,1) = cost3*(gf_local(1,1)*cost1 - gf_local(0,1)*sint1) + gf_local(2,1)*cost2*sint3 + (gf_local(0,1)*cost1 + gf_local(1,1)*sint1)*sint2*sint3;
572 
573  grhs_local(1,2) = cost3*(gf_local(1,2)*cost1 - gf_local(0,2)*sint1) + gf_local(2,2)*cost2*sint3 + (gf_local(0,2)*cost1 + gf_local(1,2)*sint1)*sint2*sint3;
574 
575  grhs_local(2,0) = cost4*(gf_local(2,0)*cost2*cost3 + cost3*(gf_local(0,0)*cost1 + gf_local(1,0)*sint1)*sint2 + (-(gf_local(1,0)*cost1) + gf_local(0,0)*sint1)*sint3) + (gf_local(0,0)*cost1*cost2 + gf_local(1,0)*cost2*sint1 - gf_local(2,0)*sint2)*sint4;
576 
577  grhs_local(2,1) = cost4*(gf_local(2,1)*cost2*cost3 + cost3*(gf_local(0,1)*cost1 + gf_local(1,1)*sint1)*sint2 + (-(gf_local(1,1)*cost1) + gf_local(0,1)*sint1)*sint3) + (gf_local(0,1)*cost1*cost2 + gf_local(1,1)*cost2*sint1 - gf_local(2,1)*sint2)*sint4;
578 
579  grhs_local(2,2) = cost4*(gf_local(2,2)*cost2*cost3 + cost3*(gf_local(0,2)*cost1 + gf_local(1,2)*sint1)*sint2 + (-(gf_local(1,2)*cost1) + gf_local(0,2)*sint1)*sint3) + (gf_local(0,2)*cost1*cost2 + gf_local(1,2)*cost2*sint1 - gf_local(2,2)*sint2)*sint4;
580 
581 
582 
583 
584 
585 
586 
587 
588 
589  double val1 = sqrt(grhs_local(0,1)*grhs_local(0,1)+grhs_local(0,2)*grhs_local(0,2));
590  double val2 = grhs_local(0,0);
591 
592  theta6[0] = atan2(val1,val2);
593  theta6[1] = atan2(-val1,val2);
594 
595  for(int mm = 0; mm < 2; mm++)
596  {
597  t6 = theta6[mm];
598 #ifdef DEBUG
599  std::cout << "t6 " << t6 << std::endl;
600 #endif
601  if(!checkJointLimits(t6,5))
602  {
603  continue;
604  }
605 
606 
607  if(fabs(cos(t6) - grhs_local(0,0)) > IK_EPS)
608  continue;
609 
610  if(fabs(sin(t6)) < IK_EPS)
611  {
612  // std::cout << "Singularity" << std::endl;
613  theta5[0] = acos(grhs_local(1,1))/2.0;
614  theta7[0] = theta5[0];
615 // theta7[1] = M_PI+theta7[0];
616 // theta5[1] = theta7[1];
617  }
618  else
619  {
620  theta7[0] = atan2(grhs_local(0,1)/sin(t6),grhs_local(0,2)/sin(t6));
621  theta5[0] = atan2(grhs_local(1,0)/sin(t6),-grhs_local(2,0)/sin(t6));
622 // theta7[1] = M_PI+theta7[0];
623 // theta5[1] = M_PI+theta5[0];
624  }
625  for(int lll =0; lll < 1; lll++)
626  {
627  t5 = theta5[lll];
628  t7 = theta7[lll];
629 
630  if(!checkJointLimits(t5,4))
631  {
632  continue;
633  }
634  if(!checkJointLimits(t7,6))
635  {
636  continue;
637  }
638 
639 
640 #ifdef DEBUG
641  std::cout << "t5 " << t5 << std::endl;
642  std::cout << "t7 " << t7 << std::endl;
643 #endif
644  // if(fabs(sin(t6)*sin(t7)-grhs_local(0,1)) > IK_EPS || fabs(cos(t7)*sin(t6)-grhs_local(0,2)) > IK_EPS)
645  // continue;
646 
647 #ifdef DEBUG
648  std::cout << "theta1: " << t1 << std::endl;
649  std::cout << "theta2: " << t2 << std::endl;
650  std::cout << "theta3: " << t3 << std::endl;
651  std::cout << "theta4: " << t4 << std::endl;
652  std::cout << "theta5: " << t5 << std::endl;
653  std::cout << "theta6: " << t6 << std::endl;
654  std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
655 #endif
656 
657 
658  solution_ik[0] = normalize_angle(t1*angle_multipliers_[0]);
659  solution_ik[1] = normalize_angle(t2*angle_multipliers_[1]);
660  solution_ik[2] = t3*angle_multipliers_[2];
661  solution_ik[3] = normalize_angle(t4*angle_multipliers_[3]);
662  solution_ik[4] = normalize_angle(t5*angle_multipliers_[4]);
663  solution_ik[5] = normalize_angle(t6*angle_multipliers_[5]);
664  solution_ik[6] = normalize_angle(t7*angle_multipliers_[6]);
665  solution.push_back(solution_ik);
666 #ifdef DEBUG
667  std::cout << "SOLN " << solution_ik[0] << " " << solution_ik[1] << " " << solution_ik[2] << " " << solution_ik[3] << " " << solution_ik[4] << " " << solution_ik[5] << " " << solution_ik[6] << std::endl << std::endl;
668 #endif
669  }
670  }
671  }
672  }
673  }
674 }
675 
676 
677 bool PR2ArmIK::checkJointLimits(const std::vector<double> &joint_values) const
678 {
679  for(int i=0; i<NUM_JOINTS_ARM7DOF; i++)
680  {
681  if(!checkJointLimits(angles::normalize_angle(joint_values[i]*angle_multipliers_[i]),i))
682  {
683  return false;
684  }
685  }
686  return true;
687 }
688 
689 bool PR2ArmIK::checkJointLimits(const double &joint_value, const int &joint_num) const
690 {
691  double jv;
692  if(continuous_joint_[joint_num])
693  jv= angles::normalize_angle(joint_value*angle_multipliers_[joint_num]);
694  else if (joint_num ==2)
695  jv = joint_value*angle_multipliers_[joint_num];
696  else
697  jv= angles::normalize_angle(joint_value*angle_multipliers_[joint_num]);
698 
699  if(jv < min_angles_[joint_num] || jv > max_angles_[joint_num])
700  {
701  // ROS_INFO("Angle %d = %f out of range: (%f,%f)\n",joint_num,joint_value,min_angles_[joint_num],max_angles_[joint_num]);
702  return false;
703  }
704  return true;
705 }
ROS_FATAL
#define ROS_FATAL(...)
d2
Chain d2()
pr2_arm_ik.h
pr2_arm_kinematics
Namespace for the PR2ArmKinematics.
solveQuadratic
bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
M_PI
#define M_PI
ROS_WARN
#define ROS_WARN(...)
y
double y
z
double z
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
angles::normalize_angle
def normalize_angle(angle)
acos
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
x
double x
ROS_ERROR
#define ROS_ERROR(...)
distance
double distance(const urdf::Pose &transform)
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
solveCosineEqn
bool solveCosineEqn(const double &a, const double &b, const double &c, double &soln1, double &soln2)
ROS_DEBUG
#define ROS_DEBUG(...)

pr2_moveit_plugins
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Nov 17 2019 03:24:45