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 #ifndef MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_CONSTRAINT_SAMPLERS_
00038 #define MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_CONSTRAINT_SAMPLERS_
00039
00040 #include <moveit/constraint_samplers/constraint_sampler.h>
00041 #include <moveit/macros/class_forward.h>
00042 #include <random_numbers/random_numbers.h>
00043
00044 namespace constraint_samplers
00045 {
00046 MOVEIT_CLASS_FORWARD(JointConstraintSampler);
00047
00057 class JointConstraintSampler : public ConstraintSampler
00058 {
00059 public:
00070 JointConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name)
00071 : ConstraintSampler(scene, group_name)
00072 {
00073 }
00091 virtual bool configure(const moveit_msgs::Constraints& constr);
00092
00118 bool configure(const std::vector<kinematic_constraints::JointConstraint>& jc);
00119
00120 virtual bool sample(robot_state::RobotState& state, const robot_state::RobotState& ks, unsigned int max_attempts);
00121
00122 virtual bool project(robot_state::RobotState& state, unsigned int max_attempts);
00123
00131 std::size_t getConstrainedJointCount() const
00132 {
00133 return bounds_.size();
00134 }
00135
00142 std::size_t getUnconstrainedJointCount() const
00143 {
00144 return unbounded_.size();
00145 }
00146
00152 virtual const std::string& getName() const
00153 {
00154 static const std::string SAMPLER_NAME = "JointConstraintSampler";
00155 return SAMPLER_NAME;
00156 }
00157
00158 protected:
00160 struct JointInfo
00161 {
00167 JointInfo()
00168 {
00169 min_bound_ = -std::numeric_limits<double>::max();
00170 max_bound_ = std::numeric_limits<double>::max();
00171 }
00172
00182 void potentiallyAdjustMinMaxBounds(double min, double max)
00183 {
00184 min_bound_ = std::max(min, min_bound_);
00185 max_bound_ = std::min(max, max_bound_);
00186 }
00187
00188 double min_bound_;
00189 double max_bound_;
00190 std::size_t index_;
00191 };
00192
00193 virtual void clear();
00194
00195 random_numbers::RandomNumberGenerator random_number_generator_;
00196 std::vector<JointInfo> bounds_;
00199 std::vector<const robot_model::JointModel*> unbounded_;
00201 std::vector<unsigned int> uindex_;
00202 std::vector<double> values_;
00203 };
00204
00210 struct IKSamplingPose
00211 {
00217 IKSamplingPose();
00218
00225 IKSamplingPose(const kinematic_constraints::PositionConstraint& pc);
00226
00234 IKSamplingPose(const kinematic_constraints::OrientationConstraint& oc);
00235
00245 IKSamplingPose(const kinematic_constraints::PositionConstraint& pc,
00246 const kinematic_constraints::OrientationConstraint& oc);
00247
00255 IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc);
00256
00264 IKSamplingPose(const kinematic_constraints::OrientationConstraintPtr& oc);
00265
00274 IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc,
00275 const kinematic_constraints::OrientationConstraintPtr& oc);
00276
00277 kinematic_constraints::PositionConstraintPtr position_constraint_;
00279 kinematic_constraints::OrientationConstraintPtr
00280 orientation_constraint_;
00281 };
00282
00283 MOVEIT_CLASS_FORWARD(IKConstraintSampler);
00284
00293 class IKConstraintSampler : public ConstraintSampler
00294 {
00295 public:
00306 IKConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name)
00307 : ConstraintSampler(scene, group_name)
00308 {
00309 }
00310
00335 virtual bool configure(const moveit_msgs::Constraints& constr);
00336
00361 bool configure(const IKSamplingPose& sp);
00362
00369 double getIKTimeout() const
00370 {
00371 return ik_timeout_;
00372 }
00373
00379 void setIKTimeout(double timeout)
00380 {
00381 ik_timeout_ = timeout;
00382 }
00383
00390 const kinematic_constraints::PositionConstraintPtr& getPositionConstraint() const
00391 {
00392 return sampling_pose_.position_constraint_;
00393 }
00400 const kinematic_constraints::OrientationConstraintPtr& getOrientationConstraint() const
00401 {
00402 return sampling_pose_.orientation_constraint_;
00403 }
00404
00418 double getSamplingVolume() const;
00419
00426 const std::string& getLinkName() const;
00427
00449 virtual bool sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state,
00450 unsigned int max_attempts);
00451
00452 virtual bool project(robot_state::RobotState& state, unsigned int max_attempts);
00475 bool samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const robot_state::RobotState& ks,
00476 unsigned int max_attempts);
00477
00483 virtual const std::string& getName() const
00484 {
00485 static const std::string SAMPLER_NAME = "IKConstraintSampler";
00486 return SAMPLER_NAME;
00487 }
00488
00489 protected:
00490 virtual void clear();
00491
00498 bool loadIKSolver();
00499
00510 bool callIK(const geometry_msgs::Pose& ik_query,
00511 const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback, double timeout,
00512 robot_state::RobotState& state, bool use_as_seed);
00513 bool sampleHelper(robot_state::RobotState& state, const robot_state::RobotState& reference_state,
00514 unsigned int max_attempts, bool project);
00515 bool validate(robot_state::RobotState& state) const;
00516
00517 random_numbers::RandomNumberGenerator random_number_generator_;
00518 IKSamplingPose sampling_pose_;
00519 kinematics::KinematicsBaseConstPtr kb_;
00520 double ik_timeout_;
00521 std::string ik_frame_;
00522 bool transform_ik_;
00524 };
00525 }
00526
00527 #endif