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