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 MOVEIT_ROS_PLANNING_KDL_KINEMATICS_PLUGIN_
00038 #define MOVEIT_ROS_PLANNING_KDL_KINEMATICS_PLUGIN_
00039
00040
00041 #include <ros/ros.h>
00042 #include <random_numbers/random_numbers.h>
00043
00044
00045 #include <boost/shared_ptr.hpp>
00046
00047
00048 #include <geometry_msgs/PoseStamped.h>
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
00055 #include <kdl/jntarray.hpp>
00056 #include <kdl/chainiksolvervel_pinv.hpp>
00057 #include <kdl/chainiksolverpos_nr_jl.hpp>
00058 #include <kdl/chainfksolverpos_recursive.hpp>
00059 #include <moveit/kdl_kinematics_plugin/chainiksolver_pos_nr_jl_mimic.hpp>
00060 #include <moveit/kdl_kinematics_plugin/chainiksolver_vel_pinv_mimic.hpp>
00061 #include <moveit/kdl_kinematics_plugin/joint_mimic.hpp>
00062
00063
00064 #include <moveit/kinematics_base/kinematics_base.h>
00065 #include <moveit/robot_model/robot_model.h>
00066 #include <moveit/robot_state/robot_state.h>
00067
00068 namespace kdl_kinematics_plugin
00069 {
00073 class KDLKinematicsPlugin : public kinematics::KinematicsBase
00074 {
00075 public:
00076
00080 KDLKinematicsPlugin();
00081
00082 virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00083 const std::vector<double> &ik_seed_state,
00084 std::vector<double> &solution,
00085 moveit_msgs::MoveItErrorCodes &error_code,
00086 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00087
00088 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00089 const std::vector<double> &ik_seed_state,
00090 double timeout,
00091 std::vector<double> &solution,
00092 moveit_msgs::MoveItErrorCodes &error_code,
00093 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00094
00095 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00096 const std::vector<double> &ik_seed_state,
00097 double timeout,
00098 const std::vector<double> &consistency_limits,
00099 std::vector<double> &solution,
00100 moveit_msgs::MoveItErrorCodes &error_code,
00101 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00102
00103 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00104 const std::vector<double> &ik_seed_state,
00105 double timeout,
00106 std::vector<double> &solution,
00107 const IKCallbackFn &solution_callback,
00108 moveit_msgs::MoveItErrorCodes &error_code,
00109 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00110
00111 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00112 const std::vector<double> &ik_seed_state,
00113 double timeout,
00114 const std::vector<double> &consistency_limits,
00115 std::vector<double> &solution,
00116 const IKCallbackFn &solution_callback,
00117 moveit_msgs::MoveItErrorCodes &error_code,
00118 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00119
00120 virtual bool getPositionFK(const std::vector<std::string> &link_names,
00121 const std::vector<double> &joint_angles,
00122 std::vector<geometry_msgs::Pose> &poses) const;
00123
00124 virtual bool initialize(const std::string &robot_description,
00125 const std::string &group_name,
00126 const std::string &base_name,
00127 const std::string &tip_name,
00128 double search_discretization);
00129
00133 const std::vector<std::string>& getJointNames() const;
00134
00138 const std::vector<std::string>& getLinkNames() const;
00139
00140 protected:
00141
00157 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00158 const std::vector<double> &ik_seed_state,
00159 double timeout,
00160 std::vector<double> &solution,
00161 const IKCallbackFn &solution_callback,
00162 moveit_msgs::MoveItErrorCodes &error_code,
00163 const std::vector<double> &consistency_limits,
00164 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00165
00166 virtual bool setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices);
00167
00168 private:
00169
00170 bool timedOut(const ros::WallTime &start_time, double duration) const;
00171
00172
00180 bool checkConsistency(const KDL::JntArray& seed_state,
00181 const std::vector<double> &consistency_limit,
00182 const KDL::JntArray& solution) const;
00183
00184 int getJointIndex(const std::string &name) const;
00185
00186 int getKDLSegmentIndex(const std::string &name) const;
00187
00188 void getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const;
00189
00196 void getRandomConfiguration(const KDL::JntArray& seed_state,
00197 const std::vector<double> &consistency_limits,
00198 KDL::JntArray &jnt_array,
00199 bool lock_redundancy) const;
00200
00201 bool isRedundantJoint(unsigned int index) const;
00202
00203 bool active_;
00205 moveit_msgs::KinematicSolverInfo ik_chain_info_;
00207 moveit_msgs::KinematicSolverInfo fk_chain_info_;
00209 KDL::Chain kdl_chain_;
00210
00211 unsigned int dimension_;
00213 KDL::JntArray joint_min_, joint_max_;
00215 mutable random_numbers::RandomNumberGenerator random_number_generator_;
00216
00217 robot_model::RobotModelPtr robot_model_;
00218
00219 robot_state::RobotStatePtr state_, state_2_;
00220
00221 int num_possible_redundant_joints_;
00222 std::vector<unsigned int> redundant_joints_map_index_;
00223
00224
00225 bool position_ik_;
00226 robot_model::JointModelGroup* joint_model_group_;
00227 double max_solver_iterations_;
00228 double epsilon_;
00229 std::vector<JointMimic> mimic_joints_;
00230
00231 };
00232 }
00233
00234 #endif