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