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 <random_numbers/random_numbers.h>
00042
00043 namespace constraint_samplers
00044 {
00045
00055 class JointConstraintSampler : public ConstraintSampler
00056 {
00057 public:
00058
00069 JointConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene,
00070 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,
00121 const robot_state::RobotState &ks,
00122 unsigned int max_attempts);
00123
00124 virtual bool project(robot_state::RobotState &state,
00125 unsigned int max_attempts);
00126
00134 std::size_t getConstrainedJointCount() const
00135 {
00136 return bounds_.size();
00137 }
00138
00145 std::size_t getUnconstrainedJointCount() const
00146 {
00147 return unbounded_.size();
00148 }
00149
00155 virtual const std::string& getName() const
00156 {
00157 static const std::string SAMPLER_NAME = "JointConstraintSampler";
00158 return SAMPLER_NAME;
00159 }
00160
00161 protected:
00162
00164 struct JointInfo
00165 {
00171 JointInfo()
00172 {
00173 min_bound_ = -std::numeric_limits<double>::max();
00174 max_bound_ = std::numeric_limits<double>::max();
00175 }
00176
00186 void potentiallyAdjustMinMaxBounds(double min, double max)
00187 {
00188 min_bound_ = std::max(min, min_bound_);
00189 max_bound_ = std::min(max, max_bound_);
00190 }
00191
00192 double min_bound_;
00193 double max_bound_;
00194 std::size_t index_;
00195 };
00196
00197 virtual void clear();
00198
00199 random_numbers::RandomNumberGenerator random_number_generator_;
00200 std::vector<JointInfo> bounds_;
00202 std::vector<const robot_model::JointModel*> unbounded_;
00203 std::vector<unsigned int> uindex_;
00204 std::vector<double> values_;
00205 };
00206
00212 struct IKSamplingPose
00213 {
00219 IKSamplingPose();
00220
00227 IKSamplingPose(const kinematic_constraints::PositionConstraint &pc);
00228
00236 IKSamplingPose(const kinematic_constraints::OrientationConstraint &oc);
00237
00247 IKSamplingPose(const kinematic_constraints::PositionConstraint &pc,
00248 const kinematic_constraints::OrientationConstraint &oc);
00249
00257 IKSamplingPose(const boost::shared_ptr<kinematic_constraints::PositionConstraint> &pc);
00258
00266 IKSamplingPose(const boost::shared_ptr<kinematic_constraints::OrientationConstraint> &oc);
00267
00268
00277 IKSamplingPose(const boost::shared_ptr<kinematic_constraints::PositionConstraint> &pc,
00278 const boost::shared_ptr<kinematic_constraints::OrientationConstraint> &oc);
00279
00280 boost::shared_ptr<kinematic_constraints::PositionConstraint> position_constraint_;
00281 boost::shared_ptr<kinematic_constraints::OrientationConstraint> orientation_constraint_;
00282 };
00283
00293 class IKConstraintSampler : public ConstraintSampler
00294 {
00295 public:
00296
00307 IKConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene,
00308 const std::string &group_name) :
00309 ConstraintSampler(scene, group_name)
00310 {
00311 }
00312
00337 virtual bool configure(const moveit_msgs::Constraints &constr);
00338
00363 bool configure(const IKSamplingPose &sp);
00364
00371 double getIKTimeout() const
00372 {
00373 return ik_timeout_;
00374 }
00375
00381 void setIKTimeout(double timeout)
00382 {
00383 ik_timeout_ = timeout;
00384 }
00385
00392 const boost::shared_ptr<kinematic_constraints::PositionConstraint>& getPositionConstraint() const
00393 {
00394 return sampling_pose_.position_constraint_;
00395 }
00402 const boost::shared_ptr<kinematic_constraints::OrientationConstraint>& getOrientationConstraint() const
00403 {
00404 return sampling_pose_.orientation_constraint_;
00405 }
00406
00420 double getSamplingVolume() const;
00421
00428 const std::string& getLinkName() const;
00429
00451 virtual bool sample(robot_state::RobotState &state,
00452 const robot_state::RobotState &reference_state,
00453 unsigned int max_attempts);
00454
00455 virtual bool project(robot_state::RobotState &state,
00456 unsigned int max_attempts);
00479 bool samplePose(Eigen::Vector3d &pos, Eigen::Quaterniond &quat, const robot_state::RobotState &ks, unsigned int max_attempts);
00480
00486 virtual const std::string& getName() const
00487 {
00488 static const std::string SAMPLER_NAME = "IKConstraintSampler";
00489 return SAMPLER_NAME;
00490 }
00491
00492 protected:
00493
00494 virtual void clear();
00495
00501 bool loadIKSolver();
00502
00513 bool callIK(const geometry_msgs::Pose &ik_query, const kinematics::KinematicsBase::IKCallbackFn &adapted_ik_validity_callback,
00514 double timeout, robot_state::RobotState &state, bool use_as_seed);
00515 bool sampleHelper(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts, bool project);
00516 bool validate(robot_state::RobotState &state) const;
00517
00518 random_numbers::RandomNumberGenerator random_number_generator_;
00519 IKSamplingPose sampling_pose_;
00520 kinematics::KinematicsBaseConstPtr kb_;
00521 double ik_timeout_;
00522 std::string ik_frame_;
00523 bool transform_ik_;
00524 };
00525
00526
00527 }
00528
00529
00530 #endif