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) +
00079                                    joint->axis.z * fabs(joint->axis.z));
00080       logDebug("Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x, joint->axis.y, joint->axis.z);
00081       if (joint->type != urdf::Joint::CONTINUOUS)
00082       {
00083         if (joint->safety)
00084         {
00085           min_angles_.push_back(joint->safety->soft_lower_limit);
00086           max_angles_.push_back(joint->safety->soft_upper_limit);
00087         }
00088         else
00089         {
00090           if (joint->limits)
00091           {
00092             min_angles_.push_back(joint->limits->lower);
00093             max_angles_.push_back(joint->limits->upper);
00094           }
00095           else
00096           {
00097             min_angles_.push_back(0.0);
00098             max_angles_.push_back(0.0);
00099             logWarn("No joint limits or joint '%s'", joint->name.c_str());
00100           }
00101         }
00102         continuous_joint_.push_back(false);
00103       }
00104       else
00105       {
00106         min_angles_.push_back(-M_PI);
00107         max_angles_.push_back(M_PI);
00108         continuous_joint_.push_back(true);
00109       }
00110       addJointToChainInfo(link->parent_joint, solver_info_);
00111       num_joints++;
00112     }
00113     link = robot_model.getLink(link->getParent()->name);
00114   }
00115 
00116   solver_info_.link_names.push_back(tip_name);
00117 
00118   //  solver_info_.link_names.push_back(tip_name);
00119   // We expect order from root to tip, so reverse the order
00120   std::reverse(angle_multipliers_.begin(), angle_multipliers_.end());
00121   std::reverse(min_angles_.begin(), min_angles_.end());
00122   std::reverse(max_angles_.begin(), max_angles_.end());
00123   std::reverse(link_offset.begin(), link_offset.end());
00124   std::reverse(solver_info_.limits.begin(), solver_info_.limits.end());
00125   std::reverse(solver_info_.joint_names.begin(), solver_info_.joint_names.end());
00126   std::reverse(solver_info_.link_names.begin(), solver_info_.link_names.end());
00127   std::reverse(continuous_joint_.begin(), continuous_joint_.end());
00128 
00129   if (num_joints != 7)
00130   {
00131     logError("PR2ArmIK:: Chain from %s to %s does not have 7 joints", root_name.c_str(), tip_name.c_str());
00132     return false;
00133   }
00134 
00135   torso_shoulder_offset_x_ = link_offset[0].position.x;
00136   torso_shoulder_offset_y_ = link_offset[0].position.y;
00137   torso_shoulder_offset_z_ = link_offset[0].position.z;
00138   shoulder_upperarm_offset_ = distance(link_offset[1]);
00139   upperarm_elbow_offset_ = distance(link_offset[3]);
00140   elbow_wrist_offset_ = distance(link_offset[5]);
00141   shoulder_elbow_offset_ = shoulder_upperarm_offset_ + upperarm_elbow_offset_;
00142   shoulder_wrist_offset_ = shoulder_upperarm_offset_ + upperarm_elbow_offset_ + elbow_wrist_offset_;
00143 
00144   Eigen::Matrix4f home = Eigen::Matrix4f::Identity();
00145   home(0, 3) = shoulder_upperarm_offset_ + upperarm_elbow_offset_ + elbow_wrist_offset_;
00146   home_inv_ = home.inverse();
00147   grhs_ = home;
00148   gf_ = home_inv_;
00149   solution_.resize(NUM_JOINTS_ARM7DOF);
00150   return true;
00151 }
00152 
00153 void PR2ArmIK::addJointToChainInfo(boost::shared_ptr<const urdf::Joint> joint, moveit_msgs::KinematicSolverInfo& info)
00154 {
00155   moveit_msgs::JointLimits limit;
00156   info.joint_names.push_back(joint->name);  // Joints are coming in reverse order
00157 
00158   if (joint->type != urdf::Joint::CONTINUOUS)
00159   {
00160     if (joint->safety)
00161     {
00162       limit.min_position = joint->safety->soft_lower_limit;
00163       limit.max_position = joint->safety->soft_upper_limit;
00164       limit.has_position_limits = true;
00165     }
00166     else 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 void PR2ArmIK::computeIKShoulderPan(const Eigen::Matrix4f& g_in, const double& t1_in,
00197                                     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   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 =
00243       dd - shoulder_upperarm_offset_ * shoulder_upperarm_offset_ +
00244       2 * shoulder_upperarm_offset_ * shoulder_elbow_offset_ - 2 * shoulder_elbow_offset_ * shoulder_elbow_offset_ +
00245       2 * shoulder_elbow_offset_ * shoulder_wrist_offset_ - shoulder_wrist_offset_ * shoulder_wrist_offset_;
00246   double denominator =
00247       2 * (shoulder_upperarm_offset_ - shoulder_elbow_offset_) * (shoulder_elbow_offset_ - shoulder_wrist_offset_);
00248 
00249   double acosTerm = numerator / denominator;
00250 
00251   if (acosTerm > 1.0 || acosTerm < -1.0)
00252     return;
00253 
00254   double acos_angle = acos(acosTerm);
00255 
00256   theta4[0] = acos_angle;
00257   theta4[1] = -acos_angle;
00258 
00259 #ifdef DEBUG
00260   std::cout << "ComputeIK::theta3:" << numerator << "," << denominator << "," << std::endl << theta4[0] << std::endl;
00261 #endif
00262 
00263   for (int jj = 0; jj < 2; jj++)
00264   {
00265     t4 = theta4[jj];
00266     cost4 = cos(t4);
00267     sint4 = sin(t4);
00268 
00269 #ifdef DEBUG
00270     std::cout << "t4 " << t4 << std::endl;
00271 #endif
00272     if (std::isnan(t4))
00273       continue;
00274 
00275     if (!checkJointLimits(t4, 3))
00276       continue;
00277 
00278     at = x * cost1 + y * sint1 - shoulder_upperarm_offset_;
00279     bt = -z;
00280     ct = -shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
00281          (shoulder_wrist_offset_ - shoulder_elbow_offset_) * cos(t4);
00282 
00283     if (!solveCosineEqn(at, bt, ct, theta2[0], theta2[1]))
00284       continue;
00285 
00286     for (int ii = 0; ii < 2; ii++)
00287     {
00288       t2 = theta2[ii];
00289       if (!checkJointLimits(t2, 1))
00290         continue;
00291 
00292 #ifdef DEBUG
00293       std::cout << "t2 " << t2 << std::endl;
00294 #endif
00295       sint2 = sin(t2);
00296       cost2 = cos(t2);
00297 
00298       at = sint1 * (shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint2 * sint4;
00299       bt = (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost1 * sint4;
00300       ct = y -
00301            (shoulder_upperarm_offset_ +
00302             cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
00303                      (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cos(t4))) *
00304                sint1;
00305       if (!solveCosineEqn(at, bt, ct, theta3[0], theta3[1]))
00306         continue;
00307 
00308       for (int kk = 0; kk < 2; kk++)
00309       {
00310         t3 = theta3[kk];
00311 
00312         if (!checkJointLimits(angles::normalize_angle(t3), 2))
00313           continue;
00314 
00315         sint3 = sin(t3);
00316         cost3 = cos(t3);
00317 #ifdef DEBUG
00318         std::cout << "t3 " << t3 << std::endl;
00319 #endif
00320         if (fabs((shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
00321                   (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost4) *
00322                      sint2 +
00323                  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost2 * cost3 * sint4 - z) > IK_EPS)
00324           continue;
00325 
00326         if (fabs((shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint1 * sint3 * sint4 +
00327                  cost1 * (shoulder_upperarm_offset_ +
00328                           cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
00329                                    (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost4) +
00330                           (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost3 * sint2 * sint4) -
00331                  x) > IK_EPS)
00332           continue;
00333 
00334         grhs_local(0, 0) =
00335             cost4 * (gf_local(0, 0) * cost1 * cost2 + gf_local(1, 0) * cost2 * sint1 - gf_local(2, 0) * sint2) -
00336             (gf_local(2, 0) * cost2 * cost3 + cost3 * (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 +
00337              (-(gf_local(1, 0) * cost1) + gf_local(0, 0) * sint1) * sint3) *
00338                 sint4;
00339 
00340         grhs_local(0, 1) =
00341             cost4 * (gf_local(0, 1) * cost1 * cost2 + gf_local(1, 1) * cost2 * sint1 - gf_local(2, 1) * sint2) -
00342             (gf_local(2, 1) * cost2 * cost3 + cost3 * (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 +
00343              (-(gf_local(1, 1) * cost1) + gf_local(0, 1) * sint1) * sint3) *
00344                 sint4;
00345 
00346         grhs_local(0, 2) =
00347             cost4 * (gf_local(0, 2) * cost1 * cost2 + gf_local(1, 2) * cost2 * sint1 - gf_local(2, 2) * sint2) -
00348             (gf_local(2, 2) * cost2 * cost3 + cost3 * (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 +
00349              (-(gf_local(1, 2) * cost1) + gf_local(0, 2) * sint1) * sint3) *
00350                 sint4;
00351 
00352         grhs_local(1, 0) = cost3 * (gf_local(1, 0) * cost1 - gf_local(0, 0) * sint1) + gf_local(2, 0) * cost2 * sint3 +
00353                            (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 * sint3;
00354 
00355         grhs_local(1, 1) = cost3 * (gf_local(1, 1) * cost1 - gf_local(0, 1) * sint1) + gf_local(2, 1) * cost2 * sint3 +
00356                            (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 * sint3;
00357 
00358         grhs_local(1, 2) = cost3 * (gf_local(1, 2) * cost1 - gf_local(0, 2) * sint1) + gf_local(2, 2) * cost2 * sint3 +
00359                            (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 * sint3;
00360 
00361         grhs_local(2, 0) =
00362             cost4 *
00363                 (gf_local(2, 0) * cost2 * cost3 + cost3 * (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 +
00364                  (-(gf_local(1, 0) * cost1) + gf_local(0, 0) * sint1) * sint3) +
00365             (gf_local(0, 0) * cost1 * cost2 + gf_local(1, 0) * cost2 * sint1 - gf_local(2, 0) * sint2) * sint4;
00366 
00367         grhs_local(2, 1) =
00368             cost4 *
00369                 (gf_local(2, 1) * cost2 * cost3 + cost3 * (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 +
00370                  (-(gf_local(1, 1) * cost1) + gf_local(0, 1) * sint1) * sint3) +
00371             (gf_local(0, 1) * cost1 * cost2 + gf_local(1, 1) * cost2 * sint1 - gf_local(2, 1) * sint2) * sint4;
00372 
00373         grhs_local(2, 2) =
00374             cost4 *
00375                 (gf_local(2, 2) * cost2 * cost3 + cost3 * (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 +
00376                  (-(gf_local(1, 2) * cost1) + gf_local(0, 2) * sint1) * sint3) +
00377             (gf_local(0, 2) * cost1 * cost2 + gf_local(1, 2) * cost2 * sint1 - gf_local(2, 2) * sint2) * sint4;
00378 
00379         double val1 = sqrt(grhs_local(0, 1) * grhs_local(0, 1) + grhs_local(0, 2) * grhs_local(0, 2));
00380         double val2 = grhs_local(0, 0);
00381 
00382         theta6[0] = atan2(val1, val2);
00383         theta6[1] = atan2(-val1, val2);
00384 
00385         //            theta6[3] = M_PI + theta6[0];
00386         //            theta6[4] = M_PI + theta6[1];
00387 
00388         for (int mm = 0; mm < 2; mm++)
00389         {
00390           t6 = theta6[mm];
00391           if (!checkJointLimits(angles::normalize_angle(t6), 5))
00392             continue;
00393 
00394 #ifdef DEBUG
00395           std::cout << "t6 " << t6 << std::endl;
00396 #endif
00397           if (fabs(cos(t6) - grhs_local(0, 0)) > IK_EPS)
00398             continue;
00399 
00400           if (fabs(sin(t6)) < IK_EPS)
00401           {
00402             //                std::cout << "Singularity" << std::endl;
00403             theta5[0] = acos(grhs_local(1, 1)) / 2.0;
00404             theta7[0] = theta7[0];
00405             theta7[1] = M_PI + theta7[0];
00406             theta5[1] = theta7[1];
00407           }
00408           else
00409           {
00410             theta7[0] = atan2(grhs_local(0, 1), grhs_local(0, 2));
00411             theta5[0] = atan2(grhs_local(1, 0), -grhs_local(2, 0));
00412             theta7[1] = M_PI + theta7[0];
00413             theta5[1] = M_PI + theta5[0];
00414           }
00415 #ifdef DEBUG
00416           std::cout << "theta1: " << t1 << std::endl;
00417           std::cout << "theta2: " << t2 << std::endl;
00418           std::cout << "theta3: " << t3 << std::endl;
00419           std::cout << "theta4: " << t4 << std::endl;
00420           std::cout << "theta5: " << t5 << std::endl;
00421           std::cout << "theta6: " << t6 << std::endl;
00422           std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
00423 #endif
00424           for (int lll = 0; lll < 2; lll++)
00425           {
00426             t5 = theta5[lll];
00427             t7 = theta7[lll];
00428             if (!checkJointLimits(t5, 4))
00429               continue;
00430             if (!checkJointLimits(t7, 6))
00431               continue;
00432 
00433 #ifdef DEBUG
00434             std::cout << "t5" << t5 << std::endl;
00435             std::cout << "t7" << t7 << std::endl;
00436 #endif
00437             if (fabs(sin(t6) * sin(t7) - grhs_local(0, 1)) > IK_EPS ||
00438                 fabs(cos(t7) * sin(t6) - grhs_local(0, 2)) > IK_EPS)
00439               continue;
00440 
00441             solution_ik[0] = normalize_angle(t1) * angle_multipliers_[0];
00442             solution_ik[1] = normalize_angle(t2) * angle_multipliers_[1];
00443             solution_ik[2] = normalize_angle(t3) * angle_multipliers_[2];
00444             solution_ik[3] = normalize_angle(t4) * angle_multipliers_[3];
00445             solution_ik[4] = normalize_angle(t5) * angle_multipliers_[4];
00446             solution_ik[5] = normalize_angle(t6) * angle_multipliers_[5];
00447             solution_ik[6] = normalize_angle(t7) * angle_multipliers_[6];
00448             solution.push_back(solution_ik);
00449 
00450 #ifdef DEBUG
00451             std::cout << "SOLN " << solution_ik[0] << " " << solution_ik[1] << " " << solution_ik[2] << " "
00452                       << solution_ik[3] << " " << solution_ik[4] << " " << solution_ik[5] << " " << solution_ik[6]
00453                       << std::endl
00454                       << std::endl;
00455 #endif
00456           }
00457         }
00458       }
00459     }
00460   }
00461 }
00462 
00463 void PR2ArmIK::computeIKShoulderRoll(const Eigen::Matrix4f& g_in, const double& t3,
00464                                      std::vector<std::vector<double> >& solution) const
00465 {
00466   std::vector<double> solution_ik(NUM_JOINTS_ARM7DOF, 0.0);
00467   //  ROS_INFO(" ");
00468   // solution_ik_.clear();
00469   //  ROS_INFO("Solution IK size: %d",solution_ik_.size());
00470   //  for(unsigned int i=0; i < solution_ik_.size(); i++)
00471   //  {
00472   //    solution_ik_[i].clear();
00473   //  }
00474   //  if(!solution_ik_.empty())
00475   //    solution_ik_.resize(0);
00476   // t3 = shoulder/turret roll is specified
00477   Eigen::Matrix4f g = g_in;
00478   Eigen::Matrix4f gf_local = home_inv_;
00479   Eigen::Matrix4f grhs_local = home_inv_;
00480   // First bring everything into the arm frame
00481   g(0, 3) = g_in(0, 3) - torso_shoulder_offset_x_;
00482   g(1, 3) = g_in(1, 3) - torso_shoulder_offset_y_;
00483   g(2, 3) = g_in(2, 3) - torso_shoulder_offset_z_;
00484 
00485   if (!checkJointLimits(t3, 2))
00486   {
00487     return;
00488   }
00489   double x = g(0, 3);
00490   double y = g(1, 3);
00491   double z = g(2, 3);
00492   double cost1, cost2, cost3, cost4;
00493   double sint1, sint2, sint3, sint4;
00494 
00495   gf_local = g * home_inv_;
00496 
00497   cost3 = cos(t3);
00498   sint3 = sin(t3);
00499 
00500   double t1(0), t2(0), t4(0), t5(0), t6(0), t7(0);
00501 
00502   double at(0), bt(0), ct(0);
00503 
00504   double theta1[2], theta2[2], theta4[4], theta5[2], theta6[4], theta7[2];
00505 
00506   double c0 = -sin(-t3) * elbow_wrist_offset_;
00507   double c1 = -cos(-t3) * elbow_wrist_offset_;
00508 
00509   double d0 = 4 * shoulder_upperarm_offset_ * shoulder_upperarm_offset_ *
00510               (upperarm_elbow_offset_ * upperarm_elbow_offset_ + c1 * c1 - z * z);
00511   double d1 = 8 * shoulder_upperarm_offset_ * shoulder_upperarm_offset_ * upperarm_elbow_offset_ * elbow_wrist_offset_;
00512   double d2 =
00513       4 * shoulder_upperarm_offset_ * shoulder_upperarm_offset_ * (elbow_wrist_offset_ * elbow_wrist_offset_ - c1 * c1);
00514 
00515   double b0 = x * x + y * y + z * z - shoulder_upperarm_offset_ * shoulder_upperarm_offset_ -
00516               upperarm_elbow_offset_ * upperarm_elbow_offset_ - c0 * c0 - c1 * c1;
00517   double b1 = -2 * upperarm_elbow_offset_ * elbow_wrist_offset_;
00518 
00519   if (!solveQuadratic(b1 * b1 - d2, 2 * b0 * b1 - d1, b0 * b0 - d0, &theta4[0], &theta4[1]))
00520   {
00521 #ifdef DEBUG
00522     printf("No solution to quadratic eqn\n");
00523 #endif
00524     return;
00525   }
00526   theta4[0] = acos(theta4[0]);
00527   theta4[2] = acos(theta4[1]);
00528   theta4[1] = -theta4[0];
00529   theta4[3] = -theta4[2];
00530 
00531   for (int jj = 0; jj < 4; jj++)
00532   {
00533     t4 = theta4[jj];
00534 
00535     if (!checkJointLimits(t4, 3))
00536     {
00537       continue;
00538     }
00539     cost4 = cos(t4);
00540     sint4 = sin(t4);
00541 #ifdef DEBUG
00542     std::cout << "t4 " << t4 << std::endl;
00543 #endif
00544     if (std::isnan(t4))
00545       continue;
00546     at = cos(t3) * sin(t4) * (shoulder_elbow_offset_ - shoulder_wrist_offset_);
00547     bt = (shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
00548           (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cos(t4));
00549     ct = z;
00550 
00551     if (!solveCosineEqn(at, bt, ct, theta2[0], theta2[1]))
00552       continue;
00553 
00554     for (int ii = 0; ii < 2; ii++)
00555     {
00556       t2 = theta2[ii];
00557 #ifdef DEBUG
00558       std::cout << "t2 " << t2 << std::endl;
00559 #endif
00560       if (!checkJointLimits(t2, 1))
00561       {
00562         continue;
00563       }
00564 
00565       sint2 = sin(t2);
00566       cost2 = cos(t2);
00567 
00568       at = -y;
00569       bt = x;
00570       ct = (shoulder_elbow_offset_ - shoulder_wrist_offset_) * sin(t3) * sin(t4);
00571       if (!solveCosineEqn(at, bt, ct, theta1[0], theta1[1]))
00572       {
00573 #ifdef DEBUG
00574         std::cout << "could not solve cosine equation for t1" << std::endl;
00575 #endif
00576         continue;
00577       }
00578 
00579       for (int kk = 0; kk < 2; kk++)
00580       {
00581         t1 = theta1[kk];
00582 #ifdef DEBUG
00583         std::cout << "t1 " << t1 << std::endl;
00584 #endif
00585         if (!checkJointLimits(t1, 0))
00586         {
00587           continue;
00588         }
00589         sint1 = sin(t1);
00590         cost1 = cos(t1);
00591         if (fabs((shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
00592                   (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost4) *
00593                      sint2 +
00594                  (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost2 * cost3 * sint4 - z) > IK_EPS)
00595         {
00596 #ifdef DEBUG
00597           printf("z value not matched %f\n",
00598                  fabs((shoulder_upperarm_offset_ - shoulder_elbow_offset_ +
00599                        (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost4) *
00600                           sint2 +
00601                       (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost2 * cost3 * sint4 - z));
00602 #endif
00603           continue;
00604         }
00605         if (fabs((shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint1 * sint3 * sint4 +
00606                  cost1 * (shoulder_upperarm_offset_ +
00607                           cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
00608                                    (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost4) +
00609                           (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost3 * sint2 * sint4) -
00610                  x) > IK_EPS)
00611         {
00612 #ifdef DEBUG
00613           printf("x value not matched by %f\n",
00614                  fabs((shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint1 * sint3 * sint4 +
00615                       cost1 * (shoulder_upperarm_offset_ +
00616                                cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
00617                                         (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost4) +
00618                                (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost3 * sint2 * sint4) -
00619                       x));
00620 #endif
00621           continue;
00622         }
00623         if (fabs(-(shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost1 * sint3 * sint4 +
00624                  sint1 * (shoulder_upperarm_offset_ +
00625                           cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ +
00626                                    (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost4) +
00627                           (shoulder_elbow_offset_ - shoulder_wrist_offset_) * cost3 * sint2 * sint4) -
00628                  y) > IK_EPS)
00629         {
00630 #ifdef DEBUG
00631           printf("y value not matched\n");
00632 #endif
00633           continue;
00634         }
00635         grhs_local(0, 0) =
00636             cost4 * (gf_local(0, 0) * cost1 * cost2 + gf_local(1, 0) * cost2 * sint1 - gf_local(2, 0) * sint2) -
00637             (gf_local(2, 0) * cost2 * cost3 + cost3 * (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 +
00638              (-(gf_local(1, 0) * cost1) + gf_local(0, 0) * sint1) * sint3) *
00639                 sint4;
00640 
00641         grhs_local(0, 1) =
00642             cost4 * (gf_local(0, 1) * cost1 * cost2 + gf_local(1, 1) * cost2 * sint1 - gf_local(2, 1) * sint2) -
00643             (gf_local(2, 1) * cost2 * cost3 + cost3 * (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 +
00644              (-(gf_local(1, 1) * cost1) + gf_local(0, 1) * sint1) * sint3) *
00645                 sint4;
00646 
00647         grhs_local(0, 2) =
00648             cost4 * (gf_local(0, 2) * cost1 * cost2 + gf_local(1, 2) * cost2 * sint1 - gf_local(2, 2) * sint2) -
00649             (gf_local(2, 2) * cost2 * cost3 + cost3 * (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 +
00650              (-(gf_local(1, 2) * cost1) + gf_local(0, 2) * sint1) * sint3) *
00651                 sint4;
00652 
00653         grhs_local(1, 0) = cost3 * (gf_local(1, 0) * cost1 - gf_local(0, 0) * sint1) + gf_local(2, 0) * cost2 * sint3 +
00654                            (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 * sint3;
00655 
00656         grhs_local(1, 1) = cost3 * (gf_local(1, 1) * cost1 - gf_local(0, 1) * sint1) + gf_local(2, 1) * cost2 * sint3 +
00657                            (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 * sint3;
00658 
00659         grhs_local(1, 2) = cost3 * (gf_local(1, 2) * cost1 - gf_local(0, 2) * sint1) + gf_local(2, 2) * cost2 * sint3 +
00660                            (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 * sint3;
00661 
00662         grhs_local(2, 0) =
00663             cost4 *
00664                 (gf_local(2, 0) * cost2 * cost3 + cost3 * (gf_local(0, 0) * cost1 + gf_local(1, 0) * sint1) * sint2 +
00665                  (-(gf_local(1, 0) * cost1) + gf_local(0, 0) * sint1) * sint3) +
00666             (gf_local(0, 0) * cost1 * cost2 + gf_local(1, 0) * cost2 * sint1 - gf_local(2, 0) * sint2) * sint4;
00667 
00668         grhs_local(2, 1) =
00669             cost4 *
00670                 (gf_local(2, 1) * cost2 * cost3 + cost3 * (gf_local(0, 1) * cost1 + gf_local(1, 1) * sint1) * sint2 +
00671                  (-(gf_local(1, 1) * cost1) + gf_local(0, 1) * sint1) * sint3) +
00672             (gf_local(0, 1) * cost1 * cost2 + gf_local(1, 1) * cost2 * sint1 - gf_local(2, 1) * sint2) * sint4;
00673 
00674         grhs_local(2, 2) =
00675             cost4 *
00676                 (gf_local(2, 2) * cost2 * cost3 + cost3 * (gf_local(0, 2) * cost1 + gf_local(1, 2) * sint1) * sint2 +
00677                  (-(gf_local(1, 2) * cost1) + gf_local(0, 2) * sint1) * sint3) +
00678             (gf_local(0, 2) * cost1 * cost2 + gf_local(1, 2) * cost2 * sint1 - gf_local(2, 2) * sint2) * sint4;
00679 
00680         double val1 = sqrt(grhs_local(0, 1) * grhs_local(0, 1) + grhs_local(0, 2) * grhs_local(0, 2));
00681         double val2 = grhs_local(0, 0);
00682 
00683         theta6[0] = atan2(val1, val2);
00684         theta6[1] = atan2(-val1, val2);
00685 
00686         for (int mm = 0; mm < 2; mm++)
00687         {
00688           t6 = theta6[mm];
00689 #ifdef DEBUG
00690           std::cout << "t6 " << t6 << std::endl;
00691 #endif
00692           if (!checkJointLimits(t6, 5))
00693           {
00694             continue;
00695           }
00696 
00697           if (fabs(cos(t6) - grhs_local(0, 0)) > IK_EPS)
00698             continue;
00699 
00700           if (fabs(sin(t6)) < IK_EPS)
00701           {
00702             //                std::cout << "Singularity" << std::endl;
00703             theta5[0] = acos(grhs_local(1, 1)) / 2.0;
00704             theta7[0] = theta5[0];
00705             //            theta7[1] = M_PI+theta7[0];
00706             //            theta5[1] = theta7[1];
00707           }
00708           else
00709           {
00710             theta7[0] = atan2(grhs_local(0, 1) / sin(t6), grhs_local(0, 2) / sin(t6));
00711             theta5[0] = atan2(grhs_local(1, 0) / sin(t6), -grhs_local(2, 0) / sin(t6));
00712             //            theta7[1] = M_PI+theta7[0];
00713             //            theta5[1] = M_PI+theta5[0];
00714           }
00715           for (int lll = 0; lll < 1; lll++)
00716           {
00717             t5 = theta5[lll];
00718             t7 = theta7[lll];
00719 
00720             if (!checkJointLimits(t5, 4))
00721             {
00722               continue;
00723             }
00724             if (!checkJointLimits(t7, 6))
00725             {
00726               continue;
00727             }
00728 
00729 #ifdef DEBUG
00730             std::cout << "t5 " << t5 << std::endl;
00731             std::cout << "t7 " << t7 << std::endl;
00732 #endif
00733 //           if(fabs(sin(t6)*sin(t7)-grhs_local(0,1)) > IK_EPS || fabs(cos(t7)*sin(t6)-grhs_local(0,2)) > IK_EPS)
00734 //  continue;
00735 
00736 #ifdef DEBUG
00737             std::cout << "theta1: " << t1 << std::endl;
00738             std::cout << "theta2: " << t2 << std::endl;
00739             std::cout << "theta3: " << t3 << std::endl;
00740             std::cout << "theta4: " << t4 << std::endl;
00741             std::cout << "theta5: " << t5 << std::endl;
00742             std::cout << "theta6: " << t6 << std::endl;
00743             std::cout << "theta7: " << t7 << std::endl << std::endl << std::endl;
00744 #endif
00745 
00746             solution_ik[0] = normalize_angle(t1 * angle_multipliers_[0]);
00747             solution_ik[1] = normalize_angle(t2 * angle_multipliers_[1]);
00748             solution_ik[2] = t3 * angle_multipliers_[2];
00749             solution_ik[3] = normalize_angle(t4 * angle_multipliers_[3]);
00750             solution_ik[4] = normalize_angle(t5 * angle_multipliers_[4]);
00751             solution_ik[5] = normalize_angle(t6 * angle_multipliers_[5]);
00752             solution_ik[6] = normalize_angle(t7 * angle_multipliers_[6]);
00753             solution.push_back(solution_ik);
00754 #ifdef DEBUG
00755             std::cout << "SOLN " << solution_ik[0] << " " << solution_ik[1] << " " << solution_ik[2] << " "
00756                       << solution_ik[3] << " " << solution_ik[4] << " " << solution_ik[5] << " " << solution_ik[6]
00757                       << std::endl
00758                       << std::endl;
00759 #endif
00760           }
00761         }
00762       }
00763     }
00764   }
00765 }
00766 
00767 bool PR2ArmIK::checkJointLimits(const std::vector<double>& joint_values) const
00768 {
00769   for (int i = 0; i < NUM_JOINTS_ARM7DOF; i++)
00770   {
00771     if (!checkJointLimits(angles::normalize_angle(joint_values[i] * angle_multipliers_[i]), i))
00772     {
00773       return false;
00774     }
00775   }
00776   return true;
00777 }
00778 
00779 bool PR2ArmIK::checkJointLimits(const double& joint_value, const int& joint_num) const
00780 {
00781   double jv;
00782   if (continuous_joint_[joint_num])
00783     jv = angles::normalize_angle(joint_value * angle_multipliers_[joint_num]);
00784   else if (joint_num == 2)
00785     jv = joint_value * angle_multipliers_[joint_num];
00786   else
00787     jv = angles::normalize_angle(joint_value * angle_multipliers_[joint_num]);
00788 
00789   if (jv < min_angles_[joint_num] || jv > max_angles_[joint_num])
00790   {
00791     // ROS_INFO("Angle %d = %f out of range:
00792     // (%f,%f)\n",joint_num,joint_value,min_angles_[joint_num],max_angles_[joint_num]);
00793     return false;
00794   }
00795   return true;
00796 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Apr 20 2018 03:31:11