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 #ifndef MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_CONSTRAINT_SAMPLERS_
38 #define MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_CONSTRAINT_SAMPLERS_
39 
43 
44 namespace constraint_samplers
45 {
46 MOVEIT_CLASS_FORWARD(JointConstraintSampler);
47 
58 {
59 public:
70  JointConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name)
71  : ConstraintSampler(scene, group_name)
72  {
73  }
91  virtual bool configure(const moveit_msgs::Constraints& constr);
92 
118  bool configure(const std::vector<kinematic_constraints::JointConstraint>& jc);
119 
120  virtual bool sample(robot_state::RobotState& state, const robot_state::RobotState& ks, unsigned int max_attempts);
121 
122  virtual bool project(robot_state::RobotState& state, unsigned int max_attempts);
123 
131  std::size_t getConstrainedJointCount() const
132  {
133  return bounds_.size();
134  }
135 
142  std::size_t getUnconstrainedJointCount() const
143  {
144  return unbounded_.size();
145  }
146 
152  virtual const std::string& getName() const
153  {
154  static const std::string SAMPLER_NAME = "JointConstraintSampler";
155  return SAMPLER_NAME;
156  }
157 
158 protected:
160  struct JointInfo
161  {
168  {
169  min_bound_ = -std::numeric_limits<double>::max();
170  max_bound_ = std::numeric_limits<double>::max();
171  }
172 
182  void potentiallyAdjustMinMaxBounds(double min, double max)
183  {
184  min_bound_ = std::max(min, min_bound_);
185  max_bound_ = std::min(max, max_bound_);
186  }
187 
188  double min_bound_;
189  double max_bound_;
190  std::size_t index_;
191  };
192 
193  virtual void clear();
194 
196  std::vector<JointInfo> bounds_;
199  std::vector<const robot_model::JointModel*> unbounded_;
201  std::vector<unsigned int> uindex_;
202  std::vector<double> values_;
203 };
204 
211 {
217  IKSamplingPose();
218 
226 
235 
247 
255  IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc);
256 
264  IKSamplingPose(const kinematic_constraints::OrientationConstraintPtr& oc);
265 
274  IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc,
275  const kinematic_constraints::OrientationConstraintPtr& oc);
276 
277  kinematic_constraints::PositionConstraintPtr position_constraint_;
279  kinematic_constraints::OrientationConstraintPtr
281 };
282 
284 
294 {
295 public:
306  IKConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name)
307  : ConstraintSampler(scene, group_name)
308  {
309  }
310 
335  virtual bool configure(const moveit_msgs::Constraints& constr);
336 
361  bool configure(const IKSamplingPose& sp);
362 
369  double getIKTimeout() const
370  {
371  return ik_timeout_;
372  }
373 
379  void setIKTimeout(double timeout)
380  {
381  ik_timeout_ = timeout;
382  }
383 
390  const kinematic_constraints::PositionConstraintPtr& getPositionConstraint() const
391  {
392  return sampling_pose_.position_constraint_;
393  }
400  const kinematic_constraints::OrientationConstraintPtr& getOrientationConstraint() const
401  {
402  return sampling_pose_.orientation_constraint_;
403  }
404 
418  double getSamplingVolume() const;
419 
426  const std::string& getLinkName() const;
427 
449  virtual bool sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state,
450  unsigned int max_attempts);
451 
452  virtual bool project(robot_state::RobotState& state, unsigned int max_attempts);
475  bool samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const robot_state::RobotState& ks,
476  unsigned int max_attempts);
477 
483  virtual const std::string& getName() const
484  {
485  static const std::string SAMPLER_NAME = "IKConstraintSampler";
486  return SAMPLER_NAME;
487  }
488 
489 protected:
490  virtual void clear();
491 
498  bool loadIKSolver();
499 
510  bool callIK(const geometry_msgs::Pose& ik_query,
511  const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback, double timeout,
512  robot_state::RobotState& state, bool use_as_seed);
513  bool sampleHelper(robot_state::RobotState& state, const robot_state::RobotState& reference_state,
514  unsigned int max_attempts, bool project);
515  bool validate(robot_state::RobotState& state) const;
516 
519  kinematics::KinematicsBaseConstPtr kb_;
520  double ik_timeout_;
521  std::string ik_frame_;
526  Eigen::Affine3d eef_to_ik_tip_transform_;
527 };
528 }
529 
530 #endif
double getIKTimeout() const
Gets the timeout argument passed to the IK solver.
std::vector< unsigned int > uindex_
The index of the unbounded joints in the joint state vector.
A structure for potentially holding a position constraint and an orientation constraint for use durin...
IKConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name)
Constructor.
Class for constraints on the orientation of a link.
void potentiallyAdjustMinMaxBounds(double min, double max)
Function that adjusts the joints only if they are more restrictive. This means that the min limit is ...
std::vector< double > values_
Values associated with this group to avoid continuously reallocating.
MOVEIT_CLASS_FORWARD(ConstraintSampler)
random_numbers::RandomNumberGenerator random_number_generator_
Random number generator used to sample.
ROSCPP_DECL bool validate(const std::string &name, std::string &error)
double ik_timeout_
Holds the timeout associated with IK.
const kinematic_constraints::OrientationConstraintPtr & getOrientationConstraint() const
Gets the orientation constraint associated with this sampler.
kinematic_constraints::OrientationConstraintPtr orientation_constraint_
Holds the orientation constraint for sampling.
JointConstraintSampler is a class that allows the sampling of joints in a particular group of the rob...
ConstraintSampler is an abstract base class that allows the sampling of a kinematic state for a parti...
std::size_t getUnconstrainedJointCount() const
Gets the number of unconstrained joints - joint that have no additional bound beyond the joint limits...
virtual void clear()
Clears all data from the constraint.
virtual const std::string & getName() const
Get the name of the constraint sampler, for debugging purposes should be in CamelCase format...
std::size_t getConstrainedJointCount() const
Gets the number of constrained joints - joints that have an additional bound beyond the joint limits...
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
std::string ik_frame_
Holds the base from of the IK solver.
virtual bool configure(const moveit_msgs::Constraints &constr)
Configures a joint constraint given a Constraints message.
std::vector< JointInfo > bounds_
The bounds for any joint with bounds that are more restrictive than the joint limits.
random_numbers::RandomNumberGenerator random_number_generator_
Random generator used by the sampler.
const kinematic_constraints::PositionConstraintPtr & getPositionConstraint() const
Gets the position constraint associated with this sampler.
A class that allows the sampling of IK constraints.
void setIKTimeout(double timeout)
Sets the timeout argument passed to the IK solver.
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...
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
The signature for a callback that can compute IK.
Class for constraints on the XYZ position of a link.
Eigen::Affine3d eef_to_ik_tip_transform_
Holds the transformation from end effector to IK tip frame.
virtual bool sample(robot_state::RobotState &state, const robot_state::RobotState &ks, unsigned int max_attempts)
Samples given the constraints, populating state. This function allows the parameter max_attempts to b...
An internal structure used for maintaining constraints on a particular joint.
bool transform_ik_
True if the frame associated with the kinematic model is different than the base frame of the IK solv...
virtual const std::string & getName() const
Get the name of the constraint sampler, for debugging purposes should be in CamelCase format...
IKSamplingPose sampling_pose_
Holder for the pose used for sampling.
std::vector< const robot_model::JointModel * > unbounded_
The joints that are not bounded except by joint limits.
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
JointConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name)
kinematic_constraints::PositionConstraintPtr position_constraint_
Holds the position constraint for sampling.
kinematics::KinematicsBaseConstPtr kb_
Holds the kinematics solver.
virtual bool project(robot_state::RobotState &state, unsigned int max_attempts)
Project a sample given the constraints, updating the joint state group. This function allows the para...


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33