model_based_state_space.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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_OMPL_INTERFACE_PARAMETERIZATION_MODEL_BASED_STATE_SPACE_
38 #define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_MODEL_BASED_STATE_SPACE_
39 
40 #include <ompl/base/StateSpace.h>
45 #include "../detail/same_shared_ptr.hpp"
46 
47 namespace ompl_interface
48 {
49 typedef boost::function<bool(const ompl::base::State* from, const ompl::base::State* to, const double t,
50  ompl::base::State* state)>
52 typedef boost::function<double(const ompl::base::State* state1, const ompl::base::State* state2)> DistanceFunction;
53 
55 {
56  ModelBasedStateSpaceSpecification(const robot_model::RobotModelConstPtr& robot_model,
57  const robot_model::JointModelGroup* jmg)
58  : robot_model_(robot_model), joint_model_group_(jmg)
59  {
60  }
61 
62  ModelBasedStateSpaceSpecification(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name)
63  : robot_model_(robot_model), joint_model_group_(robot_model_->getJointModelGroup(group_name))
64  {
65  if (!joint_model_group_)
66  throw std::runtime_error("Group '" + group_name + "' was not found");
67  }
68 
69  robot_model::RobotModelConstPtr robot_model_;
70  const robot_model::JointModelGroup* joint_model_group_;
71  robot_model::JointBoundsVector joint_bounds_;
72 };
73 
74 class ModelBasedStateSpace : public ompl::base::StateSpace
75 {
76 public:
77  class StateType : public ompl::base::State
78  {
79  public:
80  enum
81  {
82  VALIDITY_KNOWN = 1,
83  GOAL_DISTANCE_KNOWN = 2,
84  VALIDITY_TRUE = 4,
85  IS_START_STATE = 8,
86  IS_GOAL_STATE = 16
87  };
88 
89  StateType() : ompl::base::State(), values(NULL), tag(-1), flags(0), distance(0.0)
90  {
91  }
92 
93  void markValid(double d)
94  {
95  distance = d;
96  flags |= GOAL_DISTANCE_KNOWN;
97  markValid();
98  }
99 
100  void markValid()
101  {
102  flags |= (VALIDITY_KNOWN | VALIDITY_TRUE);
103  }
104 
105  void markInvalid(double d)
106  {
107  distance = d;
108  flags |= GOAL_DISTANCE_KNOWN;
109  markInvalid();
110  }
111 
112  void markInvalid()
113  {
114  flags &= ~VALIDITY_TRUE;
115  flags |= VALIDITY_KNOWN;
116  }
117 
118  bool isValidityKnown() const
119  {
120  return flags & VALIDITY_KNOWN;
121  }
122 
124  {
125  flags = 0;
126  }
127 
128  bool isMarkedValid() const
129  {
130  return flags & VALIDITY_TRUE;
131  }
132 
133  bool isGoalDistanceKnown() const
134  {
135  return flags & GOAL_DISTANCE_KNOWN;
136  }
137 
138  bool isStartState() const
139  {
140  return flags & IS_START_STATE;
141  }
142 
143  bool isGoalState() const
144  {
145  return flags & IS_GOAL_STATE;
146  }
147 
148  bool isInputState() const
149  {
150  return flags & (IS_START_STATE | IS_GOAL_STATE);
151  }
152 
154  {
155  flags |= IS_START_STATE;
156  }
157 
159  {
160  flags |= IS_GOAL_STATE;
161  }
162 
163  double* values;
164  int tag;
165  int flags;
166  double distance;
167  };
168 
170  virtual ~ModelBasedStateSpace();
171 
173  {
174  interpolation_function_ = fun;
175  }
176 
177  void setDistanceFunction(const DistanceFunction& fun)
178  {
179  distance_function_ = fun;
180  }
181 
182  virtual ompl::base::State* allocState() const;
183  virtual void freeState(ompl::base::State* state) const;
184  virtual unsigned int getDimension() const;
185  virtual void enforceBounds(ompl::base::State* state) const;
186  virtual bool satisfiesBounds(const ompl::base::State* state) const;
187 
188  virtual void copyState(ompl::base::State* destination, const ompl::base::State* source) const;
189  virtual void interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t,
190  ompl::base::State* state) const;
191  virtual double distance(const ompl::base::State* state1, const ompl::base::State* state2) const;
192  virtual bool equalStates(const ompl::base::State* state1, const ompl::base::State* state2) const;
193  virtual double getMaximumExtent() const;
194  virtual double getMeasure() const;
195 
196  virtual unsigned int getSerializationLength() const;
197  virtual void serialize(void* serialization, const ompl::base::State* state) const;
198  virtual void deserialize(ompl::base::State* state, const void* serialization) const;
199  virtual double* getValueAddressAtIndex(ompl::base::State* state, const unsigned int index) const;
200 
201  virtual ompl::base::StateSamplerPtr allocDefaultStateSampler() const;
202 
203  const robot_model::RobotModelConstPtr& getRobotModel() const
204  {
205  return spec_.robot_model_;
206  }
207 
208  const robot_model::JointModelGroup* getJointModelGroup() const
209  {
210  return spec_.joint_model_group_;
211  }
212 
213  const std::string& getJointModelGroupName() const
214  {
215  return getJointModelGroup()->getName();
216  }
217 
219  {
220  return spec_;
221  }
222 
223  virtual void printState(const ompl::base::State* state, std::ostream& out) const;
224  virtual void printSettings(std::ostream& out) const;
225 
227  virtual void setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ);
228 
229  const robot_model::JointBoundsVector& getJointsBounds() const
230  {
231  return spec_.joint_bounds_;
232  }
233 
235  // The joint states \b must be specified in the same order as the joint models in the constructor
236  virtual void copyToRobotState(robot_state::RobotState& rstate, const ompl::base::State* state) const;
237 
239  // The joint states \b must be specified in the same order as the joint models in the constructor
240  virtual void copyToOMPLState(ompl::base::State* state, const robot_state::RobotState& rstate) const;
241 
252  virtual void copyJointToOMPLState(ompl::base::State* state, const robot_state::RobotState& robot_state,
253  const moveit::core::JointModel* joint_model, int ompl_state_joint_index) const;
254 
255  double getTagSnapToSegment() const;
256  void setTagSnapToSegment(double snap);
257 
258 protected:
260  std::vector<robot_model::JointModel::Bounds> joint_bounds_storage_;
261  std::vector<const robot_model::JointModel*> joint_model_vector_;
262  unsigned int variable_count_;
264 
266  DistanceFunction distance_function_;
267 
270 };
271 
273 }
274 
275 #endif
const ModelBasedStateSpaceSpecification & getSpecification() const
std::vector< const robot_model::JointModel * > joint_model_vector_
const robot_model::JointModelGroup * joint_model_group_
std::vector< double > values
void setInterpolationFunction(const InterpolationFunction &fun)
std::vector< robot_model::JointModel::Bounds > joint_bounds_storage_
const robot_model::JointModelGroup * getJointModelGroup() const
ModelBasedStateSpaceSpecification(const robot_model::RobotModelConstPtr &robot_model, const robot_model::JointModelGroup *jmg)
The MoveIt! interface to OMPL.
same_shared_ptr< ModelBasedStateSpace, ompl::base::StateSpacePtr >::type ModelBasedStateSpacePtr
unsigned int index
const robot_model::RobotModelConstPtr & getRobotModel() const
const robot_model::JointBoundsVector & getJointsBounds() const
void setDistanceFunction(const DistanceFunction &fun)
boost::function< double(const ompl::base::State *state1, const ompl::base::State *state2)> DistanceFunction
ModelBasedStateSpaceSpecification spec_
ModelBasedStateSpaceSpecification(const robot_model::RobotModelConstPtr &robot_model, const std::string &group_name)
double distance(const urdf::Pose &transform)
boost::function< bool(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state)> InterpolationFunction
const std::string & getJointModelGroupName() const


ompl
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:01