37 #include <angles/angles.h> 56 bool PR2ArmIK::init(
const urdf::ModelInterface&
robot_model,
const std::string& root_name,
const std::string& tip_name)
58 std::vector<urdf::Pose> link_offset;
60 urdf::LinkConstSharedPtr link = robot_model.getLink(tip_name);
61 while (link && num_joints < 7)
63 urdf::JointConstSharedPtr joint;
64 if (link->parent_joint)
65 joint = robot_model.getJoint(link->parent_joint->name);
68 if (link->parent_joint)
69 ROS_ERROR_NAMED(
"pr2_arm_kinematics_plugin",
"Could not find joint: %s", link->parent_joint->name.c_str());
71 ROS_ERROR_NAMED(
"pr2_arm_kinematics_plugin",
"Link %s has no parent joint", link->name.c_str());
74 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
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)
85 min_angles_.push_back(joint->safety->soft_lower_limit);
86 max_angles_.push_back(joint->safety->soft_upper_limit);
92 min_angles_.push_back(joint->limits->lower);
93 max_angles_.push_back(joint->limits->upper);
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());
102 continuous_joint_.push_back(
false);
106 min_angles_.push_back(-
M_PI);
107 max_angles_.push_back(
M_PI);
108 continuous_joint_.push_back(
true);
110 addJointToChainInfo(link->parent_joint, solver_info_);
113 link = robot_model.getLink(link->getParent()->name);
116 solver_info_.link_names.push_back(tip_name);
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());
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());
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_;
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);
154 void PR2ArmIK::addJointToChainInfo(urdf::JointConstSharedPtr joint, moveit_msgs::KinematicSolverInfo& info)
156 moveit_msgs::JointLimits limit;
157 info.joint_names.push_back(joint->name);
159 if (joint->type != urdf::Joint::CONTINUOUS)
163 limit.min_position = joint->safety->soft_lower_limit;
164 limit.max_position = joint->safety->soft_upper_limit;
165 limit.has_position_limits =
true;
167 else if (joint->limits)
169 limit.min_position = joint->limits->lower;
170 limit.max_position = joint->limits->upper;
171 limit.has_position_limits =
true;
174 limit.has_position_limits =
false;
178 limit.min_position = -
M_PI;
179 limit.max_position =
M_PI;
180 limit.has_position_limits =
false;
184 limit.max_velocity = joint->limits->velocity;
185 limit.has_velocity_limits = 1;
188 limit.has_velocity_limits = 0;
189 info.limits.push_back(limit);
192 void PR2ArmIK::getSolverInfo(moveit_msgs::KinematicSolverInfo& info)
197 void PR2ArmIK::computeIKShoulderPan(
const Eigen::Affine3f& g_in,
const double& t1_in,
198 std::vector<std::vector<double> >& solution)
const 203 Eigen::Affine3f g = g_in;
204 Eigen::Affine3f gf_local = home_inv_;
205 Eigen::Affine3f grhs_local = home_inv_;
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_;
211 double t1 = angles::normalize_angle(t1_in);
212 if (!checkJointLimits(t1, 0))
215 double cost1, cost2, cost3, cost4;
216 double sint1, sint2, sint3, sint4;
218 gf_local = g * home_inv_;
223 double t2(0), t3(0), t4(0), t5(0), t6(0), t7(0);
225 double at(0), bt(0), ct(0);
227 double theta2[2], theta3[2], theta4[2], theta5[2], theta6[4], theta7[2];
229 double sopx = shoulder_upperarm_offset_ * cost1;
230 double sopy = shoulder_upperarm_offset_ * sint1;
237 double dx = x - sopx;
238 double dy = y - sopy;
239 double dz = z - sopz;
241 double dd = dx * dx + dy * dy + dz * dz;
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_;
248 2 * (shoulder_upperarm_offset_ - shoulder_elbow_offset_) * (shoulder_elbow_offset_ - shoulder_wrist_offset_);
250 double acosTerm = numerator / denominator;
252 if (acosTerm > 1.0 || acosTerm < -1.0)
255 double acos_angle =
acos(acosTerm);
257 theta4[0] = acos_angle;
258 theta4[1] = -acos_angle;
261 std::cout <<
"ComputeIK::theta3:" << numerator <<
"," << denominator <<
"," << std::endl << theta4[0] << std::endl;
264 for (
int jj = 0; jj < 2; jj++)
271 std::cout <<
"t4 " << t4 << std::endl;
276 if (!checkJointLimits(t4, 3))
279 at = x * cost1 + y * sint1 - shoulder_upperarm_offset_;
281 ct = -shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
282 (shoulder_wrist_offset_ - shoulder_elbow_offset_) *
cos(t4);
287 for (
int ii = 0; ii < 2; ii++)
290 if (!checkJointLimits(t2, 1))
294 std::cout <<
"t2 " << t2 << std::endl;
299 at = sint1 * (shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint2 * sint4;
300 bt = (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost1 * sint4;
302 (shoulder_upperarm_offset_ +
303 cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
304 (-shoulder_elbow_offset_ + shoulder_wrist_offset_) *
cos(t4))) *
309 for (
int kk = 0; kk < 2; kk++)
313 if (!checkJointLimits(angles::normalize_angle(t3), 2))
319 std::cout <<
"t3 " << t3 << std::endl;
321 if (fabs((shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
322 (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost4) *
324 (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost2 * cost3 * sint4 - z) >
IK_EPS)
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) -
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) *
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) *
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) *
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;
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;
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;
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;
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;
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;
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);
383 theta6[0] =
atan2(val1, val2);
384 theta6[1] =
atan2(-val1, val2);
389 for (
int mm = 0; mm < 2; mm++)
392 if (!checkJointLimits(angles::normalize_angle(t6), 5))
396 std::cout <<
"t6 " << t6 << std::endl;
398 if (fabs(
cos(t6) - grhs_local(0, 0)) >
IK_EPS)
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];
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];
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;
425 for (
int lll = 0; lll < 2; lll++)
429 if (!checkJointLimits(t5, 4))
431 if (!checkJointLimits(t7, 6))
435 std::cout <<
"t5" << t5 << std::endl;
436 std::cout <<
"t7" << t7 << std::endl;
438 if (fabs(
sin(t6) *
sin(t7) - grhs_local(0, 1)) >
IK_EPS ||
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);
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]
464 void PR2ArmIK::computeIKShoulderRoll(
const Eigen::Affine3f& g_in,
const double& t3,
465 std::vector<std::vector<double> >& solution)
const 478 Eigen::Affine3f g = g_in;
479 Eigen::Affine3f gf_local = home_inv_;
480 Eigen::Affine3f grhs_local = home_inv_;
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_;
486 if (!checkJointLimits(t3, 2))
493 double cost1, cost2, cost3, cost4;
494 double sint1, sint2, sint3, sint4;
496 gf_local = g * home_inv_;
501 double t1(0), t2(0), t4(0), t5(0), t6(0), t7(0);
503 double at(0), bt(0), ct(0);
505 double theta1[2], theta2[2], theta4[4], theta5[2], theta6[4], theta7[2];
507 double c0 = -
sin(-t3) * elbow_wrist_offset_;
508 double c1 = -
cos(-t3) * elbow_wrist_offset_;
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_;
514 4 * shoulder_upperarm_offset_ * shoulder_upperarm_offset_ * (elbow_wrist_offset_ * elbow_wrist_offset_ - c1 * c1);
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_;
520 if (!
solveQuadratic(b1 * b1 - d2, 2 * b0 * b1 - d1, b0 * b0 - d0, &theta4[0], &theta4[1]))
523 printf(
"No solution to quadratic eqn\n");
527 theta4[0] =
acos(theta4[0]);
528 theta4[2] =
acos(theta4[1]);
529 theta4[1] = -theta4[0];
530 theta4[3] = -theta4[2];
532 for (
int jj = 0; jj < 4; jj++)
536 if (!checkJointLimits(t4, 3))
543 std::cout <<
"t4 " << t4 << std::endl;
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));
555 for (
int ii = 0; ii < 2; ii++)
559 std::cout <<
"t2 " << t2 << std::endl;
561 if (!checkJointLimits(t2, 1))
571 ct = (shoulder_elbow_offset_ - shoulder_wrist_offset_) *
sin(t3) *
sin(t4);
575 std::cout <<
"could not solve cosine equation for t1" << std::endl;
580 for (
int kk = 0; kk < 2; kk++)
584 std::cout <<
"t1 " << t1 << std::endl;
586 if (!checkJointLimits(t1, 0))
592 if (fabs((shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
593 (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost4) *
595 (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost2 * cost3 * sint4 - z) >
IK_EPS)
598 printf(
"z value not matched %f\n",
599 fabs((shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
600 (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost4) *
602 (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost2 * cost3 * sint4 - z));
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) -
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) -
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) -
632 printf(
"y value not matched\n");
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) *
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) *
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) *
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;
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;
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;
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;
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;
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;
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);
684 theta6[0] =
atan2(val1, val2);
685 theta6[1] =
atan2(-val1, val2);
687 for (
int mm = 0; mm < 2; mm++)
691 std::cout <<
"t6 " << t6 << std::endl;
693 if (!checkJointLimits(t6, 5))
698 if (fabs(
cos(t6) - grhs_local(0, 0)) >
IK_EPS)
704 theta5[0] =
acos(grhs_local(1, 1)) / 2.0;
705 theta7[0] = theta5[0];
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));
716 for (
int lll = 0; lll < 1; lll++)
721 if (!checkJointLimits(t5, 4))
725 if (!checkJointLimits(t7, 6))
731 std::cout <<
"t5 " << t5 << std::endl;
732 std::cout <<
"t7 " << t7 << std::endl;
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;
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);
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]
768 bool PR2ArmIK::checkJointLimits(
const std::vector<double>& joint_values)
const 772 if (!checkJointLimits(angles::normalize_angle(joint_values[i] * angle_multipliers_[i]), i))
780 bool PR2ArmIK::checkJointLimits(
const double& joint_value,
const int& joint_num)
const 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];
788 jv = angles::normalize_angle(joint_value * angle_multipliers_[joint_num]);
790 if (jv < min_angles_[joint_num] || jv > max_angles_[joint_num])
Core components of MoveIt!
#define ROS_WARN_NAMED(name,...)
static const int NUM_JOINTS_ARM7DOF
bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
#define ROS_DEBUG_NAMED(name,...)
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)
#define ROS_ERROR_NAMED(name,...)
static const double IK_EPS
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
double distance(const urdf::Pose &transform)
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)