pr2_arm_ik_solver.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 MOVEIT_PR2_ARM_IK_SOLVER_
00038 #define MOVEIT_PR2_ARM_IK_SOLVER_
00039 
00040 #include <urdf/model.h>
00041 #include <Eigen/Core>
00042 #include <kdl/chainiksolver.hpp>
00043 #include <pr2_arm_kinematics/pr2_arm_ik.h>
00044 #include <moveit/kinematics_base/kinematics_base.h>
00045 #include <pr2_arm_kinematics/pr2_arm_kinematics_utils.h>
00046 #include <moveit_msgs/GetKinematicSolverInfo.h>
00047 #include <moveit_msgs/PositionIKRequest.h>
00048 #include <geometry_msgs/PoseStamped.h>
00049 #include <tf_conversions/tf_kdl.h>
00050 
00051 namespace pr2_arm_kinematics
00052 {
00053 
00054 static const int NO_IK_SOLUTION = -1;
00055 static const int TIMED_OUT = -2;
00056 
00057   class PR2ArmIKSolver : public KDL::ChainIkSolverPos
00058   {
00059     public:
00060 
00066     PR2ArmIKSolver(const urdf::Model &robot_model,
00067                    const std::string &root_frame_name,
00068                    const std::string &tip_frame_name,
00069                    const double &search_discretization_angle,
00070                    const int &free_angle);
00071 
00072     ~PR2ArmIKSolver(){ delete pr2_arm_ik_; };
00073 
00077     PR2ArmIK *pr2_arm_ik_;
00078 
00082     bool active_;
00083 
00096     int CartToJnt(const KDL::JntArray& q_init,
00097                   const KDL::Frame& p_in,
00098                   KDL::JntArray& q_out);
00099 
00112     int CartToJnt(const KDL::JntArray& q_init,
00113                   const KDL::Frame& p_in,
00114                   std::vector<KDL::JntArray> &q_out);
00115 
00127     int CartToJntSearch(const KDL::JntArray& q_in,
00128                         const KDL::Frame& p_in,
00129                         std::vector<KDL::JntArray> &q_out,
00130                         const double &timeout);
00131 
00144 //  
00145 //  Undefined in pr2_arm_ik_solver.cpp, so commented out dash@clearpathrobotics.com
00146  //int CartToJntSearch(const KDL::JntArray& q_in,
00147  //                       const KDL::Frame& p_in,
00148  //                       KDL::JntArray &q_out,
00149  //                       const double &timeout);
00150 
00165     int CartToJntSearch(const KDL::JntArray& q_in,
00166                         const KDL::Frame& p_in,
00167                         KDL::JntArray &q_out,
00168                         const double &timeout,
00169                         const double& consistency_limit);
00170 
00184     int CartToJntSearch(const KDL::JntArray& q_in,
00185                         const KDL::Frame& p_in,
00186                         KDL::JntArray &q_out,
00187                         const double &timeout,
00188                         moveit_msgs::MoveItErrorCodes &error_code,
00189                         const kinematics::KinematicsBase::IKCallbackFn &solution_callback);
00190 
00205     int CartToJntSearch(const KDL::JntArray& q_in,
00206                         const KDL::Frame& p_in,
00207                         KDL::JntArray &q_out,
00208                         const double &timeout,
00209                         const double& consistency_limit,
00210                         moveit_msgs::MoveItErrorCodes &error_code,
00211                         const kinematics::KinematicsBase::IKCallbackFn &solution_callback);
00212 
00218     void getSolverInfo(moveit_msgs::KinematicSolverInfo &response);
00219 
00220     std::string getFrameId();
00221 
00222     unsigned int getFreeAngle() const {
00223       return free_angle_;
00224     }
00225 
00226     void setFreeAngle(const unsigned int& free_angle) {
00227       free_angle_ = free_angle;
00228     }
00229 
00230   private:
00231 
00247     int CartToJntSearch(const KDL::JntArray& q_in,
00248                         const KDL::Frame& p_in,
00249                         KDL::JntArray &q_out,
00250                         const double &timeout,
00251                         bool use_consistency_limit,
00252                         const double& consistency_limit,
00253                         moveit_msgs::MoveItErrorCodes &error_code,
00254                         const kinematics::KinematicsBase::IKCallbackFn &solution_callback);
00255 
00256     bool getCount(int &count, const int &max_count, const int &min_count);
00257 
00258     double search_discretization_angle_;
00259 
00260     int free_angle_;
00261 
00262     std::string root_frame_name_;
00263   };
00264 }
00265 #endif// PR2_ARM_IK_SOLVER_H


pr2_arm_kinematics
Author(s): Sachin Chitta
autogenerated on Thu Jun 6 2019 20:11:52