Loading...
Searching...
No Matches
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
49namespace 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
76 {
77 public:
87 Constraint(const unsigned int ambientDim, const unsigned int coDim,
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
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
The exception type for ompl.
Definition: Exception.h:47
Definition of a constraint composed of multiple constraints that all must be satisfied simultaneously...
Definition: Constraint.h:245
ConstraintIntersection(const unsigned int ambientDim, std::initializer_list< ConstraintPtr > constraints)
Constructor. If constraints is empty assume it will be filled later.
Definition: Constraint.h:258
std::vector< ConstraintPtr > constraints_
Constituent constraints.
Definition: Constraint.h:293
ConstraintIntersection(const unsigned int ambientDim, std::vector< ConstraintPtr > constraints)
Constructor. If constraints is empty assume it will be filled later.
Definition: Constraint.h:249
Wrapper around ompl::base::Constraint to use as an optimization objective.
Definition: Constraint.h:303
ConstraintPtr constraint_
Optimizing constraint.
Definition: Constraint.h:320
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:313
ConstraintObjective(ConstraintPtr constraint, SpaceInformationPtr si)
Constructor.
Definition: Constraint.h:306
A shared pointer wrapper for ompl::base::Constraint.
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:76
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Abstract definition of optimization objectives.
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
static const double CONSTRAINT_PROJECTION_TOLERANCE
Default projection tolerance of a constraint unless otherwise specified.
Definition: Constraint.h:55
static const unsigned int CONSTRAINT_PROJECTION_MAX_ITERATIONS
Maximum number of iterations in projection routine until giving up.
Definition: Constraint.h:59
Main namespace. Contains everything in this library.
STL namespace.