Constraint.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, Rice University
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 the Rice University 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: Zachary Kingston, Ryan Luna */
36 
37 #ifndef OMPL_BASE_CONSTRAINTS_CONSTRAINT_
38 #define OMPL_BASE_CONSTRAINTS_CONSTRAINT_
39 
40 #include "ompl/base/StateSpace.h"
41 #include "ompl/base/OptimizationObjective.h"
42 #include "ompl/util/ClassForward.h"
43 #include "ompl/util/Exception.h"
44 
45 #include <eigen3/Eigen/Core>
46 #include <eigen3/Eigen/Dense>
47 #include <utility>
48 
49 namespace ompl
50 {
51  namespace magic
52  {
55  static const double CONSTRAINT_PROJECTION_TOLERANCE = 1e-4;
56 
59  static const unsigned int CONSTRAINT_PROJECTION_MAX_ITERATIONS = 50;
60  }
61 
62  namespace base
63  {
65 
66  OMPL_CLASS_FORWARD(Constraint);
68 
75  class Constraint
76  {
77  public:
87  Constraint(const unsigned int ambientDim, const unsigned int coDim, double tolerance = magic::CONSTRAINT_PROJECTION_TOLERANCE)
88  : n_(ambientDim)
89  , k_(ambientDim - coDim)
90  , tolerance_(tolerance)
91  , maxIterations_(magic::CONSTRAINT_PROJECTION_MAX_ITERATIONS)
92  {
93  if (n_ <= 0 || k_ <= 0)
94  throw ompl::Exception("ompl::base::Constraint(): "
95  "Ambient and manifold dimensions must be positive.");
96  }
97 
98  virtual ~Constraint() = default;
99 
105  void function(const State *state, Eigen::Ref<Eigen::VectorXd> out) const;
106 
109  virtual void function(const Eigen::Ref<const Eigen::VectorXd> &x,
110  Eigen::Ref<Eigen::VectorXd> out) const = 0;
111 
117  void jacobian(const State *state, Eigen::Ref<Eigen::MatrixXd> out) const;
118 
124  virtual void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const;
125 
134  bool project(State *state) const;
135 
138  virtual bool project(Eigen::Ref<Eigen::VectorXd> x) const;
139 
142  double distance(const State *state) const;
143 
145  virtual double distance(const Eigen::Ref<const Eigen::VectorXd> &x) const;
146 
149  bool isSatisfied(const State *state) const;
150 
152  virtual bool isSatisfied(const Eigen::Ref<const Eigen::VectorXd> &x) const;
153 
160  unsigned int getAmbientDimension() const
161  {
162  return n_;
163  }
164 
166  unsigned int getManifoldDimension() const
167  {
168  return k_;
169  }
170 
172  unsigned int getCoDimension() const
173  {
174  return n_ - k_;
175  }
176 
178  void setManifoldDimension(unsigned int k)
179  {
180  if (k <= 0)
181  throw ompl::Exception("ompl::base::Constraint(): "
182  "Space is over constrained!");
183  k_ = k;
184  }
185 
187  double getTolerance() const
188  {
189  return tolerance_;
190  }
191 
194  unsigned int getMaxIterations() const
195  {
196  return maxIterations_;
197  }
198 
200  void setTolerance(const double tolerance)
201  {
202  if (tolerance <= 0)
203  throw ompl::Exception("ompl::base::Constraint::setProjectionTolerance(): "
204  "tolerance must be positive.");
205  tolerance_ = tolerance;
206  }
207 
210  void setMaxIterations(const unsigned int iterations)
211  {
212  if (iterations == 0)
213  throw ompl::Exception("ompl::base::Constraint::setProjectionMaxIterations(): "
214  "iterations must be positive.");
215  maxIterations_ = iterations;
216  }
217 
220  protected:
222  const unsigned int n_;
223 
225  unsigned int k_;
226 
229  double tolerance_;
230 
233  unsigned int maxIterations_;
234  };
235 
237  OMPL_CLASS_FORWARD(ConstraintIntersection);
239 
244  {
245  public:
248  ConstraintIntersection(const unsigned int ambientDim, std::initializer_list<Constraint *> constraints)
249  : Constraint(ambientDim, 0)
250  {
251  for (auto constraint : constraints)
252  addConstraint(constraint);
253  }
254 
257  {
258  for (auto constraint : constraints_)
259  delete constraint;
260  }
261 
262  void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
263  {
264  unsigned int i = 0;
265  for (auto constraint : constraints_)
266  {
267  constraint->function(x, out.segment(i, constraint->getCoDimension()));
268  i += constraint->getCoDimension();
269  }
270  }
271 
272  void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
273  {
274  unsigned int i = 0;
275  for (auto constraint : constraints_)
276  {
277  constraint->jacobian(x, out.block(i, 0, constraint->getCoDimension(), n_));
278  i += constraint->getCoDimension();
279  }
280  }
281 
282  protected:
283  void addConstraint(Constraint *constraint)
284  {
285  setManifoldDimension(k_ - constraint->getCoDimension());
286  constraints_.push_back(constraint);
287  }
288 
290  std::vector<Constraint *> constraints_;
291  };
292 
294  OMPL_CLASS_FORWARD(ConstraintObjective);
296 
300  {
301  public:
304  : OptimizationObjective(std::move(si)), constraint_(std::move(constraint))
305  {
306  }
307 
310  Cost stateCost(const State *s) const override
311  {
312  return Cost(constraint_->distance(s));
313  }
314 
315  protected:
318  };
319  }
320 }
321 
322 #endif
unsigned int k_
Manifold dimension.
Definition: Constraint.h:225
Cost stateCost(const State *s) const override
Evaluate a cost map defined on the state space at a state s. Cost map is defined as the distance from...
Definition: Constraint.h:310
unsigned int getManifoldDimension() const
Returns the dimension of the manifold.
Definition: Constraint.h:166
double tolerance_
Tolerance for Newton method used in projection onto manifold.
Definition: Constraint.h:229
unsigned int getMaxIterations() const
Returns the maximum number of allowed iterations in the projection routine.
Definition: Constraint.h:194
~ConstraintIntersection() override
Destructor. Destroys all constituent constraints.
Definition: Constraint.h:256
ConstraintPtr constraint_
Optimizing constraint.
Definition: Constraint.h:317
const unsigned int n_
Ambient space dimension.
Definition: Constraint.h:222
double getTolerance() const
Returns the tolerance of the projection routine.
Definition: Constraint.h:187
A shared pointer wrapper for ompl::base::Constraint.
ConstraintIntersection(const unsigned int ambientDim, std::initializer_list< Constraint *> constraints)
Constructor. If constraints is empty assume it will be filled later.
Definition: Constraint.h:248
STL namespace.
void setTolerance(const double tolerance)
Sets the projection tolerance.
Definition: Constraint.h:200
unsigned int maxIterations_
Maximum number of iterations for Newton method used in projection onto manifold.
Definition: Constraint.h:233
void setMaxIterations(const unsigned int iterations)
Sets the maximum number of iterations in the projection routine.
Definition: Constraint.h:210
bool isSatisfied(const State *state) const
Check whether a state state satisfies the constraints.
Definition: Constraint.cpp:126
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
static const double CONSTRAINT_PROJECTION_TOLERANCE
Default projection tolerance of a constraint unless otherwise specified.
Definition: Constraint.h:55
unsigned int getAmbientDimension() const
Returns the dimension of the ambient space.
Definition: Constraint.h:160
Constraint(const unsigned int ambientDim, const unsigned int coDim, double tolerance=magic::CONSTRAINT_PROJECTION_TOLERANCE)
Constructor. The dimension of the ambient configuration space as well as the dimension of the functio...
Definition: Constraint.h:87
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:75
void jacobian(const State *state, Eigen::Ref< Eigen::MatrixXd > out) const
Compute the Jacobian of the constraint function at state. Result is returned in out, which should be allocated to size coDim by ambientDim. Default implementation performs the differentiation numerically with a seven-point central difference stencil. It is best to provide an analytic formulation.
Definition: Constraint.cpp:45
A shared pointer wrapper for ompl::base::SpaceInformation.
void jacobian(const Eigen::Ref< const Eigen::VectorXd > &x, Eigen::Ref< Eigen::MatrixXd > out) const override
Compute the Jacobian of the constraint function at x. Result is returned in out, which should be allo...
Definition: Constraint.h:272
ConstraintObjective(ConstraintPtr constraint, SpaceInformationPtr si)
Constructor.
Definition: Constraint.h:303
Definition of an abstract state.
Definition: State.h:49
static const unsigned int CONSTRAINT_PROJECTION_MAX_ITERATIONS
Maximum number of iterations in projection routine until giving up.
Definition: Constraint.h:59
Abstract definition of optimization objectives.
double distance(const State *state) const
Returns the distance of state to the constraint manifold.
Definition: Constraint.cpp:114
The exception type for ompl.
Definition: Exception.h:46
unsigned int getCoDimension() const
Returns the dimension of the manifold.
Definition: Constraint.h:172
bool project(State *state) const
Project a state state given the constraints. If a valid projection cannot be found, this method will return false. Even if this method fails, state will be modified.
Definition: Constraint.cpp:88
Wrapper around ompl::base::Constraint to use as an optimization objective.
Definition: Constraint.h:299
void setManifoldDimension(unsigned int k)
Sets the underlying manifold dimension.
Definition: Constraint.h:178
std::vector< Constraint * > constraints_
Constituent constraints.
Definition: Constraint.h:290
Definition of a constraint composed of multiple constraints that all must be satisfied simultaneously...
Definition: Constraint.h:243
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47