default_constraint_samplers.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #pragma once
38 
42 
43 namespace constraint_samplers
44 {
45 random_numbers::RandomNumberGenerator createSeededRNG(const std::string& seed_param);
46 
47 MOVEIT_CLASS_FORWARD(JointConstraintSampler); // Defines JointConstraintSamplerPtr, ConstPtr, WeakPtr... etc
48 
58 class JointConstraintSampler : public ConstraintSampler
59 {
60 public:
71  JointConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name)
72  : ConstraintSampler(scene, group_name)
73  , random_number_generator_(createSeededRNG("~joint_constraint_sampler_random_seed"))
74  {
75  }
76 
94  bool configure(const moveit_msgs::Constraints& constr) override;
95 
121  bool configure(const std::vector<kinematic_constraints::JointConstraint>& jc);
122 
123  bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& ks, unsigned int max_attempts) override;
124 
125  bool project(moveit::core::RobotState& state, unsigned int max_attempts) override;
126 
134  std::size_t getConstrainedJointCount() const
135  {
136  return bounds_.size();
137  }
138 
145  std::size_t getUnconstrainedJointCount() const
146  {
147  return unbounded_.size();
148  }
149 
155  const std::string& getName() const override
156  {
157  static const std::string SAMPLER_NAME = "JointConstraintSampler";
158  return SAMPLER_NAME;
159  }
160 
161 protected:
163  struct JointInfo
164  {
170  JointInfo()
171  {
172  min_bound_ = -std::numeric_limits<double>::max();
173  max_bound_ = std::numeric_limits<double>::max();
174  }
175 
185  void potentiallyAdjustMinMaxBounds(double min, double max)
186  {
187  min_bound_ = std::max(min, min_bound_);
188  max_bound_ = std::min(max, max_bound_);
189  }
190 
191  double min_bound_;
192  double max_bound_;
193  std::size_t index_;
194  };
195 
196  void clear() override;
197 
199  std::vector<JointInfo> bounds_;
202  std::vector<const moveit::core::JointModel*> unbounded_;
204  std::vector<unsigned int> uindex_;
205  std::vector<double> values_;
206 };
207 
213 struct IKSamplingPose
214 {
220  IKSamplingPose();
221 
229 
238 
250 
258  IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc);
259 
267  IKSamplingPose(const kinematic_constraints::OrientationConstraintPtr& oc);
268 
277  IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc,
278  const kinematic_constraints::OrientationConstraintPtr& oc);
279 
280  kinematic_constraints::PositionConstraintPtr position_constraint_;
282  kinematic_constraints::OrientationConstraintPtr
284 };
285 
286 MOVEIT_CLASS_FORWARD(IKConstraintSampler); // Defines IKConstraintSamplerPtr, ConstPtr, WeakPtr... etc
287 
297 {
298 public:
309  IKConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name)
310  : ConstraintSampler(scene, group_name)
311  , random_number_generator_(createSeededRNG("~ik_constraint_sampler_random_seed"))
312  {
313  }
314 
339  bool configure(const moveit_msgs::Constraints& constr) override;
340 
365  bool configure(const IKSamplingPose& sp);
366 
373  double getIKTimeout() const
374  {
375  return ik_timeout_;
376  }
377 
383  void setIKTimeout(double timeout)
384  {
385  ik_timeout_ = timeout;
386  }
387 
394  const kinematic_constraints::PositionConstraintPtr& getPositionConstraint() const
395  {
397  }
404  const kinematic_constraints::OrientationConstraintPtr& getOrientationConstraint() const
405  {
407  }
408 
422  double getSamplingVolume() const;
423 
430  const std::string& getLinkName() const;
431 
453  bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state,
454  unsigned int max_attempts) override;
455 
456  bool project(moveit::core::RobotState& state, unsigned int max_attempts) override;
479  bool samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const moveit::core::RobotState& ks,
480  unsigned int max_attempts);
481 
487  const std::string& getName() const override
488  {
489  static const std::string SAMPLER_NAME = "IKConstraintSampler";
490  return SAMPLER_NAME;
491  }
492 
493 protected:
494  void clear() override;
495 
502  bool loadIKSolver();
503 
514  bool callIK(const geometry_msgs::Pose& ik_query,
515  const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback, double timeout,
516  moveit::core::RobotState& state, bool use_as_seed);
517  bool sampleHelper(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state,
518  unsigned int max_attempts, bool project);
519  bool validate(moveit::core::RobotState& state) const;
520 
523  kinematics::KinematicsBaseConstPtr kb_;
524  double ik_timeout_;
525  std::string ik_frame_;
526  bool transform_ik_;
530  Eigen::Isometry3d eef_to_ik_tip_transform_;
531 };
532 } // namespace constraint_samplers
constraint_samplers::IKSamplingPose
A structure for potentially holding a position constraint and an orientation constraint for use durin...
Definition: default_constraint_samplers.h:245
random_numbers.h
constraint_samplers::createSeededRNG
random_numbers::RandomNumberGenerator createSeededRNG(const std::string &seed_param)
Definition: default_constraint_samplers.cpp:75
constraint_samplers::IKConstraintSampler::getSamplingVolume
double getSamplingVolume() const
Gets the volume associated with the position and orientation constraints.
Definition: default_constraint_samplers.cpp:359
constraint_samplers::IKConstraintSampler
A class that allows the sampling of IK constraints.
Definition: default_constraint_samplers.h:328
constraint_samplers::IKConstraintSampler::sampleHelper
bool sampleHelper(moveit::core::RobotState &state, const moveit::core::RobotState &reference_state, unsigned int max_attempts, bool project)
Definition: default_constraint_samplers.cpp:602
constraint_samplers::IKConstraintSampler::project
bool project(moveit::core::RobotState &state, unsigned int max_attempts) override
Project a sample given the constraints, updating the joint state group. This function allows the para...
Definition: default_constraint_samplers.cpp:667
constraint_samplers::JointConstraintSampler::getUnconstrainedJointCount
std::size_t getUnconstrainedJointCount() const
Gets the number of unconstrained joints - joint that have no additional bound beyond the joint limits...
Definition: default_constraint_samplers.h:177
constraint_samplers::IKConstraintSampler::setIKTimeout
void setIKTimeout(double timeout)
Sets the timeout argument passed to the IK solver.
Definition: default_constraint_samplers.h:415
constraint_samplers::JointConstraintSampler::clear
void clear() override
Clears all data from the constraint.
Definition: default_constraint_samplers.cpp:231
constraint_samplers::IKConstraintSampler::sampling_pose_
IKSamplingPose sampling_pose_
Holder for the pose used for sampling.
Definition: default_constraint_samplers.h:554
constraint_samplers::IKConstraintSampler::getPositionConstraint
const kinematic_constraints::PositionConstraintPtr & getPositionConstraint() const
Gets the position constraint associated with this sampler.
Definition: default_constraint_samplers.h:426
constraint_samplers::IKConstraintSampler::random_number_generator_
random_numbers::RandomNumberGenerator random_number_generator_
Random generator used by the sampler.
Definition: default_constraint_samplers.h:553
constraint_samplers::IKConstraintSampler::getLinkName
const std::string & getLinkName() const
Gets the link name associated with this sampler.
Definition: default_constraint_samplers.cpp:380
class_forward.h
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
constraint_samplers::IKConstraintSampler::ik_timeout_
double ik_timeout_
Holds the timeout associated with IK.
Definition: default_constraint_samplers.h:556
kinematics::KinematicsBase::IKCallbackFn
boost::function< void(const geometry_msgs::Pose &, const std::vector< double > &, moveit_msgs::MoveItErrorCodes &)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking.
Definition: kinematics_base.h:154
constraint_samplers::JointConstraintSampler::getName
const std::string & getName() const override
Get the name of the constraint sampler, for debugging purposes should be in CamelCase format.
Definition: default_constraint_samplers.h:187
constraint_samplers::IKSamplingPose::position_constraint_
kinematic_constraints::PositionConstraintPtr position_constraint_
Holds the position constraint for sampling.
Definition: default_constraint_samplers.h:312
constraint_samplers::JointConstraintSampler::random_number_generator_
random_numbers::RandomNumberGenerator random_number_generator_
Random number generator used to sample.
Definition: default_constraint_samplers.h:230
constraint_samplers::JointConstraintSampler::JointInfo::JointInfo
JointInfo()
Constructor.
Definition: default_constraint_samplers.h:202
constraint_samplers::JointConstraintSampler::uindex_
std::vector< unsigned int > uindex_
The index of the unbounded joints in the joint state vector.
Definition: default_constraint_samplers.h:236
constraint_samplers::IKConstraintSampler::eef_to_ik_tip_transform_
Eigen::Isometry3d eef_to_ik_tip_transform_
Holds the transformation from end effector to IK tip frame.
Definition: default_constraint_samplers.h:562
constraint_samplers::IKConstraintSampler::IKConstraintSampler
IKConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name)
Constructor.
Definition: default_constraint_samplers.h:341
constraint_samplers::IKSamplingPose::IKSamplingPose
IKSamplingPose()
Empty constructor.
Definition: default_constraint_samplers.cpp:240
constraint_samplers::JointConstraintSampler::values_
std::vector< double > values_
Values associated with this group to avoid continuously reallocating.
Definition: default_constraint_samplers.h:237
constraint_samplers
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
Definition: constraint_sampler.h:51
constraint_samplers::JointConstraintSampler::sample
bool sample(moveit::core::RobotState &state, const moveit::core::RobotState &ks, unsigned int max_attempts) override
Samples given the constraints, populating state. This function allows the parameter max_attempts to b...
Definition: default_constraint_samplers.cpp:196
constraint_samplers::JointConstraintSampler::bounds_
std::vector< JointInfo > bounds_
The bounds for any joint with bounds that are more restrictive than the joint limits.
Definition: default_constraint_samplers.h:231
kinematic_constraints::OrientationConstraint
Class for constraints on the orientation of a link.
Definition: kinematic_constraint.h:379
constraint_samplers::IKConstraintSampler::sample
bool sample(moveit::core::RobotState &state, const moveit::core::RobotState &reference_state, unsigned int max_attempts) override
Produces an IK sample.
Definition: default_constraint_samplers.cpp:596
constraint_samplers::IKConstraintSampler::loadIKSolver
bool loadIKSolver()
Performs checks and sets various internal values associated with the IK solver.
Definition: default_constraint_samplers.cpp:387
constraint_samplers::IKConstraintSampler::getOrientationConstraint
const kinematic_constraints::OrientationConstraintPtr & getOrientationConstraint() const
Gets the orientation constraint associated with this sampler.
Definition: default_constraint_samplers.h:436
constraint_samplers::IKConstraintSampler::clear
void clear() override
Clears all data from the constraint.
Definition: default_constraint_samplers.cpp:275
constraint_samplers::JointConstraintSampler::JointInfo::min_bound_
double min_bound_
Definition: default_constraint_samplers.h:223
constraint_samplers::IKConstraintSampler::transform_ik_
bool transform_ik_
True if the frame associated with the kinematic model is different than the base frame of the IK solv...
Definition: default_constraint_samplers.h:558
random_numbers::RandomNumberGenerator
constraint_samplers::IKConstraintSampler::kb_
kinematics::KinematicsBaseConstPtr kb_
Holds the kinematics solver.
Definition: default_constraint_samplers.h:555
constraint_samplers::JointConstraintSampler::JointInfo::max_bound_
double max_bound_
Definition: default_constraint_samplers.h:224
constraint_samplers::IKConstraintSampler::callIK
bool callIK(const geometry_msgs::Pose &ik_query, const kinematics::KinematicsBase::IKCallbackFn &adapted_ik_validity_callback, double timeout, moveit::core::RobotState &state, bool use_as_seed)
Actually calls IK on the given pose, generating a random seed state.
Definition: default_constraint_samplers.cpp:681
constraint_samplers::IKConstraintSampler::need_eef_to_ik_tip_transform_
bool need_eef_to_ik_tip_transform_
True if the tip frame of the inverse kinematic is different than the frame of the end effector.
Definition: default_constraint_samplers.h:560
constraint_samplers::IKConstraintSampler::getIKTimeout
double getIKTimeout() const
Gets the timeout argument passed to the IK solver.
Definition: default_constraint_samplers.h:405
constraint_samplers::JointConstraintSampler::JointInfo::index_
std::size_t index_
Definition: default_constraint_samplers.h:225
constraint_samplers::JointConstraintSampler::configure
bool configure(const moveit_msgs::Constraints &constr) override
Configures a joint constraint given a Constraints message.
Definition: default_constraint_samplers.cpp:89
constraint_samplers::ConstraintSampler
ConstraintSampler is an abstract base class that allows the sampling of a kinematic state for a parti...
Definition: constraint_sampler.h:91
constraint_samplers::JointConstraintSampler::getConstrainedJointCount
std::size_t getConstrainedJointCount() const
Gets the number of constrained joints - joints that have an additional bound beyond the joint limits.
Definition: default_constraint_samplers.h:166
constraint_samplers::IKConstraintSampler::ik_frame_
std::string ik_frame_
Holds the base from of the IK solver.
Definition: default_constraint_samplers.h:557
constraint_samplers::JointConstraintSampler::JointInfo::potentiallyAdjustMinMaxBounds
void potentiallyAdjustMinMaxBounds(double min, double max)
Function that adjusts the joints only if they are more restrictive. This means that the min limit is ...
Definition: default_constraint_samplers.h:217
constraint_samplers::IKSamplingPose::orientation_constraint_
kinematic_constraints::OrientationConstraintPtr orientation_constraint_
Holds the orientation constraint for sampling.
Definition: default_constraint_samplers.h:315
constraint_sampler.h
constraint_samplers::IKConstraintSampler::validate
bool validate(moveit::core::RobotState &state) const
Definition: default_constraint_samplers.cpp:672
constraint_samplers::JointConstraintSampler::JointConstraintSampler
JointConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name)
Definition: default_constraint_samplers.h:103
constraint_samplers::IKConstraintSampler::configure
bool configure(const moveit_msgs::Constraints &constr) override
Configures the IK constraint given a constraints message.
Definition: default_constraint_samplers.cpp:326
constraint_samplers::IKConstraintSampler::samplePose
bool samplePose(Eigen::Vector3d &pos, Eigen::Quaterniond &quat, const moveit::core::RobotState &ks, unsigned int max_attempts)
Returns a pose that falls within the constraint regions.
Definition: default_constraint_samplers.cpp:461
constraint_samplers::IKConstraintSampler::getName
const std::string & getName() const override
Get the name of the constraint sampler, for debugging purposes should be in CamelCase format.
Definition: default_constraint_samplers.h:519
constraint_samplers::JointConstraintSampler::unbounded_
std::vector< const moveit::core::JointModel * > unbounded_
The joints that are not bounded except by joint limits.
Definition: default_constraint_samplers.h:234
constraint_samplers::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(ConstraintSampler)
constraint_samplers::ConstraintSampler::ConstraintSampler
ConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name)
Constructor.
Definition: constraint_sampler.cpp:39
kinematic_constraints::PositionConstraint
Class for constraints on the XYZ position of a link.
Definition: kinematic_constraint.h:546
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
constraint_samplers::JointConstraintSampler::project
bool project(moveit::core::RobotState &state, unsigned int max_attempts) override
Project a sample given the constraints, updating the joint state group. This function allows the para...
Definition: default_constraint_samplers.cpp:226


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 15 2022 02:24:54