pr2_arm_ik_solver.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_SOLVER_H
00034 #define PR2_ARM_IK_SOLVER_H
00035 
00036 #include <urdf/model.h>
00037 #include <Eigen/Core>
00038 #include <kdl/chainiksolver.hpp>
00039 #include <pr2_arm_kinematics/pr2_arm_ik.h>
00040 #include <pr2_arm_kinematics/pr2_arm_kinematics_utils.h>
00041 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00042 #include <kinematics_msgs/PositionIKRequest.h>
00043 #include <geometry_msgs/PoseStamped.h>
00044 #include <tf_conversions/tf_kdl.h>
00045 
00046 namespace pr2_arm_kinematics
00047 {
00048 
00049 static const int NO_IK_SOLUTION = -1;
00050 static const int TIMED_OUT = -2;
00051 
00052   class PR2ArmIKSolver : public KDL::ChainIkSolverPos
00053   {
00054     public:
00055 
00056     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00057     
00065     PR2ArmIKSolver(const urdf::Model &robot_model, 
00066                    const std::string &root_frame_name,
00067                    const std::string &tip_frame_name,
00068                    const double &search_discretization_angle, 
00069                    const int &free_angle);
00070 
00071     ~PR2ArmIKSolver(){};
00072 
00076     PR2ArmIK pr2_arm_ik_;
00077 
00081     bool active_;
00082 
00094     int CartToJnt(const KDL::JntArray& q_init, 
00095                   const KDL::Frame& p_in, 
00096                   KDL::JntArray& q_out);
00097 
00109     int CartToJnt(const KDL::JntArray& q_init, 
00110                   const KDL::Frame& p_in, 
00111                   std::vector<KDL::JntArray> &q_out);
00112   
00123     int CartToJntSearch(const KDL::JntArray& q_in, 
00124                         const KDL::Frame& p_in, 
00125                         std::vector<KDL::JntArray> &q_out, 
00126                         const double &timeout);
00127 
00138     int CartToJntSearch(const KDL::JntArray& q_in, 
00139                         const KDL::Frame& p_in, 
00140                         KDL::JntArray &q_out, 
00141                         const double &timeout);
00142 
00143     int CartToJntSearch(const KDL::JntArray& q_in, 
00144                         const KDL::Frame& p_in, 
00145                         const double& consistency_limit, 
00146                         KDL::JntArray &q_out, 
00147                         const double &timeout);
00152     void getSolverInfo(kinematics_msgs::KinematicSolverInfo &response);
00153 
00164     int CartToJntSearch(const KDL::JntArray& q_in, 
00165                         const KDL::Frame& p_in, 
00166                         KDL::JntArray &q_out, 
00167                         const double &timeout, 
00168                         arm_navigation_msgs::ArmNavigationErrorCodes &error_code,
00169                         const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &desired_pose_callback,
00170                         const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &solution_callback);
00171 
00172     int CartToJntSearch(const KDL::JntArray& q_in, 
00173                         const KDL::Frame& p_in, 
00174                         KDL::JntArray &q_out, 
00175                         const double &timeout, 
00176                         const double& consistency_limit, 
00177                         arm_navigation_msgs::ArmNavigationErrorCodes &error_code,
00178                         const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &desired_pose_callback,
00179                         const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &solution_callback);
00180 
00181 
00182 
00183     std::string getFrameId();
00184 
00185     unsigned int getFreeAngle() const {
00186       return free_angle_;
00187     }
00188 
00189     void setFreeAngle(const unsigned int& free_angle) {
00190       free_angle_ = free_angle;
00191     }
00192 
00193   private:
00194 
00195     bool getCount(int &count, const int &max_count, const int &min_count);
00196 
00197     double search_discretization_angle_;
00198 
00199     int free_angle_;
00200 
00201     std::string root_frame_name_;
00202   };
00203 }
00204 #endif// PR2_ARM_IK_SOLVER_H


pr2_arm_kinematics
Author(s): Sachin Chitta
autogenerated on Thu Jan 2 2014 11:40:43