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 <Eigen/Core>
46 #include <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)
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 
243  class ConstraintIntersection : public Constraint
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 
256  ~ConstraintIntersection() override
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 maxIterations_
Maximum number of iterations for Newton method used in projection onto manifold.
Definition: Constraint.h:265
ConstraintObjective(ConstraintPtr constraint, SpaceInformationPtr si)
Constructor.
Definition: Constraint.h:335
unsigned int getCoDimension() const
Returns the dimension of the manifold.
Definition: Constraint.h:204
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:107
A shared pointer wrapper for ompl::base::Constraint.
Definition of an abstract state.
Definition: State.h:113
const unsigned int n_
Ambient space dimension.
Definition: Constraint.h:254
unsigned int getAmbientDimension() const
Returns the dimension of the ambient space.
Definition: Constraint.h:192
unsigned int k_
Manifold dimension.
Definition: Constraint.h:257
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:119
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
ConstraintIntersection(const unsigned int ambientDim, std::initializer_list< Constraint * > constraints)
Constructor. If constraints is empty assume it will be filled later.
Definition: Constraint.h:280
Wrapper around ompl::base::Constraint to use as an optimization objective.
Definition: Constraint.h:331
Abstract definition of optimization objectives.
Definition of a constraint composed of multiple constraints that all must be satisfied simultaneously...
Definition: Constraint.h:275
unsigned int getMaxIterations() const
Returns the maximum number of allowed iterations in the projection routine.
Definition: Constraint.h:226
unsigned int getManifoldDimension() const
Returns the dimension of the manifold.
Definition: Constraint.h:198
bool project(State *state) const
Project a state state given the constraints. If a valid projection cannot be found,...
Definition: Constraint.cpp:88
std::vector< Constraint * > constraints_
Constituent constraints.
Definition: Constraint.h:322
double getTolerance() const
Returns the tolerance of the projection routine.
Definition: Constraint.h:219
double tolerance_
Tolerance for Newton method used in projection onto manifold.
Definition: Constraint.h:261
void setTolerance(const double tolerance)
Sets the projection tolerance.
Definition: Constraint.h:232
ConstraintPtr constraint_
Optimizing constraint.
Definition: Constraint.h:349
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,...
Definition: Constraint.cpp:45
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:304
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:342
void setManifoldDimension(unsigned int k)
Sets the underlying manifold dimension.
Definition: Constraint.h:210
bool isSatisfied(const State *state) const
Check whether a state state satisfies the constraints.
Definition: Constraint.cpp:126
The exception type for ompl.
Definition: Exception.h:78
double distance(const State *state) const
Returns the distance of state to the constraint manifold.
Definition: Constraint.cpp:114
void setMaxIterations(const unsigned int iterations)
Sets the maximum number of iterations in the projection routine.
Definition: Constraint.h:242
~ConstraintIntersection() override
Destructor. Destroys all constituent constraints.
Definition: Constraint.h:288
static const double CONSTRAINT_PROJECTION_TOLERANCE
Default projection tolerance of a constraint unless otherwise specified.
Definition: Constraint.h:119
static const unsigned int CONSTRAINT_PROJECTION_MAX_ITERATIONS
Maximum number of iterations in projection routine until giving up.
Definition: Constraint.h:123
Main namespace. Contains everything in this library.