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_KINEMATICS_BASE_KINEMATICS_BASE_
00038 #define MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_
00039
00040 #include <geometry_msgs/PoseStamped.h>
00041 #include <moveit_msgs/MoveItErrorCodes.h>
00042 #include <moveit/macros/class_forward.h>
00043 #include <ros/node_handle.h>
00044 #include <console_bridge/console.h>
00045
00046 #include <boost/function.hpp>
00047 #include <string>
00048
00049 namespace moveit
00050 {
00051 namespace core
00052 {
00053 class JointModelGroup;
00054 class RobotState;
00055 }
00056 }
00057
00059 namespace kinematics
00060 {
00061
00062
00063
00064
00065
00066 namespace DiscretizationMethods
00067 {
00068 enum DiscretizationMethod
00069 {
00070 NO_DISCRETIZATION = 1,
00071 ALL_DISCRETIZED,
00072 SOME_DISCRETIZED,
00075 ALL_RANDOM_SAMPLED,
00076 SOME_RANDOM_SAMPLED
00078 };
00079 }
00080 typedef DiscretizationMethods::DiscretizationMethod DiscretizationMethod;
00081
00082
00083
00084
00085
00086 namespace KinematicErrors
00087 {
00088 enum KinematicError
00089 {
00090 OK = 1,
00091 UNSUPORTED_DISCRETIZATION_REQUESTED,
00092 DISCRETIZATION_NOT_INITIALIZED,
00094 MULTIPLE_TIPS_NOT_SUPPORTED,
00095 EMPTY_TIP_POSES,
00096 IK_SEED_OUTSIDE_LIMITS,
00097 SOLVER_NOT_ACTIVE,
00098 NO_SOLUTION
00100 };
00101 }
00102 typedef KinematicErrors::KinematicError KinematicError;
00103
00108 struct KinematicsQueryOptions
00109 {
00110 KinematicsQueryOptions()
00111 : lock_redundant_joints(false)
00112 , return_approximate_solution(false)
00113 , discretization_method(DiscretizationMethods::NO_DISCRETIZATION)
00114 {
00115 }
00116
00117 bool lock_redundant_joints;
00118 bool return_approximate_solution;
00119 DiscretizationMethod discretization_method;
00121 };
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132 struct KinematicsResult
00133 {
00134 KinematicError kinematic_error;
00135 double solution_percentage;
00137 };
00138
00139 MOVEIT_CLASS_FORWARD(KinematicsBase);
00140
00145 class KinematicsBase
00146 {
00147 public:
00148 static const double DEFAULT_SEARCH_DISCRETIZATION;
00149 static const double DEFAULT_TIMEOUT;
00150
00152 typedef boost::function<void(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_solution,
00153 moveit_msgs::MoveItErrorCodes& error_code)>
00154 IKCallbackFn;
00155
00166 virtual bool
00167 getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00168 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00169 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
00170
00186 virtual bool getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
00187 std::vector<std::vector<double> >& solutions, KinematicsResult& result,
00188 const kinematics::KinematicsQueryOptions& options) const;
00189
00203 virtual bool
00204 searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
00205 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00206 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
00207
00223 virtual bool
00224 searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
00225 const std::vector<double>& consistency_limits, std::vector<double>& solution,
00226 moveit_msgs::MoveItErrorCodes& error_code,
00227 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
00228
00243 virtual bool searchPositionIK(
00244 const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
00245 std::vector<double>& solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
00246 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
00247
00264 virtual bool
00265 searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
00266 const std::vector<double>& consistency_limits, std::vector<double>& solution,
00267 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
00268 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
00269
00295 virtual bool
00296 searchPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
00297 double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
00298 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
00299 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
00300 const moveit::core::RobotState* context_state = NULL) const
00301 {
00302
00303 if (ik_poses.size() == 1)
00304 {
00305
00306 if (solution_callback)
00307 {
00308 return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, solution_callback,
00309 error_code, options);
00310 }
00311 else
00312 {
00313 return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, error_code, options);
00314 }
00315 }
00316
00317
00318 logError("moveit.kinematics_base: This kinematic solver does not support searchPositionIK with multiple poses");
00319 return false;
00320 }
00321
00329 virtual bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
00330 std::vector<geometry_msgs::Pose>& poses) const = 0;
00331
00342 virtual void setValues(const std::string& robot_description, const std::string& group_name,
00343 const std::string& base_frame, const std::string& tip_frame, double search_discretization);
00344
00355 virtual void setValues(const std::string& robot_description, const std::string& group_name,
00356 const std::string& base_frame, const std::vector<std::string>& tip_frames,
00357 double search_discretization);
00358
00370 virtual bool initialize(const std::string& robot_description, const std::string& group_name,
00371 const std::string& base_frame, const std::string& tip_frame,
00372 double search_discretization) = 0;
00373
00385 virtual bool initialize(const std::string& robot_description, const std::string& group_name,
00386 const std::string& base_frame, const std::vector<std::string>& tip_frames,
00387 double search_discretization)
00388 {
00389
00390 if (tip_frames.size() == 1)
00391 {
00392 return initialize(robot_description, group_name, base_frame, tip_frames[0], search_discretization);
00393 }
00394
00395 logError("moveit.kinematics_base: This kinematic solver does not support initialization with more than one tip "
00396 "frames");
00397 return false;
00398 }
00399
00404 virtual const std::string& getGroupName() const
00405 {
00406 return group_name_;
00407 }
00408
00414 virtual const std::string& getBaseFrame() const
00415 {
00416 return base_frame_;
00417 }
00418
00426 virtual const std::string& getTipFrame() const
00427 {
00428 if (tip_frames_.size() > 1)
00429 logError("moveit.kinematics_base: This kinematic solver has more than one tip frame, do not call getTipFrame()");
00430
00431 return tip_frame_;
00432 }
00433
00439 virtual const std::vector<std::string>& getTipFrames() const
00440 {
00441 return tip_frames_;
00442 }
00443
00453 virtual bool setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices);
00454
00462 bool setRedundantJoints(const std::vector<std::string>& redundant_joint_names);
00463
00467 virtual void getRedundantJoints(std::vector<unsigned int>& redundant_joint_indices) const
00468 {
00469 redundant_joint_indices = redundant_joint_indices_;
00470 }
00471
00475 virtual const std::vector<std::string>& getJointNames() const = 0;
00476
00480 virtual const std::vector<std::string>& getLinkNames() const = 0;
00481
00498 virtual bool supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out = NULL) const;
00499
00503 void setSearchDiscretization(double sd)
00504 {
00505 redundant_joint_discretization_.clear();
00506 for (std::vector<unsigned int>::iterator i = redundant_joint_indices_.begin(); i != redundant_joint_indices_.end();
00507 i++)
00508 {
00509 redundant_joint_discretization_[*i] = sd;
00510 }
00511 }
00512
00520 void setSearchDiscretization(const std::map<int, double>& discretization)
00521 {
00522 redundant_joint_discretization_.clear();
00523 redundant_joint_indices_.clear();
00524 for (std::map<int, double>::const_iterator i = discretization.begin(); i != discretization.end(); i++)
00525 {
00526 redundant_joint_discretization_.insert(*i);
00527 redundant_joint_indices_.push_back(i->first);
00528 }
00529 }
00530
00534 double getSearchDiscretization(int joint_index = 0) const
00535 {
00536 if (redundant_joint_discretization_.count(joint_index) > 0)
00537 {
00538 return redundant_joint_discretization_.at(joint_index);
00539 }
00540 else
00541 {
00542 return 0.0;
00543 }
00544 }
00545
00550 std::vector<DiscretizationMethod> getSupportedDiscretizationMethods() const
00551 {
00552 return supported_methods_;
00553 }
00554
00557 void setDefaultTimeout(double timeout)
00558 {
00559 default_timeout_ = timeout;
00560 }
00561
00564 double getDefaultTimeout() const
00565 {
00566 return default_timeout_;
00567 }
00568
00572 virtual ~KinematicsBase()
00573 {
00574 }
00575
00576 KinematicsBase()
00577 : tip_frame_("DEPRECATED")
00578 ,
00579
00580 search_discretization_(DEFAULT_SEARCH_DISCRETIZATION)
00581 , default_timeout_(DEFAULT_TIMEOUT)
00582 {
00583 supported_methods_.push_back(DiscretizationMethods::NO_DISCRETIZATION);
00584 }
00585
00586 protected:
00587 std::string robot_description_;
00588 std::string group_name_;
00589 std::string base_frame_;
00590 std::vector<std::string> tip_frames_;
00591 std::string tip_frame_;
00592
00593
00594 double search_discretization_;
00595
00596
00597
00598 double default_timeout_;
00599 std::vector<unsigned int> redundant_joint_indices_;
00600 std::map<int, double> redundant_joint_discretization_;
00601 std::vector<DiscretizationMethod> supported_methods_;
00602
00612 template <typename T>
00613 inline bool lookupParam(const std::string& param, T& val, const T& default_val) const
00614 {
00615 ros::NodeHandle pnh("~");
00616 if (pnh.hasParam(param))
00617 {
00618 val = pnh.param(param, default_val);
00619 return true;
00620 }
00621
00622 ros::NodeHandle nh;
00623 if (nh.hasParam("robot_description_kinematics/" + group_name_ + "/" + param))
00624 {
00625 val = nh.param("robot_description_kinematics/" + group_name_ + "/" + param, default_val);
00626 return true;
00627 }
00628
00629 if (nh.hasParam("robot_description_kinematics/" + param))
00630 {
00631 val = nh.param("robot_description_kinematics/" + param, default_val);
00632 return true;
00633 }
00634
00635 val = default_val;
00636
00637 return false;
00638 }
00639
00640 private:
00641 std::string removeSlash(const std::string& str) const;
00642 };
00643 };
00644
00645 #endif