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
00032
00033
00034
00035
00036
00037
00038 #ifndef MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_
00039 #define MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_
00040
00041 #include <geometry_msgs/PoseStamped.h>
00042 #include <moveit_msgs/MoveItErrorCodes.h>
00043 #include <boost/function.hpp>
00044 #include <string>
00045
00047 namespace kinematics
00048 {
00049
00054 struct KinematicsQueryOptions
00055 {
00056 KinematicsQueryOptions() :
00057 lock_redundant_joints(false),
00058 return_approximate_solution(false)
00059 {
00060 }
00061
00062 bool lock_redundant_joints;
00063 bool return_approximate_solution;
00064 };
00065
00066
00071 class KinematicsBase
00072 {
00073 public:
00074
00075 static const double DEFAULT_SEARCH_DISCRETIZATION;
00076 static const double DEFAULT_TIMEOUT;
00077
00079 typedef boost::function<void(const geometry_msgs::Pose &ik_pose, const std::vector<double> &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn;
00080
00090 virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00091 const std::vector<double> &ik_seed_state,
00092 std::vector<double> &solution,
00093 moveit_msgs::MoveItErrorCodes &error_code,
00094 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const = 0;
00095
00108 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00109 const std::vector<double> &ik_seed_state,
00110 double timeout,
00111 std::vector<double> &solution,
00112 moveit_msgs::MoveItErrorCodes &error_code,
00113 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const = 0;
00114
00128 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00129 const std::vector<double> &ik_seed_state,
00130 double timeout,
00131 const std::vector<double> &consistency_limits,
00132 std::vector<double> &solution,
00133 moveit_msgs::MoveItErrorCodes &error_code,
00134 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const = 0;
00135
00150 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00151 const std::vector<double> &ik_seed_state,
00152 double timeout,
00153 std::vector<double> &solution,
00154 const IKCallbackFn &solution_callback,
00155 moveit_msgs::MoveItErrorCodes &error_code,
00156 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const = 0;
00157
00173 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00174 const std::vector<double> &ik_seed_state,
00175 double timeout,
00176 const std::vector<double> &consistency_limits,
00177 std::vector<double> &solution,
00178 const IKCallbackFn &solution_callback,
00179 moveit_msgs::MoveItErrorCodes &error_code,
00180 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const = 0;
00181
00189 virtual bool getPositionFK(const std::vector<std::string> &link_names,
00190 const std::vector<double> &joint_angles,
00191 std::vector<geometry_msgs::Pose> &poses) const = 0;
00192
00202 virtual void setValues(const std::string& robot_description,
00203 const std::string& group_name,
00204 const std::string& base_frame,
00205 const std::string& tip_frame,
00206 double search_discretization);
00207
00218 virtual bool initialize(const std::string& robot_description,
00219 const std::string& group_name,
00220 const std::string& base_frame,
00221 const std::string& tip_frame,
00222 double search_discretization) = 0;
00223
00228 virtual const std::string& getGroupName() const
00229 {
00230 return group_name_;
00231 }
00232
00237 virtual const std::string& getBaseFrame() const
00238 {
00239 return base_frame_;
00240 }
00241
00246 virtual const std::string& getTipFrame() const
00247 {
00248 return tip_frame_;
00249 }
00250
00258 virtual bool setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices);
00259
00267 bool setRedundantJoints(const std::vector<std::string> &redundant_joint_names);
00268
00272 virtual void getRedundantJoints(std::vector<unsigned int> &redundant_joint_indices) const
00273 {
00274 redundant_joint_indices = redundant_joint_indices_;
00275 }
00276
00280 virtual const std::vector<std::string>& getJointNames() const = 0;
00281
00285 virtual const std::vector<std::string>& getLinkNames() const = 0;
00286
00290 void setSearchDiscretization(double sd)
00291 {
00292 search_discretization_ = sd;
00293 }
00294
00298 double getSearchDiscretization() const
00299 {
00300 return search_discretization_;
00301 }
00302
00305 void setDefaultTimeout(double timeout)
00306 {
00307 default_timeout_ = timeout;
00308 }
00309
00311 double getDefaultTimeout() const
00312 {
00313 return default_timeout_;
00314 }
00315
00319 virtual ~KinematicsBase() {}
00320
00321 KinematicsBase() :
00322 search_discretization_(DEFAULT_SEARCH_DISCRETIZATION),
00323 default_timeout_(DEFAULT_TIMEOUT)
00324 {}
00325
00326 protected:
00327
00328 std::string robot_description_;
00329 std::string group_name_;
00330 std::string base_frame_;
00331 std::string tip_frame_;
00332 double search_discretization_;
00333 double default_timeout_;
00334 std::vector<unsigned int> redundant_joint_indices_;
00335
00336 private:
00337
00338 std::string removeSlash(const std::string &str) const;
00339 };
00340
00341 typedef boost::shared_ptr<KinematicsBase> KinematicsBasePtr;
00342 typedef boost::shared_ptr<const KinematicsBase> KinematicsBaseConstPtr;
00343
00344 };
00345
00346 #endif