pr2_arm_ik.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Sachin Chitta, E. Gil Jones */
00036 
00037 #include <angles/angles.h>
00038 #include <console_bridge/console.h>
00039 #include "pr2_arm_ik.h"
00040 
00041 /**** List of angles (for reference) *******
00042       th1 = shoulder/turret pan
00043       th2 = shoulder/turret lift/pitch
00044       th3 = shoulder/turret roll
00045       th4 = elbow pitch
00046       th5 = elbow roll
00047       th6 = wrist pitch
00048       th7 = wrist roll
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   //  solver_info_.link_names.push_back(tip_name);
00118   // We expect order from root to tip, so reverse the order
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);//Joints are coming in reverse order
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 //t1 = shoulder/turret pan is specified
00200 //  solution_ik_.resize(0);
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 //First bring everything into the arm frame
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 //            theta6[3] = M_PI + theta6[0];
00345 //            theta6[4] = M_PI + theta6[1];
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             //                std::cout << "Singularity" << std::endl;
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   //  ROS_INFO(" ");
00423   // solution_ik_.clear();
00424   //  ROS_INFO("Solution IK size: %d",solution_ik_.size());
00425   //  for(unsigned int i=0; i < solution_ik_.size(); i++)
00426   //  {
00427   //    solution_ik_[i].clear();
00428   //  }
00429   //  if(!solution_ik_.empty())
00430   //    solution_ik_.resize(0);
00431 //t3 = shoulder/turret roll is specified
00432   Eigen::Matrix4f g = g_in;
00433   Eigen::Matrix4f gf_local = home_inv_;
00434   Eigen::Matrix4f grhs_local = home_inv_;
00435 //First bring everything into the arm frame
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             //                std::cout << "Singularity" << std::endl;
00614             theta5[0] = acos(grhs_local(1,1))/2.0;
00615             theta7[0] = theta5[0];
00616 //            theta7[1] = M_PI+theta7[0];
00617 //            theta5[1] = theta7[1];
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 //            theta7[1] = M_PI+theta7[0];
00624 //            theta5[1] = M_PI+theta5[0];
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             //           if(fabs(sin(t6)*sin(t7)-grhs_local(0,1)) > IK_EPS || fabs(cos(t7)*sin(t6)-grhs_local(0,2)) > IK_EPS)
00646             //  continue;
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     // ROS_INFO("Angle %d = %f out of range: (%f,%f)\n",joint_num,joint_value,min_angles_[joint_num],max_angles_[joint_num]);
00703     return false;
00704   }
00705   return true;
00706 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:53