pr2_arm_kinematics_utils.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 
00034 #ifndef PR2_ARM_IK_UTILS_H
00035 #define PR2_ARM_IK_UTILS_H
00036 
00037 #include <ros/ros.h>
00038 #include <vector>
00039 #include <angles/angles.h>
00040 #include <Eigen/Core>
00041 #include <kdl/frames.hpp>
00042 #include <kdl/jntarray.hpp>
00043 #include <kdl/tree.hpp>
00044 #include <urdf/model.h>
00045 #include <kdl_parser/kdl_parser.hpp>
00046 #include <tf/tf.h>
00047 #include <tf/transform_listener.h>
00048 #include <tf_conversions/tf_kdl.h>
00049 
00050 #include <kinematics_msgs/GetPositionFK.h>
00051 #include <kinematics_msgs/GetPositionIK.h>
00052 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00053 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00054 
00055 
00056 using namespace angles;
00057 
00058 namespace pr2_arm_kinematics
00059 {
00060   Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame &p);
00061 
00062   double computeEuclideanDistance(const std::vector<double> &array_1, 
00063                                   const KDL::JntArray &array_2);
00064 
00065   double distance(const urdf::Pose &transform);
00066 
00067   bool solveQuadratic(const double &a, 
00068                       const double &b, 
00069                       const double &c, 
00070                       double *x1, 
00071                       double *x2);
00072 
00073   Eigen::Matrix4f matrixInverse(const Eigen::Matrix4f &g);
00074 
00075   bool solveCosineEqn(const double &a, 
00076                       const double &b, 
00077                       const double &c, 
00078                       double &soln1, 
00079                       double &soln2);
00080 
00081   bool loadRobotModel(ros::NodeHandle node_handle, 
00082                       urdf::Model &robot_model, 
00083                       std::string &xml_string);
00084 
00085   bool getKDLChain(const std::string &xml_string, 
00086                    const std::string &root_name, 
00087                    const std::string &tip_name, 
00088                    KDL::Chain &kdl_chain);
00089 
00090   bool getKDLTree(const std::string &xml_string, 
00091                    const std::string &root_name, 
00092                    const std::string &tip_name, 
00093                    KDL::Tree &kdl_chain);
00094 
00095   bool checkJointNames(const std::vector<std::string> &joint_names, 
00096                        const kinematics_msgs::KinematicSolverInfo &chain_info);
00097 
00098   bool checkLinkNames(const std::vector<std::string> &link_names,
00099                       const kinematics_msgs::KinematicSolverInfo &chain_info);
00100 
00101   bool checkLinkName(const std::string &link_name, 
00102                      const kinematics_msgs::KinematicSolverInfo &chain_info);
00103  
00104   bool checkRobotState(arm_navigation_msgs::RobotState &robot_state,
00105                        const kinematics_msgs::KinematicSolverInfo &chain_info);
00106 
00107   bool checkFKService(kinematics_msgs::GetPositionFK::Request &request, 
00108                       kinematics_msgs::GetPositionFK::Response &response, 
00109                       const kinematics_msgs::KinematicSolverInfo &chain_info);
00110  
00111   bool checkIKService(kinematics_msgs::GetPositionIK::Request &request, 
00112                       kinematics_msgs::GetPositionIK::Response &response,
00113                       const kinematics_msgs::KinematicSolverInfo &chain_info);
00114  
00115   bool checkConstraintAwareIKService(kinematics_msgs::GetConstraintAwarePositionIK::Request &request, 
00116                                      kinematics_msgs::GetConstraintAwarePositionIK::Response &response,
00117                                      const kinematics_msgs::KinematicSolverInfo &chain_info);
00118 
00119   int getJointIndex(const std::string &name,
00120                     const kinematics_msgs::KinematicSolverInfo &chain_info);
00121 
00122   bool convertPoseToRootFrame(const geometry_msgs::PoseStamped &pose_msg, 
00123                               KDL::Frame &pose_kdl, 
00124                               const std::string &root_frame,
00125                               tf::TransformListener& tf);
00126 
00127   bool convertPoseToRootFrame(const geometry_msgs::PoseStamped &pose_msg, 
00128                               geometry_msgs::PoseStamped &pose_msg_out, 
00129                               const std::string &root_frame,
00130                               tf::TransformListener& tf);
00131 
00132   int getKDLSegmentIndex(const KDL::Chain &chain, 
00133                          const std::string &name);
00134 
00135   void getKDLChainInfo(const KDL::Chain &chain,
00136                        kinematics_msgs::KinematicSolverInfo &chain_info);
00137 }
00138 
00139 #endif// PR2_ARM_IK_UTILS_H


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