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