$search
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