pr2_arm_ik.h
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 #ifndef PR2_ARM_IK_H
00038 #define PR2_ARM_IK_H
00039 
00040 #include <urdf_model/model.h>
00041 #include <Eigen/Core>
00042 #include <Eigen/LU>// provides LU decomposition
00043 #include <kdl/chainiksolver.hpp>
00044 #include <moveit_msgs/GetPositionFK.h>
00045 #include <moveit_msgs/GetPositionIK.h>
00046 #include <moveit_msgs/GetKinematicSolverInfo.h>
00047 
00048 namespace pr2_arm_kinematics
00049 {
00050 static const int NUM_JOINTS_ARM7DOF = 7;
00051 
00052 static const double IK_EPS = 1e-5;
00053 
00054 inline double distance(const urdf::Pose &transform)
00055 {
00056   return sqrt(transform.position.x*transform.position.x+transform.position.y*transform.position.y+transform.position.z*transform.position.z);
00057 }
00058 
00059 
00060 inline bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
00061 {
00062   double discriminant = b*b-4*a*c;
00063   if(fabs(a) < IK_EPS)
00064   {
00065     *x1 = -c/b;
00066     *x2 = *x1;
00067     return true;
00068   }
00069   //ROS_DEBUG("Discriminant: %f\n",discriminant);
00070   if (discriminant >= 0)
00071   {
00072     *x1 = (-b + sqrt(discriminant))/(2*a);
00073     *x2 = (-b - sqrt(discriminant))/(2*a);
00074     return true;
00075   }
00076   else if(fabs(discriminant) < IK_EPS)
00077   {
00078     *x1 = -b/(2*a);
00079     *x2 = -b/(2*a);
00080     return true;
00081   }
00082   else
00083   {
00084     *x1 = -b/(2*a);
00085     *x2 = -b/(2*a);
00086     return false;
00087   }
00088 }
00089 
00090 inline bool solveCosineEqn(const double &a, const double &b, const double &c, double &soln1, double &soln2)
00091 {
00092   double theta1 = atan2(b,a);
00093   double denom  = sqrt(a*a+b*b);
00094 
00095   if(fabs(denom) < IK_EPS) // should never happen, wouldn't make sense but make sure it is checked nonetheless
00096   {
00097 #ifdef DEBUG
00098     std::cout << "denom: " << denom << std::endl;
00099 #endif
00100     return false;
00101   }
00102   double rhs_ratio = c/denom;
00103   if(rhs_ratio < -1 || rhs_ratio > 1)
00104   {
00105 #ifdef DEBUG
00106     std::cout << "rhs_ratio: " << rhs_ratio << std::endl;
00107 #endif
00108     return false;
00109   }
00110   double acos_term = acos(rhs_ratio);
00111   soln1 = theta1 + acos_term;
00112   soln2 = theta1 - acos_term;
00113 
00114   return true;
00115 }
00116 
00117 class PR2ArmIK
00118 {
00119 public:
00120 
00126   PR2ArmIK();
00127   ~PR2ArmIK(){};
00128 
00136   bool init(const urdf::ModelInterface &robot_model, const std::string &root_name, const std::string &tip_name);
00137 
00143   void computeIKShoulderPan(const Eigen::Matrix4f &g_in, const double &shoulder_pan_initial_guess,std::vector<std::vector<double> > &solution) const;
00144 
00150   void computeIKShoulderRoll(const Eigen::Matrix4f &g_in, const double &shoulder_roll_initial_guess,std::vector<std::vector<double> > &solution) const;
00151 
00152 
00153   //  std::vector<std::vector<double> > solution_ik_;/// a vector of ik solutions
00154 
00159   void getSolverInfo(moveit_msgs::KinematicSolverInfo &info);
00160 
00164   moveit_msgs::KinematicSolverInfo solver_info_;
00165 
00166   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00167 
00168   private:
00169 
00170   void addJointToChainInfo(boost::shared_ptr<const urdf::Joint> joint,moveit_msgs::KinematicSolverInfo &info);
00171 
00172   bool checkJointLimits(const std::vector<double> &joint_values) const;
00173 
00174   bool checkJointLimits(const double &joint_value, const int &joint_num) const;
00175 
00176   Eigen::Matrix4f grhs_, gf_, home_inv_, home_;
00177 
00178   std::vector<double> angle_multipliers_;
00179 
00180   std::vector<double> solution_;
00181 
00182   double shoulder_upperarm_offset_, upperarm_elbow_offset_, elbow_wrist_offset_, shoulder_wrist_offset_, shoulder_elbow_offset_, torso_shoulder_offset_x_, torso_shoulder_offset_y_, torso_shoulder_offset_z_;
00183 
00184   std::vector<double> min_angles_;
00185 
00186   std::vector<double> max_angles_;
00187 
00188   std::vector<bool> continuous_joint_;
00189 
00190 };
00191 }
00192 #endif// PR2_ARM_IK_H


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