Constraint.h
125 virtual void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const;
258 ConstraintIntersection(const unsigned int ambientDim, std::initializer_list<ConstraintPtr> constraints)
262 addConstraint(constraint);
265 void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
266 {
275 void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
281 i += constraint->getCoDimension();
290 }
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:107
A shared pointer wrapper for ompl::base::Constraint.
unsigned int getAmbientDimension() const
Returns the dimension of the ambient space.
Definition: Constraint.h:193
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
Wrapper around ompl::base::Constraint to use as an optimization objective.
Definition: Constraint.h:334
Abstract definition of optimization objectives.
Definition: OptimizationObjective.h:138
Definition of a constraint composed of multiple constraints that all must be satisfied simultaneously...
Definition: Constraint.h:276
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
ConstraintIntersection(const unsigned int ambientDim, std::vector< ConstraintPtr > constraints)
Constructor. If constraints is empty assume it will be filled later.
Definition: Constraint.h:281
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
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