Go to the documentation of this file.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 #ifndef TRAC_IK_KINEMATICS_PLUGIN_
00032 #define TRAC_IK_KINEMATICS_PLUGIN_
00033
00034 #include <moveit/kinematics_base/kinematics_base.h>
00035 #include <kdl/chain.hpp>
00036
00037 namespace trac_ik_kinematics_plugin
00038 {
00039
00040 class TRAC_IKKinematicsPlugin : public kinematics::KinematicsBase
00041 {
00042 std::vector<std::string> joint_names_;
00043 std::vector<std::string> link_names_;
00044
00045 uint num_joints_;
00046 bool active_;
00047
00048 KDL::Chain chain;
00049 bool position_ik_;
00050
00051 KDL::JntArray joint_min, joint_max;
00052
00053 std::string solve_type;
00054
00055 public:
00056 const std::vector<std::string>& getJointNames() const
00057 {
00058 return joint_names_;
00059 }
00060 const std::vector<std::string>& getLinkNames() const
00061 {
00062 return link_names_;
00063 }
00064
00065
00069 TRAC_IKKinematicsPlugin(): active_(false), position_ik_(false) {}
00070
00071 ~TRAC_IKKinematicsPlugin()
00072 {
00073 }
00074
00084
00085 bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00086 const std::vector<double> &ik_seed_state,
00087 std::vector<double> &solution,
00088 moveit_msgs::MoveItErrorCodes &error_code,
00089 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00090
00099 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00100 const std::vector<double> &ik_seed_state,
00101 double timeout,
00102 std::vector<double> &solution,
00103 moveit_msgs::MoveItErrorCodes &error_code,
00104 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00105
00115 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00116 const std::vector<double> &ik_seed_state,
00117 double timeout,
00118 const std::vector<double> &consistency_limits,
00119 std::vector<double> &solution,
00120 moveit_msgs::MoveItErrorCodes &error_code,
00121 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00122
00131 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00132 const std::vector<double> &ik_seed_state,
00133 double timeout,
00134 std::vector<double> &solution,
00135 const IKCallbackFn &solution_callback,
00136 moveit_msgs::MoveItErrorCodes &error_code,
00137 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00138
00149 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00150 const std::vector<double> &ik_seed_state,
00151 double timeout,
00152 const std::vector<double> &consistency_limits,
00153 std::vector<double> &solution,
00154 const IKCallbackFn &solution_callback,
00155 moveit_msgs::MoveItErrorCodes &error_code,
00156 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00157
00158 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00159 const std::vector<double> &ik_seed_state,
00160 double timeout,
00161 std::vector<double> &solution,
00162 const IKCallbackFn &solution_callback,
00163 moveit_msgs::MoveItErrorCodes &error_code,
00164 const std::vector<double> &consistency_limits,
00165 const kinematics::KinematicsQueryOptions &options) const;
00166
00167
00179 bool getPositionFK(const std::vector<std::string> &link_names,
00180 const std::vector<double> &joint_angles,
00181 std::vector<geometry_msgs::Pose> &poses) const;
00182
00183
00184 bool initialize(const std::string &robot_description,
00185 const std::string& group_name,
00186 const std::string& base_name,
00187 const std::string& tip_name,
00188 double search_discretization);
00189
00190 private:
00191
00192 int getKDLSegmentIndex(const std::string &name) const;
00193
00194 };
00195 }
00196
00197 #endif