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 +
00057               transform.position.z * transform.position.z);
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:
00125   PR2ArmIK();
00126   ~PR2ArmIK(){};
00127 
00135   bool init(const urdf::ModelInterface& robot_model, const std::string& root_name, const std::string& tip_name);
00136 
00142   void computeIKShoulderPan(const Eigen::Matrix4f& g_in, const double& shoulder_pan_initial_guess,
00143                             std::vector<std::vector<double> >& solution) const;
00144 
00150   void computeIKShoulderRoll(const Eigen::Matrix4f& g_in, const double& shoulder_roll_initial_guess,
00151                              std::vector<std::vector<double> >& solution) const;
00152 
00153   //  std::vector<std::vector<double> > solution_ik_;/// a vector of ik solutions
00154 
00160   void getSolverInfo(moveit_msgs::KinematicSolverInfo& info);
00161 
00165   moveit_msgs::KinematicSolverInfo solver_info_;
00166 
00167   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00168 
00169 private:
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_,
00183       shoulder_elbow_offset_, torso_shoulder_offset_x_, torso_shoulder_offset_y_, torso_shoulder_offset_z_;
00184 
00185   std::vector<double> min_angles_;
00186 
00187   std::vector<double> max_angles_;
00188 
00189   std::vector<bool> continuous_joint_;
00190 };
00191 }
00192 #endif  // PR2_ARM_IK_H


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:44