pr2_arm_ik.h
Go to the documentation of this file.
00001 //Software License Agreement (BSD License)
00002 
00003 //Copyright (c) 2008, Willow Garage, Inc.
00004 //All rights reserved.
00005 
00006 //Redistribution and use in source and binary forms, with or without
00007 //modification, are permitted provided that the following conditions
00008 //are met:
00009 
00010 // * Redistributions of source code must retain the above copyright
00011 //   notice, this list of conditions and the following disclaimer.
00012 // * Redistributions in binary form must reproduce the above
00013 //   copyright notice, this list of conditions and the following
00014 //   disclaimer in the documentation and/or other materials provided
00015 //   with the distribution.
00016 // * Neither the name of Willow Garage, Inc. nor the names of its
00017 //   contributors may be used to endorse or promote products derived
00018 //   from this software without specific prior written permission.
00019 
00020 //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 //"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 //LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 //FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 //COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 //INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 //BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 //LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 //CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 //LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 //ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 //POSSIBILITY OF SUCH DAMAGE.
00032 
00033 #ifndef PR2_ARM_IK_H
00034 #define PR2_ARM_IK_H
00035 
00036 #include <urdf/model.h>
00037 #include <Eigen/Core>
00038 #include <Eigen/LU>// provides LU decomposition
00039 #include <kdl/chainiksolver.hpp>
00040 #include <pr2_arm_kinematics/pr2_arm_kinematics_utils.h>
00041 #include <pr2_arm_kinematics/pr2_arm_kinematics_constants.h>
00042 
00043 
00044 namespace pr2_arm_kinematics
00045 {
00046 class PR2ArmIK
00047 {
00048 public:
00049     
00055   PR2ArmIK();    
00056   ~PR2ArmIK(){};
00057 
00065   bool init(const urdf::Model &robot_model, const std::string &root_name, const std::string &tip_name);
00066 
00072   void computeIKShoulderPan(const Eigen::Matrix4f &g_in, const double &shoulder_pan_initial_guess,std::vector<std::vector<double> > &solution);
00073 
00079   void computeIKShoulderRoll(const Eigen::Matrix4f &g_in, const double &shoulder_roll_initial_guess,std::vector<std::vector<double> > &solution);
00080 
00081     
00082   //  std::vector<std::vector<double> > solution_ik_;/// a vector of ik solutions
00083 
00088   void getSolverInfo(kinematics_msgs::KinematicSolverInfo &info);
00089 
00093   kinematics_msgs::KinematicSolverInfo solver_info_;
00094 
00095   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00096 
00097   private:
00098 
00099   void addJointToChainInfo(boost::shared_ptr<const urdf::Joint> joint,kinematics_msgs::KinematicSolverInfo &info);
00100 
00101   bool checkJointLimits(const std::vector<double> &joint_values);
00102  
00103   bool checkJointLimits(const double &joint_value, const int &joint_num);
00104 
00105   Eigen::Matrix4f grhs_, gf_, home_inv_, home_;
00106 
00107   std::vector<double> angle_multipliers_;
00108 
00109   std::vector<double> solution_;
00110 
00111   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_;
00112 
00113   std::vector<double> min_angles_;
00114 
00115   std::vector<double> max_angles_;
00116 
00117   std::vector<bool> continuous_joint_;
00118 
00119 };
00120 }
00121 #endif// PR2_ARM_IK_H


pr2_arm_kinematics
Author(s): Sachin Chitta
autogenerated on Tue Apr 22 2014 19:33:02