37#ifndef OMPL_BASE_CONSTRAINTS_CONSTRAINT_
38#define OMPL_BASE_CONSTRAINTS_CONSTRAINT_
40#include "ompl/base/StateSpace.h"
41#include "ompl/base/OptimizationObjective.h"
42#include "ompl/util/ClassForward.h"
43#include "ompl/util/Exception.h"
66 OMPL_CLASS_FORWARD(Constraint);
87 Constraint(
const unsigned int ambientDim,
const unsigned int coDim,
90 , k_(ambientDim - coDim)
91 , tolerance_(tolerance)
94 if (n_ <= 0 || k_ <= 0)
96 "Ambient and manifold dimensions must be positive.");
106 virtual void function(
const State *state, Eigen::Ref<Eigen::VectorXd> out)
const;
110 virtual void function(
const Eigen::Ref<const Eigen::VectorXd> &x,
111 Eigen::Ref<Eigen::VectorXd> out)
const = 0;
118 virtual void jacobian(
const State *state, Eigen::Ref<Eigen::MatrixXd> out)
const;
125 virtual void jacobian(
const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out)
const;
135 virtual bool project(
State *state)
const;
139 virtual bool project(Eigen::Ref<Eigen::VectorXd> x)
const;
143 virtual double distance(
const State *state)
const;
146 virtual double distance(
const Eigen::Ref<const Eigen::VectorXd> &x)
const;
150 virtual bool isSatisfied(
const State *state)
const;
153 virtual bool isSatisfied(
const Eigen::Ref<const Eigen::VectorXd> &x)
const;
161 unsigned int getAmbientDimension()
const
167 unsigned int getManifoldDimension()
const
173 unsigned int getCoDimension()
const
179 void setManifoldDimension(
unsigned int k)
183 "Space is over constrained!");
188 double getTolerance()
const
195 unsigned int getMaxIterations()
const
197 return maxIterations_;
201 void setTolerance(
const double tolerance)
204 throw ompl::Exception(
"ompl::base::Constraint::setProjectionTolerance(): "
205 "tolerance must be positive.");
206 tolerance_ = tolerance;
211 void setMaxIterations(
const unsigned int iterations)
214 throw ompl::Exception(
"ompl::base::Constraint::setProjectionMaxIterations(): "
215 "iterations must be positive.");
216 maxIterations_ = iterations;
223 const unsigned int n_;
234 unsigned int maxIterations_;
252 for (
const auto &constraint : constraints)
253 addConstraint(constraint);
261 for (
const auto &constraint : constraints)
262 addConstraint(constraint);
265 void function(
const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out)
const override
270 constraint->function(x, out.segment(i, constraint->getCoDimension()));
271 i += constraint->getCoDimension();
275 void jacobian(
const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out)
const override
280 constraint->jacobian(x, out.block(i, 0, constraint->getCoDimension(), n_));
281 i += constraint->getCoDimension();
288 setManifoldDimension(k_ - constraint->getCoDimension());
The exception type for ompl.
Definition of a constraint composed of multiple constraints that all must be satisfied simultaneously...
ConstraintIntersection(const unsigned int ambientDim, std::initializer_list< ConstraintPtr > constraints)
Constructor. If constraints is empty assume it will be filled later.
std::vector< ConstraintPtr > constraints_
Constituent constraints.
ConstraintIntersection(const unsigned int ambientDim, std::vector< ConstraintPtr > constraints)
Constructor. If constraints is empty assume it will be filled later.
Wrapper around ompl::base::Constraint to use as an optimization objective.
ConstraintPtr constraint_
Optimizing constraint.
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...
ConstraintObjective(ConstraintPtr constraint, SpaceInformationPtr si)
Constructor.
A shared pointer wrapper for ompl::base::Constraint.
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Abstract definition of optimization objectives.
Definition of an abstract state.
static const double CONSTRAINT_PROJECTION_TOLERANCE
Default projection tolerance of a constraint unless otherwise specified.
static const unsigned int CONSTRAINT_PROJECTION_MAX_ITERATIONS
Maximum number of iterations in projection routine until giving up.
Main namespace. Contains everything in this library.