00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 #include <angles/angles.h>
00034 #include <console_bridge/console.h>
00035 #include "pr2_arm_ik.h"
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 using namespace angles;
00047 using namespace pr2_arm_kinematics;
00048
00049 PR2ArmIK::PR2ArmIK()
00050 {
00051 }
00052
00053 bool PR2ArmIK::init(const urdf::ModelInterface &robot_model, const std::string &root_name, const std::string &tip_name)
00054 {
00055 std::vector<urdf::Pose> link_offset;
00056 int num_joints = 0;
00057 boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
00058 while(link && num_joints < 7)
00059 {
00060 boost::shared_ptr<const urdf::Joint> joint;
00061 if (link->parent_joint)
00062 joint = robot_model.getJoint(link->parent_joint->name);
00063 if(!joint)
00064 {
00065 if (link->parent_joint)
00066 logError("Could not find joint: %s",link->parent_joint->name.c_str());
00067 else
00068 logError("Link %s has no parent joint",link->name.c_str());
00069 return false;
00070 }
00071 if(joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00072 {
00073 link_offset.push_back(link->parent_joint->parent_to_joint_origin_transform);
00074 angle_multipliers_.push_back(joint->axis.x*fabs(joint->axis.x) + joint->axis.y*fabs(joint->axis.y) + joint->axis.z*fabs(joint->axis.z));
00075 logDebug("Joint axis: %d, %f, %f, %f",6-num_joints,joint->axis.x,joint->axis.y,joint->axis.z);
00076 if(joint->type != urdf::Joint::CONTINUOUS)
00077 {
00078 if (joint->safety)
00079 {
00080 min_angles_.push_back(joint->safety->soft_lower_limit);
00081 max_angles_.push_back(joint->safety->soft_upper_limit);
00082 }
00083 else
00084 {
00085 if (joint->limits)
00086 {
00087 min_angles_.push_back(joint->limits->lower);
00088 max_angles_.push_back(joint->limits->upper);
00089 }
00090 else
00091 {
00092 min_angles_.push_back(0.0);
00093 max_angles_.push_back(0.0);
00094 logWarn("No joint limits or joint '%s'",joint->name.c_str());
00095 }
00096 }
00097 continuous_joint_.push_back(false);
00098 }
00099 else
00100 {
00101 min_angles_.push_back(-M_PI);
00102 max_angles_.push_back(M_PI);
00103 continuous_joint_.push_back(true);
00104 }
00105 addJointToChainInfo(link->parent_joint,solver_info_);
00106 num_joints++;
00107 }
00108 link = robot_model.getLink(link->getParent()->name);
00109 }
00110
00111 solver_info_.link_names.push_back(tip_name);
00112
00113
00114
00115 std::reverse(angle_multipliers_.begin(),angle_multipliers_.end());
00116 std::reverse(min_angles_.begin(),min_angles_.end());
00117 std::reverse(max_angles_.begin(),max_angles_.end());
00118 std::reverse(link_offset.begin(),link_offset.end());
00119 std::reverse(solver_info_.limits.begin(),solver_info_.limits.end());
00120 std::reverse(solver_info_.joint_names.begin(),solver_info_.joint_names.end());
00121 std::reverse(solver_info_.link_names.begin(),solver_info_.link_names.end());
00122 std::reverse(continuous_joint_.begin(),continuous_joint_.end());
00123
00124 if(num_joints != 7)
00125 {
00126 logError("PR2ArmIK:: Chain from %s to %s does not have 7 joints",root_name.c_str(),tip_name.c_str());
00127 return false;
00128 }
00129
00130 torso_shoulder_offset_x_ = link_offset[0].position.x;
00131 torso_shoulder_offset_y_ = link_offset[0].position.y;
00132 torso_shoulder_offset_z_ = link_offset[0].position.z;
00133 shoulder_upperarm_offset_ = distance(link_offset[1]);
00134 upperarm_elbow_offset_ = distance(link_offset[3]);
00135 elbow_wrist_offset_ = distance(link_offset[5]);
00136 shoulder_elbow_offset_ = shoulder_upperarm_offset_ + upperarm_elbow_offset_;
00137 shoulder_wrist_offset_ = shoulder_upperarm_offset_+upperarm_elbow_offset_+elbow_wrist_offset_;
00138
00139 Eigen::Matrix4f home = Eigen::Matrix4f::Identity();
00140 home(0,3) = shoulder_upperarm_offset_ + upperarm_elbow_offset_ + elbow_wrist_offset_;
00141 home_inv_ = home.inverse();
00142 grhs_ = home;
00143 gf_ = home_inv_;
00144 solution_.resize( NUM_JOINTS_ARM7DOF);
00145 return true;
00146 }
00147
00148 void PR2ArmIK::addJointToChainInfo(boost::shared_ptr<const urdf::Joint> joint, moveit_msgs::KinematicSolverInfo &info)
00149 {
00150 moveit_msgs::JointLimits limit;
00151 info.joint_names.push_back(joint->name);
00152
00153 if(joint->type != urdf::Joint::CONTINUOUS)
00154 {
00155 if (joint->safety)
00156 {
00157 limit.min_position = joint->safety->soft_lower_limit;
00158 limit.max_position = joint->safety->soft_upper_limit;
00159 limit.has_position_limits = true;
00160 }
00161 else
00162 if (joint->limits)
00163 {
00164 limit.min_position = joint->limits->lower;
00165 limit.max_position = joint->limits->upper;
00166 limit.has_position_limits = true;
00167 }
00168 else
00169 limit.has_position_limits = false;
00170 }
00171 else
00172 {
00173 limit.min_position = -M_PI;
00174 limit.max_position = M_PI;
00175 limit.has_position_limits = false;
00176 }
00177 if (joint->limits)
00178 {
00179 limit.max_velocity = joint->limits->velocity;
00180 limit.has_velocity_limits = 1;
00181 }
00182 else
00183 limit.has_velocity_limits = 0;
00184 info.limits.push_back(limit);
00185 }
00186
00187 void PR2ArmIK::getSolverInfo(moveit_msgs::KinematicSolverInfo &info)
00188 {
00189 info = solver_info_;
00190 }
00191
00192
00193 void PR2ArmIK::computeIKShoulderPan(const Eigen::Matrix4f &g_in, const double &t1_in, std::vector<std::vector<double> > &solution) const
00194 {
00195
00196
00197 std::vector<double> solution_ik(NUM_JOINTS_ARM7DOF,0.0);
00198 Eigen::Matrix4f g = g_in;
00199 Eigen::Matrix4f gf_local = home_inv_;
00200 Eigen::Matrix4f grhs_local = home_inv_;
00201
00202 g(0,3) = g_in(0,3) - torso_shoulder_offset_x_;
00203 g(1,3) = g_in(1,3) - torso_shoulder_offset_y_;
00204 g(2,3) = g_in(2,3) - torso_shoulder_offset_z_;
00205
00206 double t1 = angles::normalize_angle(t1_in);
00207 if(!checkJointLimits(t1,0))
00208 return;
00209
00210
00211 double cost1, cost2, cost3, cost4;
00212 double sint1, sint2, sint3, sint4;
00213
00214 gf_local = g*home_inv_;
00215
00216 cost1 = cos(t1);
00217 sint1 = sin(t1);
00218
00219 double t2(0), t3(0), t4(0), t5(0), t6(0), t7(0);
00220
00221 double at(0), bt(0), ct(0);
00222
00223 double theta2[2],theta3[2],theta4[2],theta5[2],theta6[4],theta7[2];
00224
00225 double sopx = shoulder_upperarm_offset_*cost1;
00226 double sopy = shoulder_upperarm_offset_*sint1;
00227 double sopz = 0;
00228
00229 double x = g(0,3);
00230 double y = g(1,3);
00231 double z = g(2,3);
00232
00233 double dx = x - sopx;
00234 double dy = y - sopy;
00235 double dz = z - sopz;
00236
00237 double dd = dx*dx + dy*dy + dz*dz;
00238
00239 double numerator = dd-shoulder_upperarm_offset_*shoulder_upperarm_offset_+2*shoulder_upperarm_offset_*shoulder_elbow_offset_-2*shoulder_elbow_offset_*shoulder_elbow_offset_+2*shoulder_elbow_offset_*shoulder_wrist_offset_-shoulder_wrist_offset_*shoulder_wrist_offset_;
00240 double denominator = 2*(shoulder_upperarm_offset_-shoulder_elbow_offset_)*(shoulder_elbow_offset_-shoulder_wrist_offset_);
00241
00242 double acosTerm = numerator/denominator;
00243
00244 if (acosTerm > 1.0 || acosTerm < -1.0)
00245 return;
00246
00247 double acos_angle = acos(acosTerm);
00248
00249 theta4[0] = acos_angle;
00250 theta4[1] = -acos_angle;
00251
00252 #ifdef DEBUG
00253 std::cout << "ComputeIK::theta3:" << numerator << "," << denominator << "," << std::endl << theta4[0] << std::endl;
00254 #endif
00255
00256 for(int jj =0; jj < 2; jj++)
00257 {
00258 t4 = theta4[jj];
00259 cost4 = cos(t4);
00260 sint4 = sin(t4);
00261
00262 #ifdef DEBUG
00263 std::cout << "t4 " << t4 << std::endl;
00264 #endif
00265 if(std::isnan(t4))
00266 continue;
00267
00268 if(!checkJointLimits(t4,3))
00269 continue;
00270
00271 at = x*cost1+y*sint1-shoulder_upperarm_offset_;
00272 bt = -z;
00273 ct = -shoulder_upperarm_offset_ + shoulder_elbow_offset_ + (shoulder_wrist_offset_-shoulder_elbow_offset_)*cos(t4);
00274
00275 if(!solveCosineEqn(at,bt,ct,theta2[0],theta2[1]))
00276 continue;
00277
00278 for(int ii=0; ii < 2; ii++)
00279 {
00280 t2 = theta2[ii];
00281 if(!checkJointLimits(t2,1))
00282 continue;
00283
00284
00285 #ifdef DEBUG
00286 std::cout << "t2 " << t2 << std::endl;
00287 #endif
00288 sint2 = sin(t2);
00289 cost2 = cos(t2);
00290
00291 at = sint1*(shoulder_elbow_offset_ - shoulder_wrist_offset_)*sint2*sint4;
00292 bt = (-shoulder_elbow_offset_+shoulder_wrist_offset_)*cost1*sint4;
00293 ct = y - (shoulder_upperarm_offset_+cost2*(-shoulder_upperarm_offset_+shoulder_elbow_offset_+(-shoulder_elbow_offset_+shoulder_wrist_offset_)*cos(t4)))*sint1;
00294 if(!solveCosineEqn(at,bt,ct,theta3[0],theta3[1]))
00295 continue;
00296
00297 for(int kk =0; kk < 2; kk++)
00298 {
00299 t3 = theta3[kk];
00300
00301 if(!checkJointLimits(angles::normalize_angle(t3),2))
00302 continue;
00303
00304 sint3 = sin(t3);
00305 cost3 = cos(t3);
00306 #ifdef DEBUG
00307 std::cout << "t3 " << t3 << std::endl;
00308 #endif
00309 if(fabs((shoulder_upperarm_offset_-shoulder_elbow_offset_+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost4)*sint2+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost2*cost3*sint4-z) > IK_EPS )
00310 continue;
00311
00312 if(fabs((shoulder_elbow_offset_-shoulder_wrist_offset_)*sint1*sint3*sint4+cost1*(shoulder_upperarm_offset_+cost2*(-shoulder_upperarm_offset_+shoulder_elbow_offset_+(-shoulder_elbow_offset_+shoulder_wrist_offset_)*cost4)+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost3*sint2*sint4) - x) > IK_EPS)
00313 continue;
00314
00315 grhs_local(0,0) = cost4*(gf_local(0,0)*cost1*cost2+gf_local(1,0)*cost2*sint1-gf_local(2,0)*sint2)-(gf_local(2,0)*cost2*cost3 + cost3*(gf_local(0,0)*cost1 + gf_local(1,0)*sint1)*sint2 + (-(gf_local(1,0)*cost1) + gf_local(0,0)*sint1)*sint3)*sint4;
00316
00317 grhs_local(0,1) = cost4*(gf_local(0,1)*cost1*cost2 + gf_local(1,1)*cost2*sint1 - gf_local(2,1)*sint2) - (gf_local(2,1)*cost2*cost3 + cost3*(gf_local(0,1)*cost1 + gf_local(1,1)*sint1)*sint2 + (-(gf_local(1,1)*cost1) + gf_local(0,1)*sint1)*sint3)*sint4;
00318
00319 grhs_local(0,2) = cost4*(gf_local(0,2)*cost1*cost2 + gf_local(1,2)*cost2*sint1 - gf_local(2,2)*sint2) - (gf_local(2,2)*cost2*cost3 + cost3*(gf_local(0,2)*cost1 + gf_local(1,2)*sint1)*sint2 + (-(gf_local(1,2)*cost1) + gf_local(0,2)*sint1)*sint3)*sint4;
00320
00321 grhs_local(1,0) = cost3*(gf_local(1,0)*cost1 - gf_local(0,0)*sint1) + gf_local(2,0)*cost2*sint3 + (gf_local(0,0)*cost1 + gf_local(1,0)*sint1)*sint2*sint3;
00322
00323 grhs_local(1,1) = cost3*(gf_local(1,1)*cost1 - gf_local(0,1)*sint1) + gf_local(2,1)*cost2*sint3 + (gf_local(0,1)*cost1 + gf_local(1,1)*sint1)*sint2*sint3;
00324
00325 grhs_local(1,2) = cost3*(gf_local(1,2)*cost1 - gf_local(0,2)*sint1) + gf_local(2,2)*cost2*sint3 + (gf_local(0,2)*cost1 + gf_local(1,2)*sint1)*sint2*sint3;
00326
00327 grhs_local(2,0) = cost4*(gf_local(2,0)*cost2*cost3 + cost3*(gf_local(0,0)*cost1 + gf_local(1,0)*sint1)*sint2 + (-(gf_local(1,0)*cost1) + gf_local(0,0)*sint1)*sint3) + (gf_local(0,0)*cost1*cost2 + gf_local(1,0)*cost2*sint1 - gf_local(2,0)*sint2)*sint4;
00328
00329 grhs_local(2,1) = cost4*(gf_local(2,1)*cost2*cost3 + cost3*(gf_local(0,1)*cost1 + gf_local(1,1)*sint1)*sint2 + (-(gf_local(1,1)*cost1) + gf_local(0,1)*sint1)*sint3) + (gf_local(0,1)*cost1*cost2 + gf_local(1,1)*cost2*sint1 - gf_local(2,1)*sint2)*sint4;
00330
00331 grhs_local(2,2) = cost4*(gf_local(2,2)*cost2*cost3 + cost3*(gf_local(0,2)*cost1 + gf_local(1,2)*sint1)*sint2 + (-(gf_local(1,2)*cost1) + gf_local(0,2)*sint1)*sint3) + (gf_local(0,2)*cost1*cost2 + gf_local(1,2)*cost2*sint1 - gf_local(2,2)*sint2)*sint4;
00332
00333
00334 double val1 = sqrt(grhs_local(0,1)*grhs_local(0,1)+grhs_local(0,2)*grhs_local(0,2));
00335 double val2 = grhs_local(0,0);
00336
00337 theta6[0] = atan2(val1,val2);
00338 theta6[1] = atan2(-val1,val2);
00339
00340
00341
00342
00343 for(int mm = 0; mm < 2; mm++)
00344 {
00345 t6 = theta6[mm];
00346 if(!checkJointLimits(angles::normalize_angle(t6),5))
00347 continue;
00348
00349 #ifdef DEBUG
00350 std::cout << "t6 " << t6 << std::endl;
00351 #endif
00352 if(fabs(cos(t6) - grhs_local(0,0)) > IK_EPS)
00353 continue;
00354
00355 if(fabs(sin(t6)) < IK_EPS)
00356 {
00357
00358 theta5[0] = acos(grhs_local(1,1))/2.0;
00359 theta7[0] = theta7[0];
00360 theta7[1] = M_PI+theta7[0];
00361 theta5[1] = theta7[1];
00362 }
00363 else
00364 {
00365 theta7[0] = atan2(grhs_local(0,1),grhs_local(0,2));
00366 theta5[0] = atan2(grhs_local(1,0),-grhs_local(2,0));
00367 theta7[1] = M_PI+theta7[0];
00368 theta5[1] = M_PI+theta5[0];
00369 }
00370 #ifdef DEBUG
00371 std::cout << "theta1: " << t1 << std::endl;
00372 std::cout << "theta2: " << t2 << std::endl;
00373 std::cout << "theta3: " << t3 << std::endl;
00374 std::cout << "theta4: " << t4 << std::endl;
00375 std::cout << "theta5: " << t5 << std::endl;
00376 std::cout << "theta6: " << t6 << std::endl;
00377 std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
00378 #endif
00379 for(int lll =0; lll < 2; lll++)
00380 {
00381 t5 = theta5[lll];
00382 t7 = theta7[lll];
00383 if(!checkJointLimits(t5,4))
00384 continue;
00385 if(!checkJointLimits(t7,6))
00386 continue;
00387
00388 #ifdef DEBUG
00389 std::cout << "t5" << t5 << std::endl;
00390 std::cout << "t7" << t7 << std::endl;
00391 #endif
00392 if(fabs(sin(t6)*sin(t7)-grhs_local(0,1)) > IK_EPS || fabs(cos(t7)*sin(t6)-grhs_local(0,2)) > IK_EPS)
00393 continue;
00394
00395 solution_ik[0] = normalize_angle(t1)*angle_multipliers_[0];
00396 solution_ik[1] = normalize_angle(t2)*angle_multipliers_[1];
00397 solution_ik[2] = normalize_angle(t3)*angle_multipliers_[2];
00398 solution_ik[3] = normalize_angle(t4)*angle_multipliers_[3];
00399 solution_ik[4] = normalize_angle(t5)*angle_multipliers_[4];
00400 solution_ik[5] = normalize_angle(t6)*angle_multipliers_[5];
00401 solution_ik[6] = normalize_angle(t7)*angle_multipliers_[6];
00402 solution.push_back(solution_ik);
00403
00404 #ifdef DEBUG
00405 std::cout << "SOLN " << solution_ik[0] << " " << solution_ik[1] << " " << solution_ik[2] << " " << solution_ik[3] << " " << solution_ik[4] << " " << solution_ik[5] << " " << solution_ik[6] << std::endl << std::endl;
00406 #endif
00407 }
00408 }
00409 }
00410 }
00411 }
00412 }
00413
00414
00415 void PR2ArmIK::computeIKShoulderRoll(const Eigen::Matrix4f &g_in, const double &t3, std::vector<std::vector<double> > &solution) const
00416 {
00417 std::vector<double> solution_ik(NUM_JOINTS_ARM7DOF,0.0);
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428 Eigen::Matrix4f g = g_in;
00429 Eigen::Matrix4f gf_local = home_inv_;
00430 Eigen::Matrix4f grhs_local = home_inv_;
00431
00432 g(0,3) = g_in(0,3) - torso_shoulder_offset_x_;
00433 g(1,3) = g_in(1,3) - torso_shoulder_offset_y_;
00434 g(2,3) = g_in(2,3) - torso_shoulder_offset_z_;
00435
00436 if(!checkJointLimits(t3,2))
00437 {
00438 return;
00439 }
00440 double x = g(0,3);
00441 double y = g(1,3);
00442 double z = g(2,3);
00443 double cost1, cost2, cost3, cost4;
00444 double sint1, sint2, sint3, sint4;
00445
00446 gf_local = g*home_inv_;
00447
00448 cost3 = cos(t3);
00449 sint3 = sin(t3);
00450
00451 double t1(0), t2(0), t4(0), t5(0), t6(0), t7(0);
00452
00453 double at(0), bt(0), ct(0);
00454
00455 double theta1[2],theta2[2],theta4[4],theta5[2],theta6[4],theta7[2];
00456
00457 double c0 = -sin(-t3)*elbow_wrist_offset_;
00458 double c1 = -cos(-t3)*elbow_wrist_offset_;
00459
00460 double d0 = 4*shoulder_upperarm_offset_*shoulder_upperarm_offset_*(upperarm_elbow_offset_*upperarm_elbow_offset_+c1*c1-z*z);
00461 double d1 = 8*shoulder_upperarm_offset_*shoulder_upperarm_offset_*upperarm_elbow_offset_*elbow_wrist_offset_;
00462 double d2 = 4*shoulder_upperarm_offset_*shoulder_upperarm_offset_*(elbow_wrist_offset_*elbow_wrist_offset_-c1*c1);
00463
00464 double b0 = x*x+y*y+z*z-shoulder_upperarm_offset_*shoulder_upperarm_offset_-upperarm_elbow_offset_*upperarm_elbow_offset_-c0*c0-c1*c1;
00465 double b1 = -2*upperarm_elbow_offset_*elbow_wrist_offset_;
00466
00467 if(!solveQuadratic(b1*b1-d2,2*b0*b1-d1,b0*b0-d0,&theta4[0],&theta4[1]))
00468 {
00469 #ifdef DEBUG
00470 printf("No solution to quadratic eqn\n");
00471 #endif
00472 return;
00473 }
00474 theta4[0] = acos(theta4[0]);
00475 theta4[2] = acos(theta4[1]);
00476 theta4[1] = -theta4[0];
00477 theta4[3] = -theta4[2];
00478
00479 for(int jj = 0; jj < 4; jj++)
00480 {
00481 t4 = theta4[jj];
00482
00483 if(!checkJointLimits(t4,3))
00484 {
00485 continue;
00486 }
00487 cost4 = cos(t4);
00488 sint4 = sin(t4);
00489 #ifdef DEBUG
00490 std::cout << "t4 " << t4 << std::endl;
00491 #endif
00492 if(std::isnan(t4))
00493 continue;
00494 at = cos(t3)*sin(t4)*(shoulder_elbow_offset_-shoulder_wrist_offset_);
00495 bt = (shoulder_upperarm_offset_-shoulder_elbow_offset_+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cos(t4));
00496 ct = z;
00497
00498 if(!solveCosineEqn(at,bt,ct,theta2[0],theta2[1]))
00499 continue;
00500
00501 for(int ii=0; ii < 2; ii++)
00502 {
00503 t2 = theta2[ii];
00504 #ifdef DEBUG
00505 std::cout << "t2 " << t2 << std::endl;
00506 #endif
00507 if(!checkJointLimits(t2,1))
00508 {
00509 continue;
00510 }
00511
00512
00513 sint2 = sin(t2);
00514 cost2 = cos(t2);
00515
00516 at = -y;
00517 bt = x;
00518 ct = (shoulder_elbow_offset_-shoulder_wrist_offset_)*sin(t3)*sin(t4);
00519 if(!solveCosineEqn(at,bt,ct,theta1[0],theta1[1]))
00520 {
00521 #ifdef DEBUG
00522 std::cout << "could not solve cosine equation for t1" << std::endl;
00523 #endif
00524 continue;
00525 }
00526
00527 for(int kk =0; kk < 2; kk++)
00528 {
00529 t1 = theta1[kk];
00530 #ifdef DEBUG
00531 std::cout << "t1 " << t1 << std::endl;
00532 #endif
00533 if(!checkJointLimits(t1,0))
00534 {
00535 continue;
00536 }
00537 sint1 = sin(t1);
00538 cost1 = cos(t1);
00539 if(fabs((shoulder_upperarm_offset_-shoulder_elbow_offset_+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost4)*sint2+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost2*cost3*sint4-z) > IK_EPS )
00540 {
00541 #ifdef DEBUG
00542 printf("z value not matched %f\n",fabs((shoulder_upperarm_offset_-shoulder_elbow_offset_+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost4)*sint2+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost2*cost3*sint4-z));
00543 #endif
00544 continue;
00545 }
00546 if(fabs((shoulder_elbow_offset_-shoulder_wrist_offset_)*sint1*sint3*sint4+cost1*(shoulder_upperarm_offset_+cost2*(-shoulder_upperarm_offset_+shoulder_elbow_offset_+(-shoulder_elbow_offset_+shoulder_wrist_offset_)*cost4)+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost3*sint2*sint4) - x) > IK_EPS)
00547 {
00548 #ifdef DEBUG
00549 printf("x value not matched by %f\n",fabs((shoulder_elbow_offset_-shoulder_wrist_offset_)*sint1*sint3*sint4+cost1*(shoulder_upperarm_offset_+cost2*(-shoulder_upperarm_offset_+shoulder_elbow_offset_+(-shoulder_elbow_offset_+shoulder_wrist_offset_)*cost4)+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost3*sint2*sint4) - x));
00550 #endif
00551 continue;
00552 }
00553 if(fabs(-(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost1*sint3*sint4+sint1*(shoulder_upperarm_offset_+cost2*(-shoulder_upperarm_offset_+shoulder_elbow_offset_+(-shoulder_elbow_offset_+shoulder_wrist_offset_)*cost4)+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cost3*sint2*sint4) - y) > IK_EPS)
00554 {
00555 #ifdef DEBUG
00556 printf("y value not matched\n");
00557 #endif
00558 continue;
00559 }
00560 grhs_local(0,0) = cost4*(gf_local(0,0)*cost1*cost2+gf_local(1,0)*cost2*sint1-gf_local(2,0)*sint2)-(gf_local(2,0)*cost2*cost3 + cost3*(gf_local(0,0)*cost1 + gf_local(1,0)*sint1)*sint2 + (-(gf_local(1,0)*cost1) + gf_local(0,0)*sint1)*sint3)*sint4;
00561
00562 grhs_local(0,1) = cost4*(gf_local(0,1)*cost1*cost2 + gf_local(1,1)*cost2*sint1 - gf_local(2,1)*sint2) - (gf_local(2,1)*cost2*cost3 + cost3*(gf_local(0,1)*cost1 + gf_local(1,1)*sint1)*sint2 + (-(gf_local(1,1)*cost1) + gf_local(0,1)*sint1)*sint3)*sint4;
00563
00564 grhs_local(0,2) = cost4*(gf_local(0,2)*cost1*cost2 + gf_local(1,2)*cost2*sint1 - gf_local(2,2)*sint2) - (gf_local(2,2)*cost2*cost3 + cost3*(gf_local(0,2)*cost1 + gf_local(1,2)*sint1)*sint2 + (-(gf_local(1,2)*cost1) + gf_local(0,2)*sint1)*sint3)*sint4;
00565
00566 grhs_local(1,0) = cost3*(gf_local(1,0)*cost1 - gf_local(0,0)*sint1) + gf_local(2,0)*cost2*sint3 + (gf_local(0,0)*cost1 + gf_local(1,0)*sint1)*sint2*sint3;
00567
00568 grhs_local(1,1) = cost3*(gf_local(1,1)*cost1 - gf_local(0,1)*sint1) + gf_local(2,1)*cost2*sint3 + (gf_local(0,1)*cost1 + gf_local(1,1)*sint1)*sint2*sint3;
00569
00570 grhs_local(1,2) = cost3*(gf_local(1,2)*cost1 - gf_local(0,2)*sint1) + gf_local(2,2)*cost2*sint3 + (gf_local(0,2)*cost1 + gf_local(1,2)*sint1)*sint2*sint3;
00571
00572 grhs_local(2,0) = cost4*(gf_local(2,0)*cost2*cost3 + cost3*(gf_local(0,0)*cost1 + gf_local(1,0)*sint1)*sint2 + (-(gf_local(1,0)*cost1) + gf_local(0,0)*sint1)*sint3) + (gf_local(0,0)*cost1*cost2 + gf_local(1,0)*cost2*sint1 - gf_local(2,0)*sint2)*sint4;
00573
00574 grhs_local(2,1) = cost4*(gf_local(2,1)*cost2*cost3 + cost3*(gf_local(0,1)*cost1 + gf_local(1,1)*sint1)*sint2 + (-(gf_local(1,1)*cost1) + gf_local(0,1)*sint1)*sint3) + (gf_local(0,1)*cost1*cost2 + gf_local(1,1)*cost2*sint1 - gf_local(2,1)*sint2)*sint4;
00575
00576 grhs_local(2,2) = cost4*(gf_local(2,2)*cost2*cost3 + cost3*(gf_local(0,2)*cost1 + gf_local(1,2)*sint1)*sint2 + (-(gf_local(1,2)*cost1) + gf_local(0,2)*sint1)*sint3) + (gf_local(0,2)*cost1*cost2 + gf_local(1,2)*cost2*sint1 - gf_local(2,2)*sint2)*sint4;
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586 double val1 = sqrt(grhs_local(0,1)*grhs_local(0,1)+grhs_local(0,2)*grhs_local(0,2));
00587 double val2 = grhs_local(0,0);
00588
00589 theta6[0] = atan2(val1,val2);
00590 theta6[1] = atan2(-val1,val2);
00591
00592 for(int mm = 0; mm < 2; mm++)
00593 {
00594 t6 = theta6[mm];
00595 #ifdef DEBUG
00596 std::cout << "t6 " << t6 << std::endl;
00597 #endif
00598 if(!checkJointLimits(t6,5))
00599 {
00600 continue;
00601 }
00602
00603
00604 if(fabs(cos(t6) - grhs_local(0,0)) > IK_EPS)
00605 continue;
00606
00607 if(fabs(sin(t6)) < IK_EPS)
00608 {
00609
00610 theta5[0] = acos(grhs_local(1,1))/2.0;
00611 theta7[0] = theta5[0];
00612
00613
00614 }
00615 else
00616 {
00617 theta7[0] = atan2(grhs_local(0,1)/sin(t6),grhs_local(0,2)/sin(t6));
00618 theta5[0] = atan2(grhs_local(1,0)/sin(t6),-grhs_local(2,0)/sin(t6));
00619
00620
00621 }
00622 for(int lll =0; lll < 1; lll++)
00623 {
00624 t5 = theta5[lll];
00625 t7 = theta7[lll];
00626
00627 if(!checkJointLimits(t5,4))
00628 {
00629 continue;
00630 }
00631 if(!checkJointLimits(t7,6))
00632 {
00633 continue;
00634 }
00635
00636
00637 #ifdef DEBUG
00638 std::cout << "t5 " << t5 << std::endl;
00639 std::cout << "t7 " << t7 << std::endl;
00640 #endif
00641
00642
00643
00644 #ifdef DEBUG
00645 std::cout << "theta1: " << t1 << std::endl;
00646 std::cout << "theta2: " << t2 << std::endl;
00647 std::cout << "theta3: " << t3 << std::endl;
00648 std::cout << "theta4: " << t4 << std::endl;
00649 std::cout << "theta5: " << t5 << std::endl;
00650 std::cout << "theta6: " << t6 << std::endl;
00651 std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
00652 #endif
00653
00654
00655 solution_ik[0] = normalize_angle(t1*angle_multipliers_[0]);
00656 solution_ik[1] = normalize_angle(t2*angle_multipliers_[1]);
00657 solution_ik[2] = t3*angle_multipliers_[2];
00658 solution_ik[3] = normalize_angle(t4*angle_multipliers_[3]);
00659 solution_ik[4] = normalize_angle(t5*angle_multipliers_[4]);
00660 solution_ik[5] = normalize_angle(t6*angle_multipliers_[5]);
00661 solution_ik[6] = normalize_angle(t7*angle_multipliers_[6]);
00662 solution.push_back(solution_ik);
00663 #ifdef DEBUG
00664 std::cout << "SOLN " << solution_ik[0] << " " << solution_ik[1] << " " << solution_ik[2] << " " << solution_ik[3] << " " << solution_ik[4] << " " << solution_ik[5] << " " << solution_ik[6] << std::endl << std::endl;
00665 #endif
00666 }
00667 }
00668 }
00669 }
00670 }
00671 }
00672
00673
00674 bool PR2ArmIK::checkJointLimits(const std::vector<double> &joint_values) const
00675 {
00676 for(int i=0; i<NUM_JOINTS_ARM7DOF; i++)
00677 {
00678 if(!checkJointLimits(angles::normalize_angle(joint_values[i]*angle_multipliers_[i]),i))
00679 {
00680 return false;
00681 }
00682 }
00683 return true;
00684 }
00685
00686 bool PR2ArmIK::checkJointLimits(const double &joint_value, const int &joint_num) const
00687 {
00688 double jv;
00689 if(continuous_joint_[joint_num])
00690 jv= angles::normalize_angle(joint_value*angle_multipliers_[joint_num]);
00691 else if (joint_num ==2)
00692 jv = joint_value*angle_multipliers_[joint_num];
00693 else
00694 jv= angles::normalize_angle(joint_value*angle_multipliers_[joint_num]);
00695
00696 if(jv < min_angles_[joint_num] || jv > max_angles_[joint_num])
00697 {
00698
00699 return false;
00700 }
00701 return true;
00702 }