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
00035
00036
00037 #ifndef PR2_ARM_IK_NODE_H
00038 #define PR2_ARM_IK_NODE_H
00039
00040 #include <kdl/frames.hpp>
00041 #include <kdl/jntarray.hpp>
00042 #include <kdl/tree.hpp>
00043 #include <kdl_parser/kdl_parser.hpp>
00044
00045 #include <angles/angles.h>
00046 #include <tf_conversions/tf_kdl.h>
00047
00048 #include <moveit_msgs/GetPositionFK.h>
00049 #include <moveit_msgs/GetPositionIK.h>
00050 #include <moveit_msgs/GetKinematicSolverInfo.h>
00051 #include <moveit_msgs/MoveItErrorCodes.h>
00052
00053 #include <kdl/chainfksolverpos_recursive.hpp>
00054
00055 #include <boost/shared_ptr.hpp>
00056
00057 #include <moveit/kinematics_base/kinematics_base.h>
00058
00059 #include "pr2_arm_ik.h"
00060
00061 namespace pr2_arm_kinematics
00062 {
00063
00064 static const int NO_IK_SOLUTION = -1;
00065 static const int TIMED_OUT = -2;
00066
00067
00068 class PR2ArmIKSolver : public KDL::ChainIkSolverPos
00069 {
00070 public:
00071
00072 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00073
00081 PR2ArmIKSolver(const urdf::ModelInterface &robot_model,
00082 const std::string &root_frame_name,
00083 const std::string &tip_frame_name,
00084 const double &search_discretization_angle,
00085 const int &free_angle);
00086
00087 ~PR2ArmIKSolver(){};
00088
00092 PR2ArmIK pr2_arm_ik_;
00093
00097 bool active_;
00098
00099 int CartToJnt(const KDL::JntArray& q_init,
00100 const KDL::Frame& p_in,
00101 KDL::JntArray& q_out);
00102
00103 int CartToJntSearch(const KDL::JntArray& q_in,
00104 const KDL::Frame& p_in,
00105 KDL::JntArray &q_out,
00106 const double &timeout);
00107
00108 void getSolverInfo(moveit_msgs::KinematicSolverInfo &response)
00109 {
00110 pr2_arm_ik_.getSolverInfo(response);
00111 }
00112
00113 private:
00114
00115 bool getCount(int &count, const int &max_count, const int &min_count);
00116
00117 double search_discretization_angle_;
00118
00119 int free_angle_;
00120
00121 std::string root_frame_name_;
00122 };
00123
00124 Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame &p);
00125 double computeEuclideanDistance(const std::vector<double> &array_1, const KDL::JntArray &array_2);
00126 void getKDLChainInfo(const KDL::Chain &chain,
00127 moveit_msgs::KinematicSolverInfo &chain_info);
00128
00129
00130 class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase
00131 {
00132 public:
00133
00137 PR2ArmKinematicsPlugin();
00138
00139 void setRobotModel(boost::shared_ptr<urdf::ModelInterface>& robot_model);
00140
00145 bool isActive();
00146
00154 virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00155 const std::vector<double> &ik_seed_state,
00156 std::vector<double> &solution,
00157 moveit_msgs::MoveItErrorCodes &error_code,
00158 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00159
00168 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00169 const std::vector<double> &ik_seed_state,
00170 double timeout,
00171 std::vector<double> &solution,
00172 moveit_msgs::MoveItErrorCodes &error_code,
00173 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00183 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00184 const std::vector<double> &ik_seed_state,
00185 double timeout,
00186 const std::vector<double> &consistency_limits,
00187 std::vector<double> &solution,
00188 moveit_msgs::MoveItErrorCodes &error_code,
00189 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00190
00199 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00200 const std::vector<double> &ik_seed_state,
00201 double timeout,
00202 std::vector<double> &solution,
00203 const IKCallbackFn &solution_callback,
00204 moveit_msgs::MoveItErrorCodes &error_code,
00205 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00206
00217 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00218 const std::vector<double> &ik_seed_state,
00219 double timeout,
00220 const std::vector<double> &consistency_limits,
00221 std::vector<double> &solution,
00222 const IKCallbackFn &solution_callback,
00223 moveit_msgs::MoveItErrorCodes &error_code,
00224 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00225
00232 virtual bool getPositionFK(const std::vector<std::string> &link_names,
00233 const std::vector<double> &joint_angles,
00234 std::vector<geometry_msgs::Pose> &poses) const;
00235
00240 virtual bool initialize(const std::string& robot_description,
00241 const std::string& group_name,
00242 const std::string& base_name,
00243 const std::string& tip_name,
00244 double search_discretization);
00245
00249 const std::vector<std::string>& getJointNames() const;
00250
00254 const std::vector<std::string>& getLinkNames() const;
00255
00256 protected:
00257
00258 bool active_;
00259 int free_angle_;
00260 boost::shared_ptr<urdf::ModelInterface> robot_model_;
00261 boost::shared_ptr<pr2_arm_kinematics::PR2ArmIKSolver> pr2_arm_ik_solver_;
00262 std::string root_name_;
00263 int dimension_;
00264 boost::shared_ptr<KDL::ChainFkSolverPos_recursive> jnt_to_pose_solver_;
00265 KDL::Chain kdl_chain_;
00266 moveit_msgs::KinematicSolverInfo ik_solver_info_, fk_solver_info_;
00267
00268 mutable IKCallbackFn desiredPoseCallback_;
00269 mutable IKCallbackFn solutionCallback_;
00270
00271 void desiredPoseCallback(const KDL::JntArray& jnt_array,
00272 const KDL::Frame& ik_pose,
00273 moveit_msgs::MoveItErrorCodes& error_code) const;
00274
00275 void jointSolutionCallback(const KDL::JntArray& jnt_array,
00276 const KDL::Frame& ik_pose,
00277 moveit_msgs::MoveItErrorCodes& error_code) const;
00278
00279
00280 };
00281 }
00282
00283 #endif