00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #ifndef ARM_KINEMATICS_CONSTRAINT_AWARE_UTILS_H
00035 #define ARM_KINEMATICS_CONSTRAINT_AWARE_UTILS_H
00036
00037
00038 #include <ros/ros.h>
00039 #include <urdf/model.h>
00040
00041
00042 #include <vector>
00043 #include <Eigen/Core>
00044
00045
00046 #include <kdl/frames.hpp>
00047 #include <kdl/jntarray.hpp>
00048 #include <kdl/tree.hpp>
00049 #include <kdl_parser/kdl_parser.hpp>
00050
00051
00052 #include <tf/tf.h>
00053 #include <tf/transform_listener.h>
00054 #include <tf_conversions/tf_kdl.h>
00055
00056
00057 #include <kinematics_base/kinematics_base.h>
00058
00059
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
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 extractJointState(const sensor_msgs::JointState& joint_state,
00156 const std::vector<std::string>& joint_names,
00157 std::vector<double>& values);
00158
00159 bool getChainInfoFromRobotModel(urdf::Model &robot_model,
00160 const std::string &root_name,
00161 const std::string &tip_name,
00162 kinematics_msgs::KinematicSolverInfo &chain_info);
00163
00164 bool getChainInfo(const std::string &name, kinematics_msgs::KinematicSolverInfo &chain_info);
00165
00166 arm_navigation_msgs::ArmNavigationErrorCodes kinematicsErrorCodeToMotionPlanningErrorCode(const int &kinematics_error_code);
00167
00168 }
00169
00170 #endif// ARM_KINEMATICS_CONSTRAINT_AWARE_UTILS_H_