pr2_arm_ik.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, 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, E. Gil Jones */
36 
37 #include <angles/angles.h>
38 #include "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::ModelInterface& 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_NAMED("pr2_arm_kinematics_plugin", "Could not find joint: %s", link->parent_joint->name.c_str());
70  else
71  ROS_ERROR_NAMED("pr2_arm_kinematics_plugin", "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) +
78  joint->axis.z * fabs(joint->axis.z));
79  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x,
80  joint->axis.y, joint->axis.z);
81  if (joint->type != urdf::Joint::CONTINUOUS)
82  {
83  if (joint->safety)
84  {
85  min_angles_.push_back(joint->safety->soft_lower_limit);
86  max_angles_.push_back(joint->safety->soft_upper_limit);
87  }
88  else
89  {
90  if (joint->limits)
91  {
92  min_angles_.push_back(joint->limits->lower);
93  max_angles_.push_back(joint->limits->upper);
94  }
95  else
96  {
97  min_angles_.push_back(0.0);
98  max_angles_.push_back(0.0);
99  ROS_WARN_NAMED("pr2_arm_kinematics_plugin", "No joint limits or joint '%s'", joint->name.c_str());
100  }
101  }
102  continuous_joint_.push_back(false);
103  }
104  else
105  {
106  min_angles_.push_back(-M_PI);
107  max_angles_.push_back(M_PI);
108  continuous_joint_.push_back(true);
109  }
110  addJointToChainInfo(link->parent_joint, solver_info_);
111  num_joints++;
112  }
113  link = robot_model.getLink(link->getParent()->name);
114  }
115 
116  solver_info_.link_names.push_back(tip_name);
117 
118  // solver_info_.link_names.push_back(tip_name);
119  // We expect order from root to tip, so reverse the order
120  std::reverse(angle_multipliers_.begin(), angle_multipliers_.end());
121  std::reverse(min_angles_.begin(), min_angles_.end());
122  std::reverse(max_angles_.begin(), max_angles_.end());
123  std::reverse(link_offset.begin(), link_offset.end());
124  std::reverse(solver_info_.limits.begin(), solver_info_.limits.end());
125  std::reverse(solver_info_.joint_names.begin(), solver_info_.joint_names.end());
126  std::reverse(solver_info_.link_names.begin(), solver_info_.link_names.end());
127  std::reverse(continuous_joint_.begin(), continuous_joint_.end());
128 
129  if (num_joints != 7)
130  {
131  ROS_ERROR_NAMED("pr2_arm_kinematics_plugin", "PR2ArmIK:: Chain from %s to %s does not have 7 joints",
132  root_name.c_str(), tip_name.c_str());
133  return false;
134  }
135 
136  torso_shoulder_offset_x_ = link_offset[0].position.x;
137  torso_shoulder_offset_y_ = link_offset[0].position.y;
138  torso_shoulder_offset_z_ = link_offset[0].position.z;
139  shoulder_upperarm_offset_ = distance(link_offset[1]);
140  upperarm_elbow_offset_ = distance(link_offset[3]);
141  elbow_wrist_offset_ = distance(link_offset[5]);
142  shoulder_elbow_offset_ = shoulder_upperarm_offset_ + upperarm_elbow_offset_;
143  shoulder_wrist_offset_ = shoulder_upperarm_offset_ + upperarm_elbow_offset_ + elbow_wrist_offset_;
144 
145  Eigen::Affine3f home = Eigen::Affine3f::Identity();
146  home(0, 3) = shoulder_upperarm_offset_ + upperarm_elbow_offset_ + elbow_wrist_offset_;
147  home_inv_ = home.inverse(Eigen::Isometry);
148  grhs_ = home;
149  gf_ = home_inv_;
150  solution_.resize(NUM_JOINTS_ARM7DOF);
151  return true;
152 }
153 
154 void PR2ArmIK::addJointToChainInfo(urdf::JointConstSharedPtr joint, moveit_msgs::KinematicSolverInfo& info)
155 {
156  moveit_msgs::JointLimits limit;
157  info.joint_names.push_back(joint->name); // Joints are coming in reverse order
158 
159  if (joint->type != urdf::Joint::CONTINUOUS)
160  {
161  if (joint->safety)
162  {
163  limit.min_position = joint->safety->soft_lower_limit;
164  limit.max_position = joint->safety->soft_upper_limit;
165  limit.has_position_limits = true;
166  }
167  else if (joint->limits)
168  {
169  limit.min_position = joint->limits->lower;
170  limit.max_position = joint->limits->upper;
171  limit.has_position_limits = true;
172  }
173  else
174  limit.has_position_limits = false;
175  }
176  else
177  {
178  limit.min_position = -M_PI;
179  limit.max_position = M_PI;
180  limit.has_position_limits = false;
181  }
182  if (joint->limits)
183  {
184  limit.max_velocity = joint->limits->velocity;
185  limit.has_velocity_limits = 1;
186  }
187  else
188  limit.has_velocity_limits = 0;
189  info.limits.push_back(limit);
190 }
191 
192 void PR2ArmIK::getSolverInfo(moveit_msgs::KinematicSolverInfo& info)
193 {
194  info = solver_info_;
195 }
196 
197 void PR2ArmIK::computeIKShoulderPan(const Eigen::Affine3f& g_in, const double& t1_in,
198  std::vector<std::vector<double> >& solution) const
199 {
200  // t1 = shoulder/turret pan is specified
201  // solution_ik_.resize(0);
202  std::vector<double> solution_ik(NUM_JOINTS_ARM7DOF, 0.0);
203  Eigen::Affine3f g = g_in;
204  Eigen::Affine3f gf_local = home_inv_;
205  Eigen::Affine3f grhs_local = home_inv_;
206  // First bring everything into the arm frame
207  g(0, 3) = g_in(0, 3) - torso_shoulder_offset_x_;
208  g(1, 3) = g_in(1, 3) - torso_shoulder_offset_y_;
209  g(2, 3) = g_in(2, 3) - torso_shoulder_offset_z_;
210 
211  double t1 = angles::normalize_angle(t1_in);
212  if (!checkJointLimits(t1, 0))
213  return;
214 
215  double cost1, cost2, cost3, cost4;
216  double sint1, sint2, sint3, sint4;
217 
218  gf_local = g * home_inv_;
219 
220  cost1 = cos(t1);
221  sint1 = sin(t1);
222 
223  double t2(0), t3(0), t4(0), t5(0), t6(0), t7(0);
224 
225  double at(0), bt(0), ct(0);
226 
227  double theta2[2], theta3[2], theta4[2], theta5[2], theta6[4], theta7[2];
228 
229  double sopx = shoulder_upperarm_offset_ * cost1;
230  double sopy = shoulder_upperarm_offset_ * sint1;
231  double sopz = 0;
232 
233  double x = g(0, 3);
234  double y = g(1, 3);
235  double z = g(2, 3);
236 
237  double dx = x - sopx;
238  double dy = y - sopy;
239  double dz = z - sopz;
240 
241  double dd = dx * dx + dy * dy + dz * dz;
242 
243  double numerator =
244  dd - shoulder_upperarm_offset_ * shoulder_upperarm_offset_ +
245  2 * shoulder_upperarm_offset_ * shoulder_elbow_offset_ - 2 * shoulder_elbow_offset_ * shoulder_elbow_offset_ +
246  2 * shoulder_elbow_offset_ * shoulder_wrist_offset_ - shoulder_wrist_offset_ * shoulder_wrist_offset_;
247  double denominator =
248  2 * (shoulder_upperarm_offset_ - shoulder_elbow_offset_) * (shoulder_elbow_offset_ - shoulder_wrist_offset_);
249 
250  double acosTerm = numerator / denominator;
251 
252  if (acosTerm > 1.0 || acosTerm < -1.0)
253  return;
254 
255  double acos_angle = acos(acosTerm);
256 
257  theta4[0] = acos_angle;
258  theta4[1] = -acos_angle;
259 
260 #ifdef DEBUG
261  std::cout << "ComputeIK::theta3:" << numerator << "," << denominator << "," << std::endl << theta4[0] << std::endl;
262 #endif
263 
264  for (int jj = 0; jj < 2; jj++)
265  {
266  t4 = theta4[jj];
267  cost4 = cos(t4);
268  sint4 = sin(t4);
269 
270 #ifdef DEBUG
271  std::cout << "t4 " << t4 << std::endl;
272 #endif
273  if (std::isnan(t4))
274  continue;
275 
276  if (!checkJointLimits(t4, 3))
277  continue;
278 
279  at = x * cost1 + y * sint1 - shoulder_upperarm_offset_;
280  bt = -z;
281  ct = -shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
282  (shoulder_wrist_offset_ - shoulder_elbow_offset_) * cos(t4);
283 
284  if (!solveCosineEqn(at, bt, ct, theta2[0], theta2[1]))
285  continue;
286 
287  for (int ii = 0; ii < 2; ii++)
288  {
289  t2 = theta2[ii];
290  if (!checkJointLimits(t2, 1))
291  continue;
292 
293 #ifdef DEBUG
294  std::cout << "t2 " << t2 << std::endl;
295 #endif
296  sint2 = sin(t2);
297  cost2 = cos(t2);
298 
299  at = sint1 * (shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint2 * sint4;
300  bt = (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost1 * sint4;
301  ct = y -
302  (shoulder_upperarm_offset_ +
303  cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
304  (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cos(t4))) *
305  sint1;
306  if (!solveCosineEqn(at, bt, ct, theta3[0], theta3[1]))
307  continue;
308 
309  for (int kk = 0; kk < 2; kk++)
310  {
311  t3 = theta3[kk];
312 
313  if (!checkJointLimits(angles::normalize_angle(t3), 2))
314  continue;
315 
316  sint3 = sin(t3);
317  cost3 = cos(t3);
318 #ifdef DEBUG
319  std::cout << "t3 " << t3 << std::endl;
320 #endif
321  if (fabs((shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
322  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost4) *
323  sint2 +
324  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost2 * cost3 * sint4 - z) > IK_EPS)
325  continue;
326 
327  if (fabs((shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint1 * sint3 * sint4 +
328  cost1 * (shoulder_upperarm_offset_ +
329  cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
330  (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost4) +
331  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost3 * sint2 * sint4) -
332  x) > IK_EPS)
333  continue;
334 
335  grhs_local(0, 0) =
336  cost4 * (gf_local(0, 0) * cost1 * cost2 + gf_local(1, 0) * cost2 * sint1 - gf_local(2, 0) * sint2) -
337  (gf_local(2, 0) * cost2 * cost3 + cost3 * (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 +
338  (-(gf_local(1, 0) * cost1) + gf_local(0, 0) * sint1) * sint3) *
339  sint4;
340 
341  grhs_local(0, 1) =
342  cost4 * (gf_local(0, 1) * cost1 * cost2 + gf_local(1, 1) * cost2 * sint1 - gf_local(2, 1) * sint2) -
343  (gf_local(2, 1) * cost2 * cost3 + cost3 * (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 +
344  (-(gf_local(1, 1) * cost1) + gf_local(0, 1) * sint1) * sint3) *
345  sint4;
346 
347  grhs_local(0, 2) =
348  cost4 * (gf_local(0, 2) * cost1 * cost2 + gf_local(1, 2) * cost2 * sint1 - gf_local(2, 2) * sint2) -
349  (gf_local(2, 2) * cost2 * cost3 + cost3 * (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 +
350  (-(gf_local(1, 2) * cost1) + gf_local(0, 2) * sint1) * sint3) *
351  sint4;
352 
353  grhs_local(1, 0) = cost3 * (gf_local(1, 0) * cost1 - gf_local(0, 0) * sint1) + gf_local(2, 0) * cost2 * sint3 +
354  (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 * sint3;
355 
356  grhs_local(1, 1) = cost3 * (gf_local(1, 1) * cost1 - gf_local(0, 1) * sint1) + gf_local(2, 1) * cost2 * sint3 +
357  (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 * sint3;
358 
359  grhs_local(1, 2) = cost3 * (gf_local(1, 2) * cost1 - gf_local(0, 2) * sint1) + gf_local(2, 2) * cost2 * sint3 +
360  (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 * sint3;
361 
362  grhs_local(2, 0) =
363  cost4 *
364  (gf_local(2, 0) * cost2 * cost3 + cost3 * (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 +
365  (-(gf_local(1, 0) * cost1) + gf_local(0, 0) * sint1) * sint3) +
366  (gf_local(0, 0) * cost1 * cost2 + gf_local(1, 0) * cost2 * sint1 - gf_local(2, 0) * sint2) * sint4;
367 
368  grhs_local(2, 1) =
369  cost4 *
370  (gf_local(2, 1) * cost2 * cost3 + cost3 * (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 +
371  (-(gf_local(1, 1) * cost1) + gf_local(0, 1) * sint1) * sint3) +
372  (gf_local(0, 1) * cost1 * cost2 + gf_local(1, 1) * cost2 * sint1 - gf_local(2, 1) * sint2) * sint4;
373 
374  grhs_local(2, 2) =
375  cost4 *
376  (gf_local(2, 2) * cost2 * cost3 + cost3 * (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 +
377  (-(gf_local(1, 2) * cost1) + gf_local(0, 2) * sint1) * sint3) +
378  (gf_local(0, 2) * cost1 * cost2 + gf_local(1, 2) * cost2 * sint1 - gf_local(2, 2) * sint2) * sint4;
379 
380  double val1 = sqrt(grhs_local(0, 1) * grhs_local(0, 1) + grhs_local(0, 2) * grhs_local(0, 2));
381  double val2 = grhs_local(0, 0);
382 
383  theta6[0] = atan2(val1, val2);
384  theta6[1] = atan2(-val1, val2);
385 
386  // theta6[3] = M_PI + theta6[0];
387  // theta6[4] = M_PI + theta6[1];
388 
389  for (int mm = 0; mm < 2; mm++)
390  {
391  t6 = theta6[mm];
392  if (!checkJointLimits(angles::normalize_angle(t6), 5))
393  continue;
394 
395 #ifdef DEBUG
396  std::cout << "t6 " << t6 << std::endl;
397 #endif
398  if (fabs(cos(t6) - grhs_local(0, 0)) > IK_EPS)
399  continue;
400 
401  if (fabs(sin(t6)) < IK_EPS)
402  {
403  // std::cout << "Singularity" << std::endl;
404  theta5[0] = acos(grhs_local(1, 1)) / 2.0;
405  theta7[0] = theta7[0];
406  theta7[1] = M_PI + theta7[0];
407  theta5[1] = theta7[1];
408  }
409  else
410  {
411  theta7[0] = atan2(grhs_local(0, 1), grhs_local(0, 2));
412  theta5[0] = atan2(grhs_local(1, 0), -grhs_local(2, 0));
413  theta7[1] = M_PI + theta7[0];
414  theta5[1] = M_PI + theta5[0];
415  }
416 #ifdef DEBUG
417  std::cout << "theta1: " << t1 << std::endl;
418  std::cout << "theta2: " << t2 << std::endl;
419  std::cout << "theta3: " << t3 << std::endl;
420  std::cout << "theta4: " << t4 << std::endl;
421  std::cout << "theta5: " << t5 << std::endl;
422  std::cout << "theta6: " << t6 << std::endl;
423  std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
424 #endif
425  for (int lll = 0; lll < 2; lll++)
426  {
427  t5 = theta5[lll];
428  t7 = theta7[lll];
429  if (!checkJointLimits(t5, 4))
430  continue;
431  if (!checkJointLimits(t7, 6))
432  continue;
433 
434 #ifdef DEBUG
435  std::cout << "t5" << t5 << std::endl;
436  std::cout << "t7" << t7 << std::endl;
437 #endif
438  if (fabs(sin(t6) * sin(t7) - grhs_local(0, 1)) > IK_EPS ||
439  fabs(cos(t7) * sin(t6) - grhs_local(0, 2)) > IK_EPS)
440  continue;
441 
442  solution_ik[0] = normalize_angle(t1) * angle_multipliers_[0];
443  solution_ik[1] = normalize_angle(t2) * angle_multipliers_[1];
444  solution_ik[2] = normalize_angle(t3) * angle_multipliers_[2];
445  solution_ik[3] = normalize_angle(t4) * angle_multipliers_[3];
446  solution_ik[4] = normalize_angle(t5) * angle_multipliers_[4];
447  solution_ik[5] = normalize_angle(t6) * angle_multipliers_[5];
448  solution_ik[6] = normalize_angle(t7) * angle_multipliers_[6];
449  solution.push_back(solution_ik);
450 
451 #ifdef DEBUG
452  std::cout << "SOLN " << solution_ik[0] << " " << solution_ik[1] << " " << solution_ik[2] << " "
453  << solution_ik[3] << " " << solution_ik[4] << " " << solution_ik[5] << " " << solution_ik[6]
454  << std::endl
455  << std::endl;
456 #endif
457  }
458  }
459  }
460  }
461  }
462 }
463 
464 void PR2ArmIK::computeIKShoulderRoll(const Eigen::Affine3f& g_in, const double& t3,
465  std::vector<std::vector<double> >& solution) const
466 {
467  std::vector<double> solution_ik(NUM_JOINTS_ARM7DOF, 0.0);
468  // ROS_INFO(" ");
469  // solution_ik_.clear();
470  // ROS_INFO("Solution IK size: %d",solution_ik_.size());
471  // for(unsigned int i=0; i < solution_ik_.size(); i++)
472  // {
473  // solution_ik_[i].clear();
474  // }
475  // if(!solution_ik_.empty())
476  // solution_ik_.resize(0);
477  // t3 = shoulder/turret roll is specified
478  Eigen::Affine3f g = g_in;
479  Eigen::Affine3f gf_local = home_inv_;
480  Eigen::Affine3f grhs_local = home_inv_;
481  // First bring everything into the arm frame
482  g(0, 3) = g_in(0, 3) - torso_shoulder_offset_x_;
483  g(1, 3) = g_in(1, 3) - torso_shoulder_offset_y_;
484  g(2, 3) = g_in(2, 3) - torso_shoulder_offset_z_;
485 
486  if (!checkJointLimits(t3, 2))
487  {
488  return;
489  }
490  double x = g(0, 3);
491  double y = g(1, 3);
492  double z = g(2, 3);
493  double cost1, cost2, cost3, cost4;
494  double sint1, sint2, sint3, sint4;
495 
496  gf_local = g * home_inv_;
497 
498  cost3 = cos(t3);
499  sint3 = sin(t3);
500 
501  double t1(0), t2(0), t4(0), t5(0), t6(0), t7(0);
502 
503  double at(0), bt(0), ct(0);
504 
505  double theta1[2], theta2[2], theta4[4], theta5[2], theta6[4], theta7[2];
506 
507  double c0 = -sin(-t3) * elbow_wrist_offset_;
508  double c1 = -cos(-t3) * elbow_wrist_offset_;
509 
510  double d0 = 4 * shoulder_upperarm_offset_ * shoulder_upperarm_offset_ *
511  (upperarm_elbow_offset_ * upperarm_elbow_offset_ + c1 * c1 - z * z);
512  double d1 = 8 * shoulder_upperarm_offset_ * shoulder_upperarm_offset_ * upperarm_elbow_offset_ * elbow_wrist_offset_;
513  double d2 =
514  4 * shoulder_upperarm_offset_ * shoulder_upperarm_offset_ * (elbow_wrist_offset_ * elbow_wrist_offset_ - c1 * c1);
515 
516  double b0 = x * x + y * y + z * z - shoulder_upperarm_offset_ * shoulder_upperarm_offset_ -
517  upperarm_elbow_offset_ * upperarm_elbow_offset_ - c0 * c0 - c1 * c1;
518  double b1 = -2 * upperarm_elbow_offset_ * elbow_wrist_offset_;
519 
520  if (!solveQuadratic(b1 * b1 - d2, 2 * b0 * b1 - d1, b0 * b0 - d0, &theta4[0], &theta4[1]))
521  {
522 #ifdef DEBUG
523  printf("No solution to quadratic eqn\n");
524 #endif
525  return;
526  }
527  theta4[0] = acos(theta4[0]);
528  theta4[2] = acos(theta4[1]);
529  theta4[1] = -theta4[0];
530  theta4[3] = -theta4[2];
531 
532  for (int jj = 0; jj < 4; jj++)
533  {
534  t4 = theta4[jj];
535 
536  if (!checkJointLimits(t4, 3))
537  {
538  continue;
539  }
540  cost4 = cos(t4);
541  sint4 = sin(t4);
542 #ifdef DEBUG
543  std::cout << "t4 " << t4 << std::endl;
544 #endif
545  if (std::isnan(t4))
546  continue;
547  at = cos(t3) * sin(t4) * (shoulder_elbow_offset_ - shoulder_wrist_offset_);
548  bt = (shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
549  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cos(t4));
550  ct = z;
551 
552  if (!solveCosineEqn(at, bt, ct, theta2[0], theta2[1]))
553  continue;
554 
555  for (int ii = 0; ii < 2; ii++)
556  {
557  t2 = theta2[ii];
558 #ifdef DEBUG
559  std::cout << "t2 " << t2 << std::endl;
560 #endif
561  if (!checkJointLimits(t2, 1))
562  {
563  continue;
564  }
565 
566  sint2 = sin(t2);
567  cost2 = cos(t2);
568 
569  at = -y;
570  bt = x;
571  ct = (shoulder_elbow_offset_ - shoulder_wrist_offset_) * sin(t3) * sin(t4);
572  if (!solveCosineEqn(at, bt, ct, theta1[0], theta1[1]))
573  {
574 #ifdef DEBUG
575  std::cout << "could not solve cosine equation for t1" << std::endl;
576 #endif
577  continue;
578  }
579 
580  for (int kk = 0; kk < 2; kk++)
581  {
582  t1 = theta1[kk];
583 #ifdef DEBUG
584  std::cout << "t1 " << t1 << std::endl;
585 #endif
586  if (!checkJointLimits(t1, 0))
587  {
588  continue;
589  }
590  sint1 = sin(t1);
591  cost1 = cos(t1);
592  if (fabs((shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
593  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost4) *
594  sint2 +
595  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost2 * cost3 * sint4 - z) > IK_EPS)
596  {
597 #ifdef DEBUG
598  printf("z value not matched %f\n",
599  fabs((shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
600  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost4) *
601  sint2 +
602  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost2 * cost3 * sint4 - z));
603 #endif
604  continue;
605  }
606  if (fabs((shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint1 * sint3 * sint4 +
607  cost1 * (shoulder_upperarm_offset_ +
608  cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
609  (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost4) +
610  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost3 * sint2 * sint4) -
611  x) > IK_EPS)
612  {
613 #ifdef DEBUG
614  printf("x value not matched by %f\n",
615  fabs((shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint1 * sint3 * sint4 +
616  cost1 * (shoulder_upperarm_offset_ +
617  cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
618  (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost4) +
619  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost3 * sint2 * sint4) -
620  x));
621 #endif
622  continue;
623  }
624  if (fabs(-(shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost1 * sint3 * sint4 +
625  sint1 * (shoulder_upperarm_offset_ +
626  cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
627  (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost4) +
628  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost3 * sint2 * sint4) -
629  y) > IK_EPS)
630  {
631 #ifdef DEBUG
632  printf("y value not matched\n");
633 #endif
634  continue;
635  }
636  grhs_local(0, 0) =
637  cost4 * (gf_local(0, 0) * cost1 * cost2 + gf_local(1, 0) * cost2 * sint1 - gf_local(2, 0) * sint2) -
638  (gf_local(2, 0) * cost2 * cost3 + cost3 * (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 +
639  (-(gf_local(1, 0) * cost1) + gf_local(0, 0) * sint1) * sint3) *
640  sint4;
641 
642  grhs_local(0, 1) =
643  cost4 * (gf_local(0, 1) * cost1 * cost2 + gf_local(1, 1) * cost2 * sint1 - gf_local(2, 1) * sint2) -
644  (gf_local(2, 1) * cost2 * cost3 + cost3 * (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 +
645  (-(gf_local(1, 1) * cost1) + gf_local(0, 1) * sint1) * sint3) *
646  sint4;
647 
648  grhs_local(0, 2) =
649  cost4 * (gf_local(0, 2) * cost1 * cost2 + gf_local(1, 2) * cost2 * sint1 - gf_local(2, 2) * sint2) -
650  (gf_local(2, 2) * cost2 * cost3 + cost3 * (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 +
651  (-(gf_local(1, 2) * cost1) + gf_local(0, 2) * sint1) * sint3) *
652  sint4;
653 
654  grhs_local(1, 0) = cost3 * (gf_local(1, 0) * cost1 - gf_local(0, 0) * sint1) + gf_local(2, 0) * cost2 * sint3 +
655  (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 * sint3;
656 
657  grhs_local(1, 1) = cost3 * (gf_local(1, 1) * cost1 - gf_local(0, 1) * sint1) + gf_local(2, 1) * cost2 * sint3 +
658  (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 * sint3;
659 
660  grhs_local(1, 2) = cost3 * (gf_local(1, 2) * cost1 - gf_local(0, 2) * sint1) + gf_local(2, 2) * cost2 * sint3 +
661  (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 * sint3;
662 
663  grhs_local(2, 0) =
664  cost4 *
665  (gf_local(2, 0) * cost2 * cost3 + cost3 * (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 +
666  (-(gf_local(1, 0) * cost1) + gf_local(0, 0) * sint1) * sint3) +
667  (gf_local(0, 0) * cost1 * cost2 + gf_local(1, 0) * cost2 * sint1 - gf_local(2, 0) * sint2) * sint4;
668 
669  grhs_local(2, 1) =
670  cost4 *
671  (gf_local(2, 1) * cost2 * cost3 + cost3 * (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 +
672  (-(gf_local(1, 1) * cost1) + gf_local(0, 1) * sint1) * sint3) +
673  (gf_local(0, 1) * cost1 * cost2 + gf_local(1, 1) * cost2 * sint1 - gf_local(2, 1) * sint2) * sint4;
674 
675  grhs_local(2, 2) =
676  cost4 *
677  (gf_local(2, 2) * cost2 * cost3 + cost3 * (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 +
678  (-(gf_local(1, 2) * cost1) + gf_local(0, 2) * sint1) * sint3) +
679  (gf_local(0, 2) * cost1 * cost2 + gf_local(1, 2) * cost2 * sint1 - gf_local(2, 2) * sint2) * sint4;
680 
681  double val1 = sqrt(grhs_local(0, 1) * grhs_local(0, 1) + grhs_local(0, 2) * grhs_local(0, 2));
682  double val2 = grhs_local(0, 0);
683 
684  theta6[0] = atan2(val1, val2);
685  theta6[1] = atan2(-val1, val2);
686 
687  for (int mm = 0; mm < 2; mm++)
688  {
689  t6 = theta6[mm];
690 #ifdef DEBUG
691  std::cout << "t6 " << t6 << std::endl;
692 #endif
693  if (!checkJointLimits(t6, 5))
694  {
695  continue;
696  }
697 
698  if (fabs(cos(t6) - grhs_local(0, 0)) > IK_EPS)
699  continue;
700 
701  if (fabs(sin(t6)) < IK_EPS)
702  {
703  // std::cout << "Singularity" << std::endl;
704  theta5[0] = acos(grhs_local(1, 1)) / 2.0;
705  theta7[0] = theta5[0];
706  // theta7[1] = M_PI+theta7[0];
707  // theta5[1] = theta7[1];
708  }
709  else
710  {
711  theta7[0] = atan2(grhs_local(0, 1) / sin(t6), grhs_local(0, 2) / sin(t6));
712  theta5[0] = atan2(grhs_local(1, 0) / sin(t6), -grhs_local(2, 0) / sin(t6));
713  // theta7[1] = M_PI+theta7[0];
714  // theta5[1] = M_PI+theta5[0];
715  }
716  for (int lll = 0; lll < 1; lll++)
717  {
718  t5 = theta5[lll];
719  t7 = theta7[lll];
720 
721  if (!checkJointLimits(t5, 4))
722  {
723  continue;
724  }
725  if (!checkJointLimits(t7, 6))
726  {
727  continue;
728  }
729 
730 #ifdef DEBUG
731  std::cout << "t5 " << t5 << std::endl;
732  std::cout << "t7 " << t7 << std::endl;
733 #endif
734 // if(fabs(sin(t6)*sin(t7)-grhs_local(0,1)) > IK_EPS || fabs(cos(t7)*sin(t6)-grhs_local(0,2)) > IK_EPS)
735 // continue;
736 
737 #ifdef DEBUG
738  std::cout << "theta1: " << t1 << std::endl;
739  std::cout << "theta2: " << t2 << std::endl;
740  std::cout << "theta3: " << t3 << std::endl;
741  std::cout << "theta4: " << t4 << std::endl;
742  std::cout << "theta5: " << t5 << std::endl;
743  std::cout << "theta6: " << t6 << std::endl;
744  std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
745 #endif
746 
747  solution_ik[0] = normalize_angle(t1 * angle_multipliers_[0]);
748  solution_ik[1] = normalize_angle(t2 * angle_multipliers_[1]);
749  solution_ik[2] = t3 * angle_multipliers_[2];
750  solution_ik[3] = normalize_angle(t4 * angle_multipliers_[3]);
751  solution_ik[4] = normalize_angle(t5 * angle_multipliers_[4]);
752  solution_ik[5] = normalize_angle(t6 * angle_multipliers_[5]);
753  solution_ik[6] = normalize_angle(t7 * angle_multipliers_[6]);
754  solution.push_back(solution_ik);
755 #ifdef DEBUG
756  std::cout << "SOLN " << solution_ik[0] << " " << solution_ik[1] << " " << solution_ik[2] << " "
757  << solution_ik[3] << " " << solution_ik[4] << " " << solution_ik[5] << " " << solution_ik[6]
758  << std::endl
759  << std::endl;
760 #endif
761  }
762  }
763  }
764  }
765  }
766 }
767 
768 bool PR2ArmIK::checkJointLimits(const std::vector<double>& joint_values) const
769 {
770  for (int i = 0; i < NUM_JOINTS_ARM7DOF; i++)
771  {
772  if (!checkJointLimits(angles::normalize_angle(joint_values[i] * angle_multipliers_[i]), i))
773  {
774  return false;
775  }
776  }
777  return true;
778 }
779 
780 bool PR2ArmIK::checkJointLimits(const double& joint_value, const int& joint_num) const
781 {
782  double jv;
783  if (continuous_joint_[joint_num])
784  jv = angles::normalize_angle(joint_value * angle_multipliers_[joint_num]);
785  else if (joint_num == 2)
786  jv = joint_value * angle_multipliers_[joint_num];
787  else
788  jv = angles::normalize_angle(joint_value * angle_multipliers_[joint_num]);
789 
790  if (jv < min_angles_[joint_num] || jv > max_angles_[joint_num])
791  {
792  // ROS_INFO("Angle %d = %f out of range:
793  // (%f,%f)\n",joint_num,joint_value,min_angles_[joint_num],max_angles_[joint_num]);
794  return false;
795  }
796  return true;
797 }
moveit::core
Core components of MoveIt!
Definition: kinematics_base.h:50
d2
Chain d2()
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
pr2_arm_ik.h
pr2_arm_kinematics
Definition: pr2_arm_ik.h:49
pr2_arm_kinematics::NUM_JOINTS_ARM7DOF
static const int NUM_JOINTS_ARM7DOF
Definition: pr2_arm_ik.h:51
pr2_arm_kinematics::solveQuadratic
bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
Definition: pr2_arm_ik.h:61
M_PI
#define M_PI
y
double y
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
z
double z
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
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)
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
pr2_arm_kinematics::IK_EPS
static const double IK_EPS
Definition: pr2_arm_ik.h:53
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
x
double x
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
pr2_arm_kinematics::solveCosineEqn
bool solveCosineEqn(const double &a, const double &b, const double &c, double &soln1, double &soln2)
Definition: pr2_arm_ik.h:91

moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:05