constraint_sampler.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_CONSTRAINT_SAMPLER_
38 #define MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_
39 
42 #include <vector>
43 
53 {
55 
61 {
62 public:
65  static const unsigned int DEFAULT_MAX_SAMPLING_ATTEMPTS = 2;
66 
76  ConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name);
77 
79  {
80  }
81 
90  virtual bool configure(const moveit_msgs::Constraints& constr) = 0;
91 
97  const std::string& getGroupName() const
98  {
99  return getJointModelGroup()->getName();
100  }
101 
109  {
110  return jmg_;
111  }
112 
119  const planning_scene::PlanningSceneConstPtr& getPlanningScene() const
120  {
121  return scene_;
122  }
123 
135  const std::vector<std::string>& getFrameDependency() const
136  {
137  return frame_depends_;
138  }
139 
145  {
147  }
148 
156  {
158  }
159 
171  {
172  return sample(state, state, DEFAULT_MAX_SAMPLING_ATTEMPTS);
173  }
174 
186  bool sample(robot_state::RobotState& state, unsigned int max_attempts)
187  {
188  return sample(state, state, max_attempts);
189  }
190 
201  bool sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state)
202  {
203  return sample(state, reference_state, DEFAULT_MAX_SAMPLING_ATTEMPTS);
204  }
205 
217  {
218  return project(state, DEFAULT_MAX_SAMPLING_ATTEMPTS);
219  }
220 
232  virtual bool sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state,
233  unsigned int max_attempts) = 0;
234 
246  virtual bool project(robot_state::RobotState& state, unsigned int max_attempts) = 0;
247 
255  bool isValid() const
256  {
257  return is_valid_;
258  }
259 
261  bool getVerbose() const
262  {
263  return verbose_;
264  }
265 
267  virtual void setVerbose(bool verbose)
268  {
269  verbose_ = verbose;
270  }
271 
277  virtual const std::string& getName() const = 0;
278 
279 protected:
284  virtual void clear();
285 
286  bool is_valid_;
287 
289  planning_scene::PlanningSceneConstPtr scene_;
293  std::vector<std::string> frame_depends_;
296  bool verbose_;
297 };
298 }
299 
300 #endif
const std::string & getGroupName() const
Gets the group name set in the constructor.
static const unsigned int DEFAULT_MAX_SAMPLING_ATTEMPTS
The default value associated with a sampling request. By default if a valid sample cannot be produced...
virtual void setVerbose(bool verbose)
Enable/disable verbose mode for sampler.
const robot_state::GroupStateValidityCallbackFn & getGroupStateValidityCallback() const
Gets the callback used to determine state validity during sampling. The sampler will attempt to satis...
virtual const std::string & getName() const =0
Get the name of the constraint sampler, for debugging purposes should be in CamelCase format...
bool project(robot_state::RobotState &state)
Project a sample given the constraints, updating the joint state group. The value DEFAULT_MAX_SAMPLIN...
const std::string & getName() const
Get the name of the joint group.
MOVEIT_CLASS_FORWARD(ConstraintSampler)
bool isValid() const
Returns whether or not the constraint sampler is valid or not. To be valid, the joint model group mus...
virtual void clear()
Clears all data from the constraint.
bool getVerbose() const
Check if the sampler is set to verbose mode.
ConstraintSampler is an abstract base class that allows the sampling of a kinematic state for a parti...
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
void setGroupStateValidityCallback(const robot_state::GroupStateValidityCallbackFn &callback)
Sets the callback used to determine the state validity during sampling. The sampler will attempt to s...
bool verbose_
True if verbosity is on.
const robot_model::JointModelGroup * getJointModelGroup() const
Gets the joint model group.
ConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name)
Constructor.
robot_state::GroupStateValidityCallbackFn group_state_validity_callback_
Holds the callback for state validity.
const planning_scene::PlanningSceneConstPtr & getPlanningScene() const
Gets the planning scene.
bool sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state)
Samples given the constraints, populating state. The value DEFAULT_MAX_SAMPLING_ATTEMPTS will be pass...
bool sample(robot_state::RobotState &state)
Samples given the constraints, populating state. The value DEFAULT_MAX_SAMPLING_ATTEMPTS will be pass...
bool sample(robot_state::RobotState &state, unsigned int max_attempts)
Samples given the constraints, populating state. This function allows the parameter max_attempts to b...
const robot_model::JointModelGroup *const jmg_
Holds the joint model group associated with this constraint.
virtual bool configure(const moveit_msgs::Constraints &constr)=0
Function for configuring a constraint sampler given a Constraints message.
const std::vector< std::string > & getFrameDependency() const
Return the names of the mobile frames whose pose is needed when sample() is called.
std::vector< std::string > frame_depends_
Holds the set of frames that must exist in the reference state to allow samples to be drawn...
bool is_valid_
Holds the value for validity.
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
Definition: robot_state.h:64
planning_scene::PlanningSceneConstPtr scene_
Holds the planning scene.


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