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::Isometry3f home = Eigen::Isometry3f::Identity();
146  home(0, 3) = shoulder_upperarm_offset_ + upperarm_elbow_offset_ + elbow_wrist_offset_;
147  home_inv_ = home.inverse();
148  grhs_ = home;
149  gf_ = home_inv_;
150  solution_.resize(NUM_JOINTS_ARM7DOF);
151  return true;
152 }
153 
154 void PR2ArmIK::addJointToChainInfo(const 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::Isometry3f& 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::Isometry3f g = g_in;
204  Eigen::Isometry3f gf_local = home_inv_;
205  Eigen::Isometry3f 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 acos_term = numerator / denominator;
251 
252  if (acos_term > 1.0 || acos_term < -1.0)
253  return;
254 
255  double acos_angle = acos(acos_term);
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 (double theta : theta4)
265  {
266  t4 = theta;
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 (double theta : theta2)
288  {
289  t2 = theta;
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 - (shoulder_upperarm_offset_ + cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
302  (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cos(t4))) *
303  sint1;
304  if (!solveCosineEqn(at, bt, ct, theta3[0], theta3[1]))
305  continue;
306 
307  for (double theta : theta3)
308  {
309  t3 = theta;
310 
311  if (!checkJointLimits(angles::normalize_angle(t3), 2))
312  continue;
313 
314  sint3 = sin(t3);
315  cost3 = cos(t3);
316 #ifdef DEBUG
317  std::cout << "t3 " << t3 << std::endl;
318 #endif
319  if (fabs((shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
320  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost4) *
321  sint2 +
322  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost2 * cost3 * sint4 - z) > IK_EPS)
323  continue;
324 
325  if (fabs((shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint1 * sint3 * sint4 +
326  cost1 * (shoulder_upperarm_offset_ +
327  cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
328  (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost4) +
329  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost3 * sint2 * sint4) -
330  x) > IK_EPS)
331  continue;
332 
333  grhs_local(0, 0) =
334  cost4 * (gf_local(0, 0) * cost1 * cost2 + gf_local(1, 0) * cost2 * sint1 - gf_local(2, 0) * sint2) -
335  (gf_local(2, 0) * cost2 * cost3 + cost3 * (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 +
336  (-(gf_local(1, 0) * cost1) + gf_local(0, 0) * sint1) * sint3) *
337  sint4;
338 
339  grhs_local(0, 1) =
340  cost4 * (gf_local(0, 1) * cost1 * cost2 + gf_local(1, 1) * cost2 * sint1 - gf_local(2, 1) * sint2) -
341  (gf_local(2, 1) * cost2 * cost3 + cost3 * (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 +
342  (-(gf_local(1, 1) * cost1) + gf_local(0, 1) * sint1) * sint3) *
343  sint4;
344 
345  grhs_local(0, 2) =
346  cost4 * (gf_local(0, 2) * cost1 * cost2 + gf_local(1, 2) * cost2 * sint1 - gf_local(2, 2) * sint2) -
347  (gf_local(2, 2) * cost2 * cost3 + cost3 * (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 +
348  (-(gf_local(1, 2) * cost1) + gf_local(0, 2) * sint1) * sint3) *
349  sint4;
350 
351  grhs_local(1, 0) = cost3 * (gf_local(1, 0) * cost1 - gf_local(0, 0) * sint1) + gf_local(2, 0) * cost2 * sint3 +
352  (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 * sint3;
353 
354  grhs_local(1, 1) = cost3 * (gf_local(1, 1) * cost1 - gf_local(0, 1) * sint1) + gf_local(2, 1) * cost2 * sint3 +
355  (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 * sint3;
356 
357  grhs_local(1, 2) = cost3 * (gf_local(1, 2) * cost1 - gf_local(0, 2) * sint1) + gf_local(2, 2) * cost2 * sint3 +
358  (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 * sint3;
359 
360  grhs_local(2, 0) =
361  cost4 *
362  (gf_local(2, 0) * cost2 * cost3 + cost3 * (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 +
363  (-(gf_local(1, 0) * cost1) + gf_local(0, 0) * sint1) * sint3) +
364  (gf_local(0, 0) * cost1 * cost2 + gf_local(1, 0) * cost2 * sint1 - gf_local(2, 0) * sint2) * sint4;
365 
366  grhs_local(2, 1) =
367  cost4 *
368  (gf_local(2, 1) * cost2 * cost3 + cost3 * (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 +
369  (-(gf_local(1, 1) * cost1) + gf_local(0, 1) * sint1) * sint3) +
370  (gf_local(0, 1) * cost1 * cost2 + gf_local(1, 1) * cost2 * sint1 - gf_local(2, 1) * sint2) * sint4;
371 
372  grhs_local(2, 2) =
373  cost4 *
374  (gf_local(2, 2) * cost2 * cost3 + cost3 * (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 +
375  (-(gf_local(1, 2) * cost1) + gf_local(0, 2) * sint1) * sint3) +
376  (gf_local(0, 2) * cost1 * cost2 + gf_local(1, 2) * cost2 * sint1 - gf_local(2, 2) * sint2) * sint4;
377 
378  double val1 = sqrt(grhs_local(0, 1) * grhs_local(0, 1) + grhs_local(0, 2) * grhs_local(0, 2));
379  double val2 = grhs_local(0, 0);
380 
381  theta6[0] = atan2(val1, val2);
382  theta6[1] = atan2(-val1, val2);
383 
384  // theta6[3] = M_PI + theta6[0];
385  // theta6[4] = M_PI + theta6[1];
386 
387  for (int mm = 0; mm < 2; mm++)
388  {
389  t6 = theta6[mm];
390  if (!checkJointLimits(angles::normalize_angle(t6), 5))
391  continue;
392 
393 #ifdef DEBUG
394  std::cout << "t6 " << t6 << std::endl;
395 #endif
396  if (fabs(cos(t6) - grhs_local(0, 0)) > IK_EPS)
397  continue;
398 
399  if (fabs(sin(t6)) < IK_EPS)
400  {
401  // std::cout << "Singularity" << std::endl;
402  theta5[0] = acos(grhs_local(1, 1)) / 2.0;
403  theta7[0] = theta7[0];
404  theta7[1] = M_PI + theta7[0];
405  theta5[1] = theta7[1];
406  }
407  else
408  {
409  theta7[0] = atan2(grhs_local(0, 1), grhs_local(0, 2));
410  theta5[0] = atan2(grhs_local(1, 0), -grhs_local(2, 0));
411  theta7[1] = M_PI + theta7[0];
412  theta5[1] = M_PI + theta5[0];
413  }
414 #ifdef DEBUG
415  std::cout << "theta1: " << t1 << std::endl;
416  std::cout << "theta2: " << t2 << std::endl;
417  std::cout << "theta3: " << t3 << std::endl;
418  std::cout << "theta4: " << t4 << std::endl;
419  std::cout << "theta5: " << t5 << std::endl;
420  std::cout << "theta6: " << t6 << std::endl;
421  std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
422 #endif
423  for (int lll = 0; lll < 2; lll++)
424  {
425  t5 = theta5[lll];
426  t7 = theta7[lll];
427  if (!checkJointLimits(t5, 4))
428  continue;
429  if (!checkJointLimits(t7, 6))
430  continue;
431 
432 #ifdef DEBUG
433  std::cout << "t5" << t5 << std::endl;
434  std::cout << "t7" << t7 << std::endl;
435 #endif
436  if (fabs(sin(t6) * sin(t7) - grhs_local(0, 1)) > IK_EPS ||
437  fabs(cos(t7) * sin(t6) - grhs_local(0, 2)) > IK_EPS)
438  continue;
439 
440  solution_ik[0] = normalize_angle(t1) * angle_multipliers_[0];
441  solution_ik[1] = normalize_angle(t2) * angle_multipliers_[1];
442  solution_ik[2] = normalize_angle(t3) * angle_multipliers_[2];
443  solution_ik[3] = normalize_angle(t4) * angle_multipliers_[3];
444  solution_ik[4] = normalize_angle(t5) * angle_multipliers_[4];
445  solution_ik[5] = normalize_angle(t6) * angle_multipliers_[5];
446  solution_ik[6] = normalize_angle(t7) * angle_multipliers_[6];
447  solution.push_back(solution_ik);
448 
449 #ifdef DEBUG
450  std::cout << "SOLN " << solution_ik[0] << " " << solution_ik[1] << " " << solution_ik[2] << " "
451  << solution_ik[3] << " " << solution_ik[4] << " " << solution_ik[5] << " " << solution_ik[6]
452  << std::endl
453  << std::endl;
454 #endif
455  }
456  }
457  }
458  }
459  }
460 }
461 
462 void PR2ArmIK::computeIKShoulderRoll(const Eigen::Isometry3f& g_in, const double& t3,
463  std::vector<std::vector<double> >& solution) const
464 {
465  std::vector<double> solution_ik(NUM_JOINTS_ARM7DOF, 0.0);
466  // ROS_INFO(" ");
467  // solution_ik_.clear();
468  // ROS_INFO("Solution IK size: %d",solution_ik_.size());
469  // for(unsigned int i=0; i < solution_ik_.size(); i++)
470  // {
471  // solution_ik_[i].clear();
472  // }
473  // if(!solution_ik_.empty())
474  // solution_ik_.resize(0);
475  // t3 = shoulder/turret roll is specified
476  Eigen::Isometry3f g = g_in;
477  Eigen::Isometry3f gf_local = home_inv_;
478  Eigen::Isometry3f grhs_local = home_inv_;
479  // First bring everything into the arm frame
480  g(0, 3) = g_in(0, 3) - torso_shoulder_offset_x_;
481  g(1, 3) = g_in(1, 3) - torso_shoulder_offset_y_;
482  g(2, 3) = g_in(2, 3) - torso_shoulder_offset_z_;
483 
484  if (!checkJointLimits(t3, 2))
485  {
486  return;
487  }
488  double x = g(0, 3);
489  double y = g(1, 3);
490  double z = g(2, 3);
491  double cost1, cost2, cost3, cost4;
492  double sint1, sint2, sint3, sint4;
493 
494  gf_local = g * home_inv_;
495 
496  cost3 = cos(t3);
497  sint3 = sin(t3);
498 
499  double t1(0), t2(0), t4(0), t5(0), t6(0), t7(0);
500 
501  double at(0), bt(0), ct(0);
502 
503  double theta1[2], theta2[2], theta4[4], theta5[2], theta6[4], theta7[2];
504 
505  double c0 = -sin(-t3) * elbow_wrist_offset_;
506  double c1 = -cos(-t3) * elbow_wrist_offset_;
507 
508  double d0 = 4 * shoulder_upperarm_offset_ * shoulder_upperarm_offset_ *
509  (upperarm_elbow_offset_ * upperarm_elbow_offset_ + c1 * c1 - z * z);
510  double d1 = 8 * shoulder_upperarm_offset_ * shoulder_upperarm_offset_ * upperarm_elbow_offset_ * elbow_wrist_offset_;
511  double d2 =
512  4 * shoulder_upperarm_offset_ * shoulder_upperarm_offset_ * (elbow_wrist_offset_ * elbow_wrist_offset_ - c1 * c1);
513 
514  double b0 = x * x + y * y + z * z - shoulder_upperarm_offset_ * shoulder_upperarm_offset_ -
515  upperarm_elbow_offset_ * upperarm_elbow_offset_ - c0 * c0 - c1 * c1;
516  double b1 = -2 * upperarm_elbow_offset_ * elbow_wrist_offset_;
517 
518  if (!solveQuadratic(b1 * b1 - d2, 2 * b0 * b1 - d1, b0 * b0 - d0, &theta4[0], &theta4[1]))
519  {
520 #ifdef DEBUG
521  printf("No solution to quadratic eqn\n");
522 #endif
523  return;
524  }
525  theta4[0] = acos(theta4[0]);
526  theta4[2] = acos(theta4[1]);
527  theta4[1] = -theta4[0];
528  theta4[3] = -theta4[2];
529 
530  for (double theta : theta4)
531  {
532  t4 = theta;
533 
534  if (!checkJointLimits(t4, 3))
535  {
536  continue;
537  }
538  cost4 = cos(t4);
539  sint4 = sin(t4);
540 #ifdef DEBUG
541  std::cout << "t4 " << t4 << std::endl;
542 #endif
543  if (std::isnan(t4))
544  continue;
545  at = cos(t3) * sin(t4) * (shoulder_elbow_offset_ - shoulder_wrist_offset_);
546  bt = (shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
547  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cos(t4));
548  ct = z;
549 
550  if (!solveCosineEqn(at, bt, ct, theta2[0], theta2[1]))
551  continue;
552 
553  for (double theta : theta2)
554  {
555  t2 = theta;
556 #ifdef DEBUG
557  std::cout << "t2 " << t2 << std::endl;
558 #endif
559  if (!checkJointLimits(t2, 1))
560  {
561  continue;
562  }
563 
564  sint2 = sin(t2);
565  cost2 = cos(t2);
566 
567  at = -y;
568  bt = x;
569  ct = (shoulder_elbow_offset_ - shoulder_wrist_offset_) * sin(t3) * sin(t4);
570  if (!solveCosineEqn(at, bt, ct, theta1[0], theta1[1]))
571  {
572 #ifdef DEBUG
573  std::cout << "could not solve cosine equation for t1" << std::endl;
574 #endif
575  continue;
576  }
577 
578  for (double theta : theta1)
579  {
580  t1 = theta;
581 #ifdef DEBUG
582  std::cout << "t1 " << t1 << std::endl;
583 #endif
584  if (!checkJointLimits(t1, 0))
585  {
586  continue;
587  }
588  sint1 = sin(t1);
589  cost1 = cos(t1);
590  if (fabs((shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
591  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost4) *
592  sint2 +
593  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost2 * cost3 * sint4 - z) > IK_EPS)
594  {
595 #ifdef DEBUG
596  printf("z value not matched %f\n",
597  fabs((shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
598  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost4) *
599  sint2 +
600  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost2 * cost3 * sint4 - z));
601 #endif
602  continue;
603  }
604  if (fabs((shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint1 * sint3 * sint4 +
605  cost1 * (shoulder_upperarm_offset_ +
606  cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
607  (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost4) +
608  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost3 * sint2 * sint4) -
609  x) > IK_EPS)
610  {
611 #ifdef DEBUG
612  printf("x value not matched by %f\n",
613  fabs((shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint1 * sint3 * sint4 +
614  cost1 * (shoulder_upperarm_offset_ +
615  cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
616  (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost4) +
617  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost3 * sint2 * sint4) -
618  x));
619 #endif
620  continue;
621  }
622  if (fabs(-(shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost1 * sint3 * sint4 +
623  sint1 * (shoulder_upperarm_offset_ +
624  cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
625  (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost4) +
626  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost3 * sint2 * sint4) -
627  y) > IK_EPS)
628  {
629 #ifdef DEBUG
630  printf("y value not matched\n");
631 #endif
632  continue;
633  }
634  grhs_local(0, 0) =
635  cost4 * (gf_local(0, 0) * cost1 * cost2 + gf_local(1, 0) * cost2 * sint1 - gf_local(2, 0) * sint2) -
636  (gf_local(2, 0) * cost2 * cost3 + cost3 * (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 +
637  (-(gf_local(1, 0) * cost1) + gf_local(0, 0) * sint1) * sint3) *
638  sint4;
639 
640  grhs_local(0, 1) =
641  cost4 * (gf_local(0, 1) * cost1 * cost2 + gf_local(1, 1) * cost2 * sint1 - gf_local(2, 1) * sint2) -
642  (gf_local(2, 1) * cost2 * cost3 + cost3 * (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 +
643  (-(gf_local(1, 1) * cost1) + gf_local(0, 1) * sint1) * sint3) *
644  sint4;
645 
646  grhs_local(0, 2) =
647  cost4 * (gf_local(0, 2) * cost1 * cost2 + gf_local(1, 2) * cost2 * sint1 - gf_local(2, 2) * sint2) -
648  (gf_local(2, 2) * cost2 * cost3 + cost3 * (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 +
649  (-(gf_local(1, 2) * cost1) + gf_local(0, 2) * sint1) * sint3) *
650  sint4;
651 
652  grhs_local(1, 0) = cost3 * (gf_local(1, 0) * cost1 - gf_local(0, 0) * sint1) + gf_local(2, 0) * cost2 * sint3 +
653  (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 * sint3;
654 
655  grhs_local(1, 1) = cost3 * (gf_local(1, 1) * cost1 - gf_local(0, 1) * sint1) + gf_local(2, 1) * cost2 * sint3 +
656  (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 * sint3;
657 
658  grhs_local(1, 2) = cost3 * (gf_local(1, 2) * cost1 - gf_local(0, 2) * sint1) + gf_local(2, 2) * cost2 * sint3 +
659  (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 * sint3;
660 
661  grhs_local(2, 0) =
662  cost4 *
663  (gf_local(2, 0) * cost2 * cost3 + cost3 * (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 +
664  (-(gf_local(1, 0) * cost1) + gf_local(0, 0) * sint1) * sint3) +
665  (gf_local(0, 0) * cost1 * cost2 + gf_local(1, 0) * cost2 * sint1 - gf_local(2, 0) * sint2) * sint4;
666 
667  grhs_local(2, 1) =
668  cost4 *
669  (gf_local(2, 1) * cost2 * cost3 + cost3 * (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 +
670  (-(gf_local(1, 1) * cost1) + gf_local(0, 1) * sint1) * sint3) +
671  (gf_local(0, 1) * cost1 * cost2 + gf_local(1, 1) * cost2 * sint1 - gf_local(2, 1) * sint2) * sint4;
672 
673  grhs_local(2, 2) =
674  cost4 *
675  (gf_local(2, 2) * cost2 * cost3 + cost3 * (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 +
676  (-(gf_local(1, 2) * cost1) + gf_local(0, 2) * sint1) * sint3) +
677  (gf_local(0, 2) * cost1 * cost2 + gf_local(1, 2) * cost2 * sint1 - gf_local(2, 2) * sint2) * sint4;
678 
679  double val1 = sqrt(grhs_local(0, 1) * grhs_local(0, 1) + grhs_local(0, 2) * grhs_local(0, 2));
680  double val2 = grhs_local(0, 0);
681 
682  theta6[0] = atan2(val1, val2);
683  theta6[1] = atan2(-val1, val2);
684 
685  for (int mm = 0; mm < 2; mm++)
686  {
687  t6 = theta6[mm];
688 #ifdef DEBUG
689  std::cout << "t6 " << t6 << std::endl;
690 #endif
691  if (!checkJointLimits(t6, 5))
692  {
693  continue;
694  }
695 
696  if (fabs(cos(t6) - grhs_local(0, 0)) > IK_EPS)
697  continue;
698 
699  if (fabs(sin(t6)) < IK_EPS)
700  {
701  // std::cout << "Singularity" << std::endl;
702  theta5[0] = acos(grhs_local(1, 1)) / 2.0;
703  theta7[0] = theta5[0];
704  // theta7[1] = M_PI+theta7[0];
705  // theta5[1] = theta7[1];
706  }
707  else
708  {
709  theta7[0] = atan2(grhs_local(0, 1) / sin(t6), grhs_local(0, 2) / sin(t6));
710  theta5[0] = atan2(grhs_local(1, 0) / sin(t6), -grhs_local(2, 0) / sin(t6));
711  // theta7[1] = M_PI+theta7[0];
712  // theta5[1] = M_PI+theta5[0];
713  }
714  for (int lll = 0; lll < 1; lll++)
715  {
716  t5 = theta5[lll];
717  t7 = theta7[lll];
718 
719  if (!checkJointLimits(t5, 4))
720  {
721  continue;
722  }
723  if (!checkJointLimits(t7, 6))
724  {
725  continue;
726  }
727 
728 #ifdef DEBUG
729  std::cout << "t5 " << t5 << std::endl;
730  std::cout << "t7 " << t7 << std::endl;
731 #endif
732  // if(fabs(sin(t6)*sin(t7)-grhs_local(0,1)) > IK_EPS || fabs(cos(t7)*sin(t6)-grhs_local(0,2)) > IK_EPS)
733  // continue;
734 
735 #ifdef DEBUG
736  std::cout << "theta1: " << t1 << std::endl;
737  std::cout << "theta2: " << t2 << std::endl;
738  std::cout << "theta3: " << t3 << std::endl;
739  std::cout << "theta4: " << t4 << std::endl;
740  std::cout << "theta5: " << t5 << std::endl;
741  std::cout << "theta6: " << t6 << std::endl;
742  std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
743 #endif
744 
745  solution_ik[0] = normalize_angle(t1 * angle_multipliers_[0]);
746  solution_ik[1] = normalize_angle(t2 * angle_multipliers_[1]);
747  solution_ik[2] = t3 * angle_multipliers_[2];
748  solution_ik[3] = normalize_angle(t4 * angle_multipliers_[3]);
749  solution_ik[4] = normalize_angle(t5 * angle_multipliers_[4]);
750  solution_ik[5] = normalize_angle(t6 * angle_multipliers_[5]);
751  solution_ik[6] = normalize_angle(t7 * angle_multipliers_[6]);
752  solution.push_back(solution_ik);
753 #ifdef DEBUG
754  std::cout << "SOLN " << solution_ik[0] << " " << solution_ik[1] << " " << solution_ik[2] << " "
755  << solution_ik[3] << " " << solution_ik[4] << " " << solution_ik[5] << " " << solution_ik[6]
756  << std::endl
757  << std::endl;
758 #endif
759  }
760  }
761  }
762  }
763  }
764 }
765 
766 bool PR2ArmIK::checkJointLimits(const std::vector<double>& joint_values) const
767 {
768  for (int i = 0; i < NUM_JOINTS_ARM7DOF; i++)
769  {
770  if (!checkJointLimits(angles::normalize_angle(joint_values[i] * angle_multipliers_[i]), i))
771  {
772  return false;
773  }
774  }
775  return true;
776 }
777 
778 bool PR2ArmIK::checkJointLimits(const double& joint_value, const int& joint_num) const
779 {
780  double jv;
781  if (continuous_joint_[joint_num])
782  jv = angles::normalize_angle(joint_value * angle_multipliers_[joint_num]);
783  else if (joint_num == 2)
784  jv = joint_value * angle_multipliers_[joint_num];
785  else
786  jv = angles::normalize_angle(joint_value * angle_multipliers_[joint_num]);
787 
788  return !(jv < min_angles_[joint_num] || jv > max_angles_[joint_num]);
789 }
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
pr2_arm_kinematics
Definition: pr2_arm_ik.h:48
pr2_arm_kinematics::solveCosineEqn
bool solveCosineEqn(const double &a, const double &b, const double &c, double &soln1, double &soln2)
Definition: pr2_arm_ik.h:122
angles
pr2_arm_kinematics::NUM_JOINTS_ARM7DOF
static const int NUM_JOINTS_ARM7DOF
Definition: pr2_arm_ik.h:82
angles::normalize_angle
def normalize_angle(angle)
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
pr2_arm_kinematics::solveQuadratic
bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
Definition: pr2_arm_ik.h:92
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
y
double y
M_PI
#define M_PI
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
x
double x
pr2_arm_kinematics::IK_EPS
static const double IK_EPS
Definition: pr2_arm_ik.h:84
pr2_arm_ik.h
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:86
z
double z
angles.h

moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Nov 21 2024 03:23:41