pr2_arm_kinematics: pr2_arm_kinematics: pr2_arm

00001 //Software License Agreement (BSD License)
00002 
00003 //Copyright (c) 2008, Willow Garage, Inc.
00004 //All rights reserved.
00005 
00006 //Redistribution and use in source and binary forms, with or without
00007 //modification, are permitted provided that the following conditions
00008 //are met:
00009 
00010 // * Redistributions of source code must retain the above copyright
00011 //   notice, this list of conditions and the following disclaimer.
00012 // * Redistributions in binary form must reproduce the above
00013 //   copyright notice, this list of conditions and the following
00014 //   disclaimer in the documentation and/or other materials provided
00015 //   with the distribution.
00016 // * Neither the name of Willow Garage, Inc. nor the names of its
00017 //   contributors may be used to endorse or promote products derived
00018 //   from this software without specific prior written permission.
00019 
00020 //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 //"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 //LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 //FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 //COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 //INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 //BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 //LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 //CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 //LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 //ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 //POSSIBILITY OF SUCH DAMAGE.
00032 
00033 #include <angles/angles.h>
00034 #include <pr2_arm_kinematics/pr2_arm_ik.h>
00035 
00036 /**** List of angles (for reference) *******
00037       th1 = shoulder/turret pan
00038       th2 = shoulder/turret lift/pitch
00039       th3 = shoulder/turret roll
00040       th4 = elbow pitch
00041       th5 = elbow roll 
00042       th6 = wrist pitch
00043       th7 = wrist roll 
00044 *****/
00045 using namespace angles;
00046 using namespace pr2_arm_kinematics;
00047 
00048 PR2ArmIK::PR2ArmIK()
00049 {
00050 }
00051 
00052 bool PR2ArmIK::init(const urdf::Model &robot_model, const std::string &root_name, const std::string &tip_name)
00053 {
00054   std::vector<urdf::Pose> link_offset;
00055   int num_joints = 0;
00056   boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
00057   while(link && num_joints < 7)
00058   {
00059     boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(link->parent_joint->name);
00060     if(!joint)
00061     {
00062       ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
00063       return false;
00064     }
00065     if(joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00066     {
00067       link_offset.push_back(link->parent_joint->parent_to_joint_origin_transform);
00068       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));
00069       ROS_DEBUG("Joint axis: %d, %f, %f, %f",6-num_joints,joint->axis.x,joint->axis.y,joint->axis.z);
00070       if(joint->type != urdf::Joint::CONTINUOUS)
00071       {
00072         min_angles_.push_back(joint->safety->soft_lower_limit);
00073         max_angles_.push_back(joint->safety->soft_upper_limit);
00074         continuous_joint_.push_back(false);
00075       }
00076       else
00077       {
00078         min_angles_.push_back(-M_PI);
00079         max_angles_.push_back(M_PI);
00080         continuous_joint_.push_back(true);
00081       }
00082       addJointToChainInfo(link->parent_joint,solver_info_);
00083       num_joints++;
00084     }
00085     link = robot_model.getLink(link->getParent()->name);
00086   } 
00087 
00088   solver_info_.link_names.push_back(tip_name);
00089 
00090   //  solver_info_.link_names.push_back(tip_name);
00091   // We expect order from root to tip, so reverse the order
00092   std::reverse(angle_multipliers_.begin(),angle_multipliers_.end());
00093   std::reverse(min_angles_.begin(),min_angles_.end());
00094   std::reverse(max_angles_.begin(),max_angles_.end());
00095   std::reverse(link_offset.begin(),link_offset.end());
00096   std::reverse(solver_info_.limits.begin(),solver_info_.limits.end());
00097   std::reverse(solver_info_.joint_names.begin(),solver_info_.joint_names.end());
00098   std::reverse(solver_info_.link_names.begin(),solver_info_.link_names.end());
00099   std::reverse(continuous_joint_.begin(),continuous_joint_.end());
00100 
00101   if(num_joints != 7)
00102   {
00103     ROS_FATAL("PR2ArmIK:: Chain from %s to %s does not have 7 joints",root_name.c_str(),tip_name.c_str());
00104     return false;
00105   }
00106 
00107   torso_shoulder_offset_x_ = link_offset[0].position.x;
00108   torso_shoulder_offset_y_ = link_offset[0].position.y;
00109   torso_shoulder_offset_z_ = link_offset[0].position.z;
00110   shoulder_upperarm_offset_ = distance(link_offset[1]);
00111   upperarm_elbow_offset_ = distance(link_offset[3]);
00112   elbow_wrist_offset_ = distance(link_offset[5]);
00113   shoulder_elbow_offset_ = shoulder_upperarm_offset_ + upperarm_elbow_offset_;
00114   shoulder_wrist_offset_ = shoulder_upperarm_offset_+upperarm_elbow_offset_+elbow_wrist_offset_;
00115 
00116   Eigen::Matrix4f home = Eigen::Matrix4f::Identity();
00117   home(0,3) = shoulder_upperarm_offset_ +  upperarm_elbow_offset_ +  elbow_wrist_offset_;
00118   home_inv_ = home.inverse();
00119   grhs_ = home;
00120   gf_ = home_inv_;
00121   solution_.resize( NUM_JOINTS_ARM7DOF);
00122   return true;
00123 }
00124 
00125 void PR2ArmIK::addJointToChainInfo(boost::shared_ptr<const urdf::Joint> joint, kinematics_msgs::KinematicSolverInfo &info)
00126 {
00127   arm_navigation_msgs::JointLimits limit;
00128   info.joint_names.push_back(joint->name);//Joints are coming in reverse order
00129   limit.min_position = joint->safety->soft_lower_limit;
00130   limit.max_position = joint->safety->soft_upper_limit;
00131   if(joint->type != urdf::Joint::CONTINUOUS)
00132   {
00133     limit.min_position = joint->safety->soft_lower_limit;
00134     limit.max_position = joint->safety->soft_upper_limit;
00135     limit.has_position_limits = true;
00136   }
00137   else
00138   {
00139     limit.min_position = -M_PI;
00140     limit.max_position = M_PI;
00141     limit.has_position_limits = false;
00142   }
00143   limit.max_velocity = joint->limits->velocity;
00144   limit.has_velocity_limits = 1;
00145   info.limits.push_back(limit);
00146 }
00147 
00148 void PR2ArmIK::getSolverInfo(kinematics_msgs::KinematicSolverInfo &info)
00149 {
00150   info = solver_info_;
00151 }
00152 
00153 
00154 void PR2ArmIK::computeIKShoulderPan(const Eigen::Matrix4f &g_in, const double &t1_in, std::vector<std::vector<double> > &solution)
00155 {
00156 //t1 = shoulder/turret pan is specified
00157 //  solution_ik_.resize(0);
00158 
00159   Eigen::Matrix4f g = g_in;
00160 //First bring everything into the arm frame
00161   g(0,3) = g_in(0,3) - torso_shoulder_offset_x_;
00162   g(1,3) = g_in(1,3) - torso_shoulder_offset_y_;
00163   g(2,3) = g_in(2,3) - torso_shoulder_offset_z_;
00164 
00165   double t1 = angles::normalize_angle(t1_in);
00166   if(!checkJointLimits(t1,0))
00167     return;
00168 
00169 
00170   double cost1, cost2, cost3, cost4;
00171   double sint1, sint2, sint3, sint4;
00172 
00173   gf_ = g*home_inv_;
00174 
00175   cost1 = cos(t1);
00176   sint1 = sin(t1);
00177 
00178   double t2(0), t3(0), t4(0), t5(0), t6(0), t7(0);
00179 
00180   double at(0), bt(0), ct(0);
00181 
00182   double theta2[2],theta3[2],theta4[2],theta5[2],theta6[4],theta7[2]; 
00183 
00184   double sopx = shoulder_upperarm_offset_*cost1;
00185   double sopy = shoulder_upperarm_offset_*sint1;
00186   double sopz = 0;
00187 
00188   double x = g(0,3);
00189   double y = g(1,3);
00190   double z = g(2,3);
00191 
00192   double dx = x - sopx;
00193   double dy = y - sopy;
00194   double dz = z - sopz;
00195 
00196   double dd = dx*dx + dy*dy + dz*dz;
00197 
00198   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_;
00199   double denominator = 2*(shoulder_upperarm_offset_-shoulder_elbow_offset_)*(shoulder_elbow_offset_-shoulder_wrist_offset_);
00200 
00201   double acosTerm = numerator/denominator;
00202 
00203   if (acosTerm > 1.0 || acosTerm < -1.0)
00204     return;
00205 
00206   double acos_angle = acos(acosTerm);
00207  
00208   theta4[0] = acos_angle;
00209   theta4[1] = -acos_angle;
00210 
00211 #ifdef DEBUG
00212   std::cout << "ComputeIK::theta3:" << numerator << "," << denominator << "," << std::endl << theta4[0] << std::endl;
00213 #endif
00214 
00215   for(int jj =0; jj < 2; jj++)
00216   {
00217     t4 = theta4[jj];
00218     cost4 = cos(t4);
00219     sint4 = sin(t4);
00220 
00221 #ifdef DEBUG
00222     std::cout << "t4 " << t4 << std::endl;
00223 #endif
00224     if(std::isnan(t4))
00225       continue;
00226 
00227     if(!checkJointLimits(t4,3))
00228       continue;
00229 
00230     at = x*cost1+y*sint1-shoulder_upperarm_offset_;
00231     bt = -z;
00232     ct = -shoulder_upperarm_offset_ + shoulder_elbow_offset_ + (shoulder_wrist_offset_-shoulder_elbow_offset_)*cos(t4);
00233 
00234     if(!solveCosineEqn(at,bt,ct,theta2[0],theta2[1]))
00235       continue;
00236 
00237     for(int ii=0; ii < 2; ii++)
00238     {
00239       t2 = theta2[ii];
00240       if(!checkJointLimits(t2,1))
00241         continue;
00242 
00243 
00244 #ifdef DEBUG
00245       std::cout << "t2 " << t2 << std::endl; 
00246 #endif
00247       sint2 = sin(t2);
00248       cost2 = cos(t2);
00249 
00250       at = sint1*(shoulder_elbow_offset_ - shoulder_wrist_offset_)*sint2*sint4;
00251       bt = (-shoulder_elbow_offset_+shoulder_wrist_offset_)*cost1*sint4;
00252       ct = y - (shoulder_upperarm_offset_+cost2*(-shoulder_upperarm_offset_+shoulder_elbow_offset_+(-shoulder_elbow_offset_+shoulder_wrist_offset_)*cos(t4)))*sint1;
00253       if(!solveCosineEqn(at,bt,ct,theta3[0],theta3[1]))
00254         continue;
00255 
00256       for(int kk =0; kk < 2; kk++)
00257       {           
00258         t3 = theta3[kk];
00259 
00260         if(!checkJointLimits(angles::normalize_angle(t3),2))
00261           continue;
00262 
00263         sint3 = sin(t3);
00264         cost3 = cos(t3);
00265 #ifdef DEBUG
00266         std::cout << "t3 " << t3 << std::endl; 
00267 #endif
00268         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 )
00269           continue;
00270 
00271         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)
00272           continue;
00273 
00274         grhs_(0,0) = cost4*(gf_(0,0)*cost1*cost2+gf_(1,0)*cost2*sint1-gf_(2,0)*sint2)-(gf_(2,0)*cost2*cost3 + cost3*(gf_(0,0)*cost1 + gf_(1,0)*sint1)*sint2 + (-(gf_(1,0)*cost1) + gf_(0,0)*sint1)*sint3)*sint4;
00275 
00276         grhs_(0,1) = cost4*(gf_(0,1)*cost1*cost2 + gf_(1,1)*cost2*sint1 - gf_(2,1)*sint2) - (gf_(2,1)*cost2*cost3 + cost3*(gf_(0,1)*cost1 + gf_(1,1)*sint1)*sint2 + (-(gf_(1,1)*cost1) + gf_(0,1)*sint1)*sint3)*sint4;
00277 
00278         grhs_(0,2) = cost4*(gf_(0,2)*cost1*cost2 + gf_(1,2)*cost2*sint1 - gf_(2,2)*sint2) - (gf_(2,2)*cost2*cost3 + cost3*(gf_(0,2)*cost1 + gf_(1,2)*sint1)*sint2 + (-(gf_(1,2)*cost1) + gf_(0,2)*sint1)*sint3)*sint4;
00279 
00280         grhs_(1,0) = cost3*(gf_(1,0)*cost1 - gf_(0,0)*sint1) + gf_(2,0)*cost2*sint3 + (gf_(0,0)*cost1 + gf_(1,0)*sint1)*sint2*sint3;
00281 
00282         grhs_(1,1) = cost3*(gf_(1,1)*cost1 - gf_(0,1)*sint1) + gf_(2,1)*cost2*sint3 + (gf_(0,1)*cost1 + gf_(1,1)*sint1)*sint2*sint3;
00283 
00284         grhs_(1,2) = cost3*(gf_(1,2)*cost1 - gf_(0,2)*sint1) + gf_(2,2)*cost2*sint3 + (gf_(0,2)*cost1 + gf_(1,2)*sint1)*sint2*sint3;
00285 
00286         grhs_(2,0) = cost4*(gf_(2,0)*cost2*cost3 + cost3*(gf_(0,0)*cost1 + gf_(1,0)*sint1)*sint2 + (-(gf_(1,0)*cost1) + gf_(0,0)*sint1)*sint3) + (gf_(0,0)*cost1*cost2 + gf_(1,0)*cost2*sint1 - gf_(2,0)*sint2)*sint4;
00287 
00288         grhs_(2,1) = cost4*(gf_(2,1)*cost2*cost3 + cost3*(gf_(0,1)*cost1 + gf_(1,1)*sint1)*sint2 + (-(gf_(1,1)*cost1) + gf_(0,1)*sint1)*sint3) + (gf_(0,1)*cost1*cost2 + gf_(1,1)*cost2*sint1 - gf_(2,1)*sint2)*sint4;
00289 
00290         grhs_(2,2) = cost4*(gf_(2,2)*cost2*cost3 + cost3*(gf_(0,2)*cost1 + gf_(1,2)*sint1)*sint2 + (-(gf_(1,2)*cost1) + gf_(0,2)*sint1)*sint3) + (gf_(0,2)*cost1*cost2 + gf_(1,2)*cost2*sint1 - gf_(2,2)*sint2)*sint4;
00291 
00292 
00293         double val1 = sqrt(grhs_(0,1)*grhs_(0,1)+grhs_(0,2)*grhs_(0,2));
00294         double val2 = grhs_(0,0);
00295 
00296         theta6[0] = atan2(val1,val2);
00297         theta6[1] = atan2(-val1,val2);
00298 
00299 //            theta6[3] = M_PI + theta6[0];
00300 //            theta6[4] = M_PI + theta6[1];
00301 
00302         for(int mm = 0; mm < 2; mm++)
00303         {
00304           t6 = theta6[mm];
00305           if(!checkJointLimits(angles::normalize_angle(t6),5))
00306             continue;
00307 
00308 #ifdef DEBUG
00309           std::cout << "t6 " << t6 << std::endl;
00310 #endif
00311           if(fabs(cos(t6) - grhs_(0,0)) > IK_EPS)
00312             continue;
00313 
00314           if(fabs(sin(t6)) < IK_EPS)
00315           {
00316             //                std::cout << "Singularity" << std::endl;
00317             theta5[0] = acos(grhs_(1,1))/2.0;
00318             theta7[0] = theta7[0];
00319             theta7[1] = M_PI+theta7[0];
00320             theta5[1] = theta7[1];
00321           }
00322           else
00323           {
00324             theta7[0] = atan2(grhs_(0,1),grhs_(0,2));
00325             theta5[0] = atan2(grhs_(1,0),-grhs_(2,0));
00326             theta7[1] = M_PI+theta7[0];
00327             theta5[1] = M_PI+theta5[0];
00328           }
00329 #ifdef DEBUG
00330           std::cout << "theta1: " << t1 << std::endl;
00331           std::cout << "theta2: " << t2 << std::endl;
00332           std::cout << "theta3: " << t3 << std::endl;
00333           std::cout << "theta4: " << t4 << std::endl;
00334           std::cout << "theta5: " << t5 << std::endl;
00335           std::cout << "theta6: " << t6 << std::endl;
00336           std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
00337 #endif
00338           for(int lll =0; lll < 2; lll++)
00339           {
00340             t5 = theta5[lll];
00341             t7 = theta7[lll];
00342             if(!checkJointLimits(t5,4))
00343               continue;
00344             if(!checkJointLimits(t7,6))
00345               continue;
00346 
00347 #ifdef DEBUG
00348             std::cout << "t5" << t5 << std::endl;
00349             std::cout << "t7" << t7 << std::endl;
00350 #endif      
00351             if(fabs(sin(t6)*sin(t7)-grhs_(0,1)) > IK_EPS || fabs(cos(t7)*sin(t6)-grhs_(0,2)) > IK_EPS)
00352               continue;
00353 
00354             solution_[0] = normalize_angle(t1)*angle_multipliers_[0];
00355             solution_[1] = normalize_angle(t2)*angle_multipliers_[1];
00356             solution_[2] = normalize_angle(t3)*angle_multipliers_[2];
00357             solution_[3] = normalize_angle(t4)*angle_multipliers_[3];
00358             solution_[4] = normalize_angle(t5)*angle_multipliers_[4];
00359             solution_[5] = normalize_angle(t6)*angle_multipliers_[5];
00360             solution_[6] = normalize_angle(t7)*angle_multipliers_[6];
00361             solution.push_back(solution_);
00362 
00363 #ifdef DEBUG
00364             std::cout << "SOLN " << solution_[0] << " " << solution_[1] << " " <<  solution_[2] << " " << solution_[3] <<  " " << solution_[4] << " " << solution_[5] <<  " " << solution_[6] << std::endl << std::endl;
00365 #endif
00366           }
00367         }
00368       }
00369     }
00370   }
00371 }
00372 
00373 
00374 void PR2ArmIK::computeIKShoulderRoll(const Eigen::Matrix4f &g_in, const double &t3, std::vector<std::vector<double> > &solution)
00375 {
00376   //  ROS_INFO(" ");
00377   // solution_ik_.clear();
00378   //  ROS_INFO("Solution IK size: %d",solution_ik_.size());
00379   //  for(unsigned int i=0; i < solution_ik_.size(); i++)
00380   //  {
00381   //    solution_ik_[i].clear();
00382   //  }
00383   //  if(!solution_ik_.empty())
00384   //    solution_ik_.resize(0);
00385 //t3 = shoulder/turret roll is specified
00386   Eigen::Matrix4f g = g_in;
00387 //First bring everything into the arm frame
00388   g(0,3) = g_in(0,3) - torso_shoulder_offset_x_;
00389   g(1,3) = g_in(1,3) - torso_shoulder_offset_y_;
00390   g(2,3) = g_in(2,3) - torso_shoulder_offset_z_;
00391 
00392   if(!checkJointLimits(t3,2))
00393   {
00394     return;
00395   }
00396   double x = g(0,3);
00397   double y = g(1,3);
00398   double z = g(2,3);
00399   double cost1, cost2, cost3, cost4;
00400   double sint1, sint2, sint3, sint4;
00401 
00402   gf_ = g*home_inv_;
00403 
00404   cost3 = cos(t3);
00405   sint3 = sin(t3);
00406 
00407   double t1(0), t2(0), t4(0), t5(0), t6(0), t7(0);
00408 
00409   double at(0), bt(0), ct(0);
00410 
00411   double theta1[2],theta2[2],theta4[4],theta5[2],theta6[4],theta7[2];
00412 
00413   double c0 = -sin(-t3)*elbow_wrist_offset_;
00414   double c1 = -cos(-t3)*elbow_wrist_offset_;
00415 
00416   double d0 = 4*shoulder_upperarm_offset_*shoulder_upperarm_offset_*(upperarm_elbow_offset_*upperarm_elbow_offset_+c1*c1-z*z);
00417   double d1 = 8*shoulder_upperarm_offset_*shoulder_upperarm_offset_*upperarm_elbow_offset_*elbow_wrist_offset_;
00418   double d2 = 4*shoulder_upperarm_offset_*shoulder_upperarm_offset_*(elbow_wrist_offset_*elbow_wrist_offset_-c1*c1);
00419 
00420   double b0 = x*x+y*y+z*z-shoulder_upperarm_offset_*shoulder_upperarm_offset_-upperarm_elbow_offset_*upperarm_elbow_offset_-c0*c0-c1*c1;
00421   double b1 = -2*upperarm_elbow_offset_*elbow_wrist_offset_;
00422 
00423   if(!solveQuadratic(b1*b1-d2,2*b0*b1-d1,b0*b0-d0,&theta4[0],&theta4[1]))
00424   {
00425 #ifdef DEBUG
00426     printf("No solution to quadratic eqn\n");
00427 #endif
00428     return;
00429   }
00430   theta4[0] = acos(theta4[0]);
00431   theta4[2] = acos(theta4[1]);
00432   theta4[1] = -theta4[0];
00433   theta4[3] = -theta4[2];
00434 
00435   for(int jj = 0; jj < 4; jj++)
00436   {
00437     t4 = theta4[jj];
00438 
00439     if(!checkJointLimits(t4,3))
00440     {
00441       continue;
00442     }
00443     cost4 = cos(t4);
00444     sint4 = sin(t4);
00445 #ifdef DEBUG
00446     std::cout << "t4 " << t4 << std::endl;
00447 #endif
00448     if(std::isnan(t4))
00449       continue;
00450     at = cos(t3)*sin(t4)*(shoulder_elbow_offset_-shoulder_wrist_offset_);
00451     bt = (shoulder_upperarm_offset_-shoulder_elbow_offset_+(shoulder_elbow_offset_-shoulder_wrist_offset_)*cos(t4));
00452     ct = z;
00453 
00454     if(!solveCosineEqn(at,bt,ct,theta2[0],theta2[1]))
00455       continue;
00456 
00457     for(int ii=0; ii < 2; ii++)
00458     {
00459       t2 = theta2[ii];
00460 #ifdef DEBUG
00461       std::cout << "t2 " << t2 << std::endl;
00462 #endif
00463       if(!checkJointLimits(t2,1))
00464       {
00465         continue;
00466       }
00467 
00468 
00469       sint2 = sin(t2);
00470       cost2 = cos(t2);
00471 
00472       at = -y;
00473       bt = x;
00474       ct = (shoulder_elbow_offset_-shoulder_wrist_offset_)*sin(t3)*sin(t4);
00475       if(!solveCosineEqn(at,bt,ct,theta1[0],theta1[1]))
00476       {
00477 #ifdef DEBUG
00478         std::cout << "could not solve cosine equation for t1" << std::endl;
00479 #endif
00480         continue;
00481       }
00482 
00483       for(int kk =0; kk < 2; kk++)
00484       {           
00485         t1 = theta1[kk];
00486 #ifdef DEBUG
00487         std::cout << "t1 " << t1 << std::endl;
00488 #endif
00489         if(!checkJointLimits(t1,0))
00490         {
00491           continue;
00492         }
00493         sint1 = sin(t1);
00494         cost1 = cos(t1);
00495         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 )
00496         {
00497 #ifdef DEBUG
00498           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));
00499 #endif
00500           continue;
00501         }
00502         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)
00503         {
00504 #ifdef DEBUG
00505           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));
00506 #endif
00507           continue;
00508         }
00509         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)
00510         {
00511 #ifdef DEBUG
00512           printf("y value not matched\n");
00513 #endif
00514           continue;
00515         }
00516         grhs_(0,0) = cost4*(gf_(0,0)*cost1*cost2+gf_(1,0)*cost2*sint1-gf_(2,0)*sint2)-(gf_(2,0)*cost2*cost3 + cost3*(gf_(0,0)*cost1 + gf_(1,0)*sint1)*sint2 + (-(gf_(1,0)*cost1) + gf_(0,0)*sint1)*sint3)*sint4;
00517 
00518         grhs_(0,1) = cost4*(gf_(0,1)*cost1*cost2 + gf_(1,1)*cost2*sint1 - gf_(2,1)*sint2) - (gf_(2,1)*cost2*cost3 + cost3*(gf_(0,1)*cost1 + gf_(1,1)*sint1)*sint2 + (-(gf_(1,1)*cost1) + gf_(0,1)*sint1)*sint3)*sint4;
00519 
00520         grhs_(0,2) = cost4*(gf_(0,2)*cost1*cost2 + gf_(1,2)*cost2*sint1 - gf_(2,2)*sint2) - (gf_(2,2)*cost2*cost3 + cost3*(gf_(0,2)*cost1 + gf_(1,2)*sint1)*sint2 + (-(gf_(1,2)*cost1) + gf_(0,2)*sint1)*sint3)*sint4;
00521 
00522         grhs_(1,0) = cost3*(gf_(1,0)*cost1 - gf_(0,0)*sint1) + gf_(2,0)*cost2*sint3 + (gf_(0,0)*cost1 + gf_(1,0)*sint1)*sint2*sint3;
00523 
00524         grhs_(1,1) = cost3*(gf_(1,1)*cost1 - gf_(0,1)*sint1) + gf_(2,1)*cost2*sint3 + (gf_(0,1)*cost1 + gf_(1,1)*sint1)*sint2*sint3;
00525 
00526         grhs_(1,2) = cost3*(gf_(1,2)*cost1 - gf_(0,2)*sint1) + gf_(2,2)*cost2*sint3 + (gf_(0,2)*cost1 + gf_(1,2)*sint1)*sint2*sint3;
00527 
00528         grhs_(2,0) = cost4*(gf_(2,0)*cost2*cost3 + cost3*(gf_(0,0)*cost1 + gf_(1,0)*sint1)*sint2 + (-(gf_(1,0)*cost1) + gf_(0,0)*sint1)*sint3) + (gf_(0,0)*cost1*cost2 + gf_(1,0)*cost2*sint1 - gf_(2,0)*sint2)*sint4;
00529 
00530         grhs_(2,1) = cost4*(gf_(2,1)*cost2*cost3 + cost3*(gf_(0,1)*cost1 + gf_(1,1)*sint1)*sint2 + (-(gf_(1,1)*cost1) + gf_(0,1)*sint1)*sint3) + (gf_(0,1)*cost1*cost2 + gf_(1,1)*cost2*sint1 - gf_(2,1)*sint2)*sint4;
00531 
00532         grhs_(2,2) = cost4*(gf_(2,2)*cost2*cost3 + cost3*(gf_(0,2)*cost1 + gf_(1,2)*sint1)*sint2 + (-(gf_(1,2)*cost1) + gf_(0,2)*sint1)*sint3) + (gf_(0,2)*cost1*cost2 + gf_(1,2)*cost2*sint1 - gf_(2,2)*sint2)*sint4;
00533 
00534 
00535 
00536 
00537 
00538 
00539 
00540 
00541 
00542         double val1 = sqrt(grhs_(0,1)*grhs_(0,1)+grhs_(0,2)*grhs_(0,2));
00543         double val2 = grhs_(0,0);
00544 
00545         theta6[0] = atan2(val1,val2);
00546         theta6[1] = atan2(-val1,val2);
00547 
00548         for(int mm = 0; mm < 2; mm++)
00549         {
00550           t6 = theta6[mm];
00551 #ifdef DEBUG
00552           std::cout << "t6 " << t6 << std::endl;
00553 #endif
00554           if(!checkJointLimits(t6,5))
00555           {
00556             continue;
00557           }
00558 
00559 
00560           if(fabs(cos(t6) - grhs_(0,0)) > IK_EPS)
00561             continue;
00562 
00563           if(fabs(sin(t6)) < IK_EPS)
00564           {
00565             //                std::cout << "Singularity" << std::endl;
00566             theta5[0] = acos(grhs_(1,1))/2.0;
00567             theta7[0] = theta5[0];
00568 //            theta7[1] = M_PI+theta7[0];
00569 //            theta5[1] = theta7[1];
00570           }
00571           else
00572           {
00573             theta7[0] = atan2(grhs_(0,1)/sin(t6),grhs_(0,2)/sin(t6));
00574             theta5[0] = atan2(grhs_(1,0)/sin(t6),-grhs_(2,0)/sin(t6));
00575 //            theta7[1] = M_PI+theta7[0];
00576 //            theta5[1] = M_PI+theta5[0];
00577           }
00578           for(int lll =0; lll < 1; lll++)
00579           {
00580             t5 = theta5[lll];
00581             t7 = theta7[lll];
00582 
00583             if(!checkJointLimits(t5,4))
00584             {
00585               continue;
00586             }
00587             if(!checkJointLimits(t7,6))
00588             {
00589               continue;
00590             }
00591 
00592 
00593 #ifdef DEBUG
00594             std::cout << "t5 " << t5 << std::endl;
00595             std::cout << "t7 " << t7 << std::endl;
00596 #endif      
00597             //           if(fabs(sin(t6)*sin(t7)-grhs_(0,1)) > IK_EPS || fabs(cos(t7)*sin(t6)-grhs_(0,2)) > IK_EPS)
00598             //  continue;
00599 
00600 #ifdef DEBUG
00601             std::cout << "theta1: " << t1 << std::endl;
00602             std::cout << "theta2: " << t2 << std::endl;
00603             std::cout << "theta3: " << t3 << std::endl;
00604             std::cout << "theta4: " << t4 << std::endl;
00605             std::cout << "theta5: " << t5 << std::endl;
00606             std::cout << "theta6: " << t6 << std::endl;
00607             std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
00608 #endif
00609 
00610 
00611             solution_[0] = normalize_angle(t1*angle_multipliers_[0]);
00612             solution_[1] = normalize_angle(t2*angle_multipliers_[1]);
00613             solution_[2] = t3*angle_multipliers_[2];
00614             solution_[3] = normalize_angle(t4*angle_multipliers_[3]);
00615             solution_[4] = normalize_angle(t5*angle_multipliers_[4]);
00616             solution_[5] = normalize_angle(t6*angle_multipliers_[5]);
00617             solution_[6] = normalize_angle(t7*angle_multipliers_[6]);
00618             solution.push_back(solution_);
00619 #ifdef DEBUG
00620             std::cout << "SOLN " << solution_[0] << " " << solution_[1] << " " <<  solution_[2] << " " << solution_[3] <<  " " << solution_[4] << " " << solution_[5] <<  " " << solution_[6] << std::endl << std::endl;
00621 #endif
00622           }
00623         }
00624       }
00625     }
00626   }
00627 }
00628 
00629 
00630 bool PR2ArmIK::checkJointLimits(const std::vector<double> &joint_values)
00631 {
00632   for(int i=0; i<NUM_JOINTS_ARM7DOF; i++)
00633   {
00634     if(!checkJointLimits(angles::normalize_angle(joint_values[i]*angle_multipliers_[i]),i))
00635     {
00636       return false;
00637     }
00638   }
00639   return true;
00640 }
00641 
00642 bool  PR2ArmIK::checkJointLimits(const double &joint_value, const int &joint_num)
00643 {
00644   double jv;
00645   if(continuous_joint_[joint_num])
00646     jv= angles::normalize_angle(joint_value*angle_multipliers_[joint_num]);
00647   else if (joint_num ==2)
00648     jv = joint_value*angle_multipliers_[joint_num];
00649   else
00650     jv= angles::normalize_angle(joint_value*angle_multipliers_[joint_num]);
00651 
00652   if(jv < min_angles_[joint_num] || jv > max_angles_[joint_num])
00653   {
00654     // ROS_INFO("Angle %d = %f out of range: (%f,%f)\n",joint_num,joint_value,min_angles_[joint_num],max_angles_[joint_num]);
00655     return false;
00656   }
00657   return true;
00658 }
pr2_arm_ik.cpp
