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