appUtil.cpp
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2010, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ioan Sucan */
12 
13 #include "omplapp/apps/detail/appUtil.h"
14 #include <ompl/base/spaces/SE2StateSpace.h>
15 #include <ompl/base/spaces/SE3StateSpace.h>
16 #include <ompl/control/planners/syclop/GridDecomposition.h>
17 #include <ompl/base/objectives/MaximizeMinClearanceObjective.h>
18 #include <ompl/base/objectives/MechanicalWorkOptimizationObjective.h>
19 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
20 #include <limits>
21 #include <utility>
22 
23 void ompl::app::InferProblemDefinitionBounds(const base::ProblemDefinitionPtr &pdef, const GeometricStateExtractor &se, double factor, double add,
24  unsigned int robotCount, const base::StateSpacePtr &space, MotionModel mtype)
25 {
26  // update the bounds based on start states, if needed
27  base::RealVectorBounds bounds = mtype == Motion_2D ? space->as<base::SE2StateSpace>()->getBounds() : space->as<base::SE3StateSpace>()->getBounds();
28 
29  std::vector<const base::State*> states;
30  pdef->getInputStates(states);
31 
32  double minX = std::numeric_limits<double>::infinity();
33  double minY = minX;
34  double minZ = minX;
35  double maxX = -minX;
36  double maxY = maxX;
37  double maxZ = maxX;
38  for (auto & state : states)
39  {
40  for (unsigned int r = 0 ; r < robotCount ; ++r)
41  {
42  const base::State *s = se(state, r);
43  double x = mtype == Motion_2D ? s->as<base::SE2StateSpace::StateType>()->getX() : s->as<base::SE3StateSpace::StateType>()->getX();
44  double y = mtype == Motion_2D ? s->as<base::SE2StateSpace::StateType>()->getY() : s->as<base::SE3StateSpace::StateType>()->getY();
45  double z = mtype == Motion_2D ? 0.0 : s->as<base::SE3StateSpace::StateType>()->getZ();
46  if (minX > x) minX = x;
47  if (maxX < x) maxX = x;
48  if (minY > y) minY = y;
49  if (maxY < y) maxY = y;
50  if (minZ > z) minZ = z;
51  if (maxZ < z) maxZ = z;
52  }
53  }
54  double dx = (maxX - minX) * (factor - 1.0) + add;
55  double dy = (maxY - minY) * (factor - 1.0) + add;
56  double dz = (maxZ - minZ) * (factor - 1.0) + add;
57 
58  if (bounds.low[0] > minX - dx) bounds.low[0] = minX - dx;
59  if (bounds.low[1] > minY - dy) bounds.low[1] = minY - dy;
60 
61  if (bounds.high[0] < maxX + dx) bounds.high[0] = maxX + dx;
62  if (bounds.high[1] < maxY + dy) bounds.high[1] = maxY + dy;
63 
64  if (mtype == Motion_3D)
65  {
66  if (bounds.high[2] < maxZ + dz) bounds.high[2] = maxZ + dz;
67  if (bounds.low[2] > minZ - dz) bounds.low[2] = minZ - dz;
68 
69  space->as<base::SE3StateSpace>()->setBounds(bounds);
70  }
71  else
72  space->as<base::SE2StateSpace>()->setBounds(bounds);
73 }
74 
75 void ompl::app::InferEnvironmentBounds(const base::StateSpacePtr &space, const RigidBodyGeometry &rbg)
76 {
77  MotionModel mtype = rbg.getMotionModel();
78 
79  base::RealVectorBounds bounds = mtype == Motion_2D ? space->as<base::SE2StateSpace>()->getBounds() : space->as<base::SE3StateSpace>()->getBounds();
80 
81  // if bounds are not valid
82  if (bounds.getVolume() < std::numeric_limits<double>::epsilon())
83  {
84  if (mtype == Motion_2D)
85  space->as<base::SE2StateSpace>()->setBounds(rbg.inferEnvironmentBounds());
86  else
87  space->as<base::SE3StateSpace>()->setBounds(rbg.inferEnvironmentBounds());
88  }
89 }
90 
91 namespace ompl
92 {
93  namespace app
94  {
95  namespace detail
96  {
97  class GeometricStateProjector2D : public base::ProjectionEvaluator
98  {
99  public:
100 
101  GeometricStateProjector2D(const base::StateSpacePtr &space, const base::StateSpacePtr &gspace, GeometricStateExtractor se) : base::ProjectionEvaluator(space), gm_(gspace->as<base::SE2StateSpace>()), se_(std::move(se))
102  {
103  }
104 
105  unsigned int getDimension() const override
106  {
107  return 2;
108  }
109 
110  void project(const base::State *state, Eigen::Ref<Eigen::VectorXd> projection) const override
111  {
112  const base::State *gs = se_(state, 0);
113  projection(0) = gs->as<base::SE2StateSpace::StateType>()->getX();
114  projection(1) = gs->as<base::SE2StateSpace::StateType>()->getY();
115  }
116 
117  void defaultCellSizes() override
118  {
119  bounds_ = gm_->getBounds();
120  const std::vector<double> b = bounds_.getDifference();
121  cellSizes_.resize(2);
122  cellSizes_[0] = b[0] / 20.0;
123  cellSizes_[1] = b[1] / 20.0;
124  }
125 
126  protected:
127 
128  const base::SE2StateSpace *gm_;
129  GeometricStateExtractor se_;
130 
131  };
132 
133  class GeometricStateProjector3D : public base::ProjectionEvaluator
134  {
135  public:
136 
137  GeometricStateProjector3D(const base::StateSpacePtr &space, const base::StateSpacePtr &gspace, GeometricStateExtractor se) : base::ProjectionEvaluator(space), gm_(gspace->as<base::SE3StateSpace>()), se_(std::move(se))
138  {
139  }
140 
141  unsigned int getDimension() const override
142  {
143  return 3;
144  }
145 
146  void project(const base::State *state, Eigen::Ref<Eigen::VectorXd> projection) const override
147  {
148  const base::State *gs = se_(state, 0);
149  projection(0) = gs->as<base::SE3StateSpace::StateType>()->getX();
150  projection(1) = gs->as<base::SE3StateSpace::StateType>()->getY();
151  projection(2) = gs->as<base::SE3StateSpace::StateType>()->getZ();
152  }
153 
154  void defaultCellSizes() override
155  {
156  bounds_ = gm_->getBounds();
157  const std::vector<double> b = bounds_.getDifference();
158  cellSizes_.resize(3);
159  cellSizes_[0] = b[0] / 20.0;
160  cellSizes_[1] = b[1] / 20.0;
161  cellSizes_[2] = b[2] / 20.0;
162  }
163 
164  protected:
165 
166  const base::SE3StateSpace *gm_;
167  GeometricStateExtractor se_;
168  };
169 
170 
171  // a decomposition is only needed for SyclopRRT and SyclopEST
172  class Decomposition2D : public ompl::control::GridDecomposition
173  {
174  public:
175  // 32 x 32 grid
176  Decomposition2D(const ompl::base::RealVectorBounds &bounds, const base::StateSpacePtr &space)
177  : GridDecomposition(32, 2, bounds), space_(space), position_(space->getValueLocations()[0])
178  {
179  }
180  void project(const ompl::base::State *s, std::vector<double> &coord) const override
181  {
182  const double* pos = space_->getValueAddressAtLocation(s, position_);
183  coord.resize(2);
184  coord[0] = pos[0];
185  coord[1] = pos[1];
186  }
187 
188  void sampleFullState(const ompl::base::StateSamplerPtr &sampler,
189  const std::vector<double>& coord, ompl::base::State *s) const override
190  {
191  double* pos = space_->getValueAddressAtLocation(s, position_);
192  sampler->sampleUniform(s);
193  pos[0] = coord[0];
194  pos[1] = coord[1];
195  }
196 
197  protected:
198  const base::StateSpacePtr space_;
199  const ompl::base::StateSpace::ValueLocation& position_;
200  };
201 
202  class Decomposition3D : public ompl::control::GridDecomposition
203  {
204  public:
205  // 16 x 16 x 16 grid
206  Decomposition3D(const ompl::base::RealVectorBounds &bounds, const base::StateSpacePtr &space)
207  : GridDecomposition(16, 3, bounds), space_(space), position_(space->getValueLocations()[0])
208  {
209  }
210  void project(const ompl::base::State *s, std::vector<double> &coord) const override
211  {
212  const double* pos = space_->getValueAddressAtLocation(s, position_);
213  coord.resize(3);
214  coord[0] = pos[0];
215  coord[1] = pos[1];
216  coord[2] = pos[2];
217  }
218 
219  void sampleFullState(const ompl::base::StateSamplerPtr &sampler,
220  const std::vector<double>& coord, ompl::base::State *s) const override
221  {
222  double* pos = space_->getValueAddressAtLocation(s, position_);
223  sampler->sampleUniform(s);
224  pos[0] = coord[0];
225  pos[1] = coord[1];
226  pos[2] = coord[2];
227  }
228 
229  protected:
230  const base::StateSpacePtr space_;
231  const ompl::base::StateSpace::ValueLocation& position_;
232  };
233  }
234  }
235 }
236 
237 ompl::base::ProjectionEvaluatorPtr ompl::app::allocGeometricStateProjector(const base::StateSpacePtr &space, MotionModel mtype,
238  const base::StateSpacePtr &gspace, const GeometricStateExtractor &se)
239 {
240  if (mtype == Motion_2D)
241  return std::make_shared<detail::GeometricStateProjector2D>(space, gspace, se);
242  return std::make_shared<detail::GeometricStateProjector3D>(space, gspace, se);
243 }
244 
245 ompl::control::DecompositionPtr ompl::app::allocDecomposition(const base::StateSpacePtr &space, MotionModel mtype,
246  const base::StateSpacePtr &gspace)
247 {
248  // \todo shouldn't this be done automatically?
249  const_cast<ompl::base::StateSpace*>(space.get())->computeLocations();
250 
251  if (mtype == Motion_2D)
252  return std::make_shared<detail::Decomposition2D>(gspace->as<ompl::base::SE2StateSpace>()->getBounds(), space);
253  return std::make_shared<detail::Decomposition3D>(gspace->as<ompl::base::SE3StateSpace>()->getBounds(), space);
254 }
255 
256 ompl::base::OptimizationObjectivePtr ompl::app::getOptimizationObjective(
257  const base::SpaceInformationPtr &si, const std::string &objective, double threshold)
258 {
259  base::OptimizationObjectivePtr obj;
260  if (objective == std::string("length"))
261  obj = std::make_shared<base::PathLengthOptimizationObjective>(si);
262  else if (objective == std::string("max min clearance"))
263  obj = std::make_shared<base::MaximizeMinClearanceObjective>(si);
264  else if (objective == std::string("mechanical work"))
265  obj = std::make_shared<base::MechanicalWorkOptimizationObjective>(si);
266  else
267  {
268  OMPL_WARN("ompl::app::getOptimizationObjective: unknown optimization objective called \"%s\"; using \"length\" instead", objective.c_str());
269  obj = std::make_shared<base::PathLengthOptimizationObjective>(si);
270  }
271  obj->setCostThreshold(base::Cost(threshold));
272  return obj;
273 }
ompl::app::getOptimizationObjective
ompl::base::OptimizationObjectivePtr getOptimizationObjective(const base::SpaceInformationPtr &si, const std::string &objective, double threshold)
Create an optimization objective. The objective name can be: "length", "max min clearance",...
Definition: appUtil.cpp:256
ompl::base::StateSpace
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
ompl::base::SE3StateSpace::getBounds
const RealVectorBounds & getBounds() const
Get the bounds for this state space.
Definition: SE3StateSpace.h:229
ompl::app::MotionModel
MotionModel
Specify whether bodies are moving in 2D or bodies moving in 3D.
Definition: GeometrySpecification.h:44
ompl::base::State
Definition of an abstract state.
Definition: State.h:113
ompl::base::StateSpace::ValueLocation
Representation of the address of a value in a state. This structure stores the indexing information n...
Definition: StateSpace.h:186
ompl::control::GridDecomposition
A GridDecomposition is a Decomposition implemented using a grid.
Definition: GridDecomposition.h:117
ompl::base::RealVectorBounds::getDifference
std::vector< double > getDifference() const
Get the difference between the high and low bounds for each dimension: result[i] = high[i] - low[i].
Definition: RealVectorBounds.cpp:62
ompl::base::Cost
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
OptimizationObjectivePtr
A shared pointer wrapper for ompl::base::OptimizationObjective.
ProjectionEvaluatorPtr
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
ompl::base::ProjectionEvaluator::bounds_
RealVectorBounds bounds_
A bounding box for projected state values.
Definition: ProjectionEvaluator.h:338
ompl::control::Decomposition::sampleFullState
virtual void sampleFullState(const base::StateSamplerPtr &sampler, const std::vector< double > &coord, base::State *s) const =0
Samples a State using a projected coordinate and a StateSampler.
ompl::base::SE2StateSpace
A state space representing SE(2)
Definition: SE2StateSpace.h:113
OMPL_WARN
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
ompl::base::CompoundStateSpace::StateType
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
ompl::app::allocDecomposition
control::DecompositionPtr allocDecomposition(const base::StateSpacePtr &space, MotionModel mtype, const base::StateSpacePtr &gspace)
Allocate a default 2D/3D grid decomposition (depending on the MotionModel) for use with the SyclopEST...
Definition: appUtil.cpp:245
ompl::base::ProjectionEvaluator::cellSizes_
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
Definition: ProjectionEvaluator.h:335
DecompositionPtr
A shared pointer wrapper for ompl::control::Decomposition.
ompl::base::SE3StateSpace
A state space representing SE(3)
Definition: SE3StateSpace.h:113
StateSamplerPtr
A shared pointer wrapper for ompl::base::StateSampler.
ompl::control::GridDecomposition::GridDecomposition
GridDecomposition(int len, int dim, const base::RealVectorBounds &b)
Constructor. Creates a GridDecomposition as a hypercube with a given dimension, side length,...
Definition: GridDecomposition.cpp:45
ompl::base::SE2StateSpace::getBounds
const RealVectorBounds & getBounds() const
Get the bounds for this state space.
Definition: SE2StateSpace.h:220
ompl
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
ompl::base::RealVectorBounds
The lower and upper bounds for an Rn space.
Definition: RealVectorBounds.h:111