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


pr2_moveit_plugins
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Thu Jul 4 2019 19:51:07