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  } // namespace magic
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,
88  double tolerance = magic::CONSTRAINT_PROJECTION_TOLERANCE)
89  : n_(ambientDim)
90  , k_(ambientDim - coDim)
91  , tolerance_(tolerance)
93  {
94  if (n_ <= 0 || k_ <= 0)
95  throw ompl::Exception("ompl::base::Constraint(): "
96  "Ambient and manifold dimensions must be positive.");
97  }
98 
99  virtual ~Constraint() = default;
100 
106  virtual void function(const State *state, Eigen::Ref<Eigen::VectorXd> out) const;
107 
110  virtual void function(const Eigen::Ref<const Eigen::VectorXd> &x,
111  Eigen::Ref<Eigen::VectorXd> out) const = 0;
112 
118  virtual void jacobian(const State *state, Eigen::Ref<Eigen::MatrixXd> out) const;
119 
125  virtual void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const;
126 
135  virtual bool project(State *state) const;
136 
139  virtual bool project(Eigen::Ref<Eigen::VectorXd> x) const;
140 
143  virtual double distance(const State *state) const;
144 
146  virtual double distance(const Eigen::Ref<const Eigen::VectorXd> &x) const;
147 
150  virtual bool isSatisfied(const State *state) const;
151 
153  virtual bool isSatisfied(const Eigen::Ref<const Eigen::VectorXd> &x) const;
154 
161  unsigned int getAmbientDimension() const
162  {
163  return n_;
164  }
165 
167  unsigned int getManifoldDimension() const
168  {
169  return k_;
170  }
171 
173  unsigned int getCoDimension() const
174  {
175  return n_ - k_;
176  }
177 
179  void setManifoldDimension(unsigned int k)
180  {
181  if (k <= 0)
182  throw ompl::Exception("ompl::base::Constraint(): "
183  "Space is over constrained!");
184  k_ = k;
185  }
186 
188  double getTolerance() const
189  {
190  return tolerance_;
191  }
192 
195  unsigned int getMaxIterations() const
196  {
197  return maxIterations_;
198  }
199 
201  void setTolerance(const double tolerance)
202  {
203  if (tolerance <= 0)
204  throw ompl::Exception("ompl::base::Constraint::setProjectionTolerance(): "
205  "tolerance must be positive.");
206  tolerance_ = tolerance;
207  }
208 
211  void setMaxIterations(const unsigned int iterations)
212  {
213  if (iterations == 0)
214  throw ompl::Exception("ompl::base::Constraint::setProjectionMaxIterations(): "
215  "iterations must be positive.");
216  maxIterations_ = iterations;
217  }
218 
221  protected:
223  const unsigned int n_;
224 
226  unsigned int k_;
227 
230  double tolerance_;
231 
234  unsigned int maxIterations_;
235  };
236 
238  OMPL_CLASS_FORWARD(ConstraintIntersection);
240 
244  class ConstraintIntersection : public Constraint
245  {
246  public:
249  ConstraintIntersection(const unsigned int ambientDim, std::vector<ConstraintPtr> constraints)
250  : Constraint(ambientDim, 0)
251  {
252  for (const auto &constraint : constraints)
253  addConstraint(constraint);
254  }
255 
258  ConstraintIntersection(const unsigned int ambientDim, std::initializer_list<ConstraintPtr> constraints)
259  : Constraint(ambientDim, 0)
260  {
261  for (const auto &constraint : constraints)
262  addConstraint(constraint);
263  }
264 
265  void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
266  {
267  unsigned int i = 0;
268  for (const auto &constraint : constraints_)
269  {
270  constraint->function(x, out.segment(i, constraint->getCoDimension()));
271  i += constraint->getCoDimension();
272  }
273  }
274 
275  void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
276  {
277  unsigned int i = 0;
278  for (const auto &constraint : constraints_)
279  {
280  constraint->jacobian(x, out.block(i, 0, constraint->getCoDimension(), n_));
281  i += constraint->getCoDimension();
282  }
283  }
284 
285  protected:
286  void addConstraint(const ConstraintPtr &constraint)
287  {
288  setManifoldDimension(k_ - constraint->getCoDimension());
289  constraints_.push_back(constraint);
290  }
291 
293  std::vector<ConstraintPtr> constraints_;
294  };
295 
297  OMPL_CLASS_FORWARD(ConstraintObjective);
299 
303  {
304  public:
307  : OptimizationObjective(std::move(si)), constraint_(std::move(constraint))
308  {
309  }
310 
313  Cost stateCost(const State *s) const override
314  {
315  return Cost(constraint_->distance(s));
316  }
317 
318  protected:
321  };
322  } // namespace base
323 } // namespace ompl
324 
325 #endif
unsigned int maxIterations_
Maximum number of iterations for Newton method used in projection onto manifold.
Definition: Constraint.h:266
ConstraintObjective(ConstraintPtr constraint, SpaceInformationPtr si)
Constructor.
Definition: Constraint.h:338
unsigned int getCoDimension() const
Returns the dimension of the manifold.
Definition: Constraint.h:205
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:108
A shared pointer wrapper for ompl::base::Constraint.
Definition of an abstract state.
Definition: State.h:114
const unsigned int n_
Ambient space dimension.
Definition: Constraint.h:255
unsigned int getAmbientDimension() const
Returns the dimension of the ambient space.
Definition: Constraint.h:193
std::vector< ConstraintPtr > constraints_
Constituent constraints.
Definition: Constraint.h:325
unsigned int k_
Manifold dimension.
Definition: Constraint.h:258
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:112
Wrapper around ompl::base::Constraint to use as an optimization objective.
Definition: Constraint.h:335
Abstract definition of optimization objectives.
Definition of a constraint composed of multiple constraints that all must be satisfied simultaneously...
Definition: Constraint.h:277
unsigned int getMaxIterations() const
Returns the maximum number of allowed iterations in the projection routine.
Definition: Constraint.h:227
unsigned int getManifoldDimension() const
Returns the dimension of the manifold.
Definition: Constraint.h:199
virtual bool project(State *state) const
Project a state state given the constraints. If a valid projection cannot be found,...
Definition: Constraint.cpp:88
double getTolerance() const
Returns the tolerance of the projection routine.
Definition: Constraint.h:220
double tolerance_
Tolerance for Newton method used in projection onto manifold.
Definition: Constraint.h:262
void setTolerance(const double tolerance)
Sets the projection tolerance.
Definition: Constraint.h:233
ConstraintIntersection(const unsigned int ambientDim, std::vector< ConstraintPtr > constraints)
Constructor. If constraints is empty assume it will be filled later.
Definition: Constraint.h:281
ConstraintPtr constraint_
Optimizing constraint.
Definition: Constraint.h:352
virtual 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:307
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:345
void setManifoldDimension(unsigned int k)
Sets the underlying manifold dimension.
Definition: Constraint.h:211
virtual 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:79
virtual 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:243
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.