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