hpp-constraints: include/hpp/constraints/explicit-constraint-set.hh Source File

hpp-constraints 9.0.2 Definition of basic geometric constraints for motion planning

Loading...
Searching...
No Matches
explicit-constraint-set.hh
Go to the documentation of this file.
1// Copyright (c) 2017, Joseph Mirabel
2// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3//
4
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are
7// met:
8//
9// 1. Redistributions of source code must retain the above copyright
10// notice, this list of conditions and the following disclaimer.
11//
12// 2. Redistributions in binary form must reproduce the above copyright
13// notice, this list of conditions and the following disclaimer in the
14// documentation and/or other materials provided with the distribution.
15//
16// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27// DAMAGE.
28
29#ifndef HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
30#define HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
31
32#include <hpp/constraints/config.hh>
33#include <hpp/constraints/differentiable-function-set.hh>
34#include <hpp/constraints/fwd.hh>
35#include <hpp/constraints/matrix-view.hh>
36#include <vector>
37
38 namespace hpp {
39 namespace constraints {
42
99 class HPP_CONSTRAINTS_DLLAPI ExplicitConstraintSet {
100 public:
101 typedef Eigen::RowBlockIndices RowBlockIndices;
102 typedef Eigen::ColBlockIndices ColBlockIndices;
103 typedef Eigen::MatrixBlockView<matrix_t, Eigen::Dynamic, Eigen::Dynamic,
104 false, false>
105 MatrixBlockView;
106
109
116 bool solve(vectorOut_t arg) const;
117
121 size_type errorSize() const { return errorSize_; }
122
128 bool isSatisfied(vectorIn_t arg, value_type errorThreshold = -1) const;
129
136 bool isSatisfied(vectorIn_t arg, vectorOut_t error,
137 value_type errorThreshold = -1) const;
138
148 bool isConstraintSatisfied(const ImplicitPtr_t& constraint, vectorIn_t arg,
149 vectorOut_t error, bool& constraintFound) const;
151
154
162 size_type add(const ExplicitPtr_t& constraint);
163
170 bool contains(const ExplicitPtr_t& numericalConstraint) const;
171
175 ExplicitConstraintSet(const LiegroupSpacePtr_t& space)
176 : configSpace_(space),
177 inArgs_(),
178 notOutArgs_(),
179 inDers_(),
180 notOutDers_(),
181 outArgs_(),
182 outDers_(),
183 argFunction_(Eigen::VectorXi::Constant(space->nq(), -1)),
184 derFunction_(Eigen::VectorXi::Constant(space->nv(), -1)),
185 errorThreshold_(Eigen::NumTraits<value_type>::epsilon()),
186 errorSize_(0)
187 // , Jg (nv, nv)
188 ,
189 arg_(space->nq()),
190 diff_(space->nv()),
191 diffSmall_() {
192 notOutArgs_.addRow(0, space->nq());
193 notOutDers_.addCol(0, space->nv());
194 }
195
196 ExplicitConstraintSet(const ExplicitConstraintSet& other)
197 : configSpace_(other.configSpace_),
198 inArgs_(other.inArgs_),
199 notOutArgs_(other.notOutArgs_),
200 inDers_(other.inDers_),
201 notOutDers_(other.notOutDers_),
202 outArgs_(other.outArgs_),
203 outDers_(other.outDers_),
204 inOutDependencies_(other.inOutDependencies_),
205 data_(other.data_),
206 computationOrder_(other.computationOrder_),
207 argFunction_(other.argFunction_),
208 derFunction_(other.derFunction_),
209 errorThreshold_(other.errorThreshold_),
210 errorSize_(other.errorSize_),
211 arg_(other.arg_),
212 diff_(other.diff_),
213 diffSmall_(other.diffSmall_) {}
215
218
220 void errorThreshold(const value_type& threshold) {
221 errorThreshold_ = threshold;
222 }
224 value_type errorThreshold() const { return errorThreshold_; }
226 value_type squaredErrorThreshold() const {
227 return errorThreshold_ * errorThreshold_;
228 }
229
231
234
236 const RowBlockIndices& inArgs() const { return inArgs_; }
237
239 const ColBlockIndices& inDers() const { return inDers_; }
240
244 const RowBlockIndices& notOutArgs() const { return notOutArgs_; }
245
249 const ColBlockIndices& notOutDers() const { return notOutDers_; }
250
252 ColBlockIndices activeParameters() const;
253
255 const ColBlockIndices& activeDerivativeParameters() const;
256
262 const Eigen::MatrixXi& inOutDependencies() const {
263 return inOutDependencies_;
264 }
265
267 Eigen::MatrixXi inOutDofDependencies() const;
268
269 const Eigen::VectorXi& derFunction() const { return derFunction_; }
270
275 const RowBlockIndices& outArgs() const { return outArgs_; }
276
281 const RowBlockIndices& outDers() const { return outDers_; }
282
284 LiegroupSpacePtr_t configSpace() const { return configSpace_; }
285
287 std::size_t nq() const { return configSpace_->nq(); }
288
290 std::size_t nv() const { return configSpace_->nv(); }
291
293
306 inline MatrixBlockView jacobianNotOutToOut(matrix_t& jacobian) const {
307 return MatrixBlockView(jacobian, outDers_.nbIndices(), outDers_.indices(),
308 notOutDers_.nbIndices(), notOutDers_.indices());
309 }
310
327 void jacobian(matrixOut_t jacobian, vectorIn_t q) const;
328
331
351 vector_t rightHandSideFromInput(vectorIn_t p);
352
373 bool rightHandSideFromInput(const ExplicitPtr_t& constraint, vectorIn_t p);
374
395 void rightHandSideFromInput(const size_type& i, vectorIn_t p);
396
403 void rightHandSide(vectorIn_t rhs);
404
409 bool rightHandSide(const ExplicitPtr_t& constraint, vectorIn_t rhs);
410
415 bool getRightHandSide(const ExplicitPtr_t& constraint, vectorOut_t rhs) const;
416
421 void rightHandSide(const size_type& i, vectorIn_t rhs);
422
426 vector_t rightHandSide() const;
427
431 size_type rightHandSideSize() const;
432
434
435 std::ostream& print(std::ostream& os) const;
436
437 private:
438 typedef std::vector<bool> Computed_t;
439
444 void solveExplicitConstraint(const std::size_t& i, vectorOut_t arg) const;
459 void computeJacobian(const std::size_t& i, matrixOut_t J) const;
460 void computeOrder(const std::size_t& iF, std::size_t& iOrder,
461 Computed_t& computed);
462
469 void replace(const ConstraintReplacement_t& oldToNew);
470
471 LiegroupSpacePtr_t configSpace_;
472
473 struct Data {
474 Data(const ExplicitPtr_t& constraint);
475 ExplicitPtr_t constraint;
476 RowBlockIndices equalityIndices;
477 LiegroupElement rhs_implicit;
478 // implicit formulation
479 mutable LiegroupElement h_value;
480 // explicit formulation
481 mutable vector_t qin, qout;
482 mutable LiegroupElement f_value, res_qout;
483 // jacobian of f
484 mutable matrix_t jacobian;
485 }; // struct Data
486
487 RowBlockIndices inArgs_, notOutArgs_;
488 ColBlockIndices inDers_, notOutDers_;
490 RowBlockIndices outArgs_, outDers_;
491
492 Eigen::MatrixXi inOutDependencies_;
493
494 std::vector<Data> data_;
495 std::vector<std::size_t> computationOrder_;
501 Eigen::VectorXi argFunction_, derFunction_;
502 value_type errorThreshold_;
503 size_type errorSize_;
504 // mutable matrix_t Jg;
505 mutable vector_t arg_, diff_, diffSmall_;
506
508 ExplicitConstraintSet()
509 : inArgs_(),
510 notOutArgs_(),
511 inDers_(),
512 notOutDers_(),
513 outArgs_(),
514 outDers_(),
515 errorThreshold_(Eigen::NumTraits<value_type>::epsilon()),
516 errorSize_(0) {}
518 void init(const LiegroupSpacePtr_t& space) {
519 configSpace_ = space;
520 argFunction_ = Eigen::VectorXi::Constant(space->nq(), -1);
521 derFunction_ = Eigen::VectorXi::Constant(space->nv(), -1);
522 arg_.resize(space->nq());
523 diff_.resize(space->nv());
524
525 notOutArgs_.addRow(0, space->nq());
526 notOutDers_.addCol(0, space->nv());
527 }
528
529 friend class solver::BySubstitution;
530}; // class ExplicitConstraintSet
532} // namespace constraints
533} // namespace hpp
534
535#endif // HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
Eigen::MatrixBlockView
Definition matrix-view.hh:835
Eigen::MatrixBlocks< false, true >
hpp::constraints::ExplicitConstraintSet
Definition explicit-constraint-set.hh:99
hpp::constraints::ExplicitConstraintSet::activeParameters
ColBlockIndices activeParameters() const
Same as inArgs.
hpp::constraints::ExplicitConstraintSet::jacobian
void jacobian(matrixOut_t jacobian, vectorIn_t q) const
hpp::constraints::ExplicitConstraintSet::contains
bool contains(const ExplicitPtr_t &numericalConstraint) const
hpp::constraints::ExplicitConstraintSet::print
std::ostream & print(std::ostream &os) const
hpp::constraints::ExplicitConstraintSet::outArgs
const RowBlockIndices & outArgs() const
Definition explicit-constraint-set.hh:275
hpp::constraints::ExplicitConstraintSet::ColBlockIndices
Eigen::ColBlockIndices ColBlockIndices
Definition explicit-constraint-set.hh:102
hpp::constraints::ExplicitConstraintSet::isSatisfied
bool isSatisfied(vectorIn_t arg, vectorOut_t error, value_type errorThreshold=-1) const
hpp::constraints::ExplicitConstraintSet::ExplicitConstraintSet
ExplicitConstraintSet(const LiegroupSpacePtr_t &space)
Definition explicit-constraint-set.hh:175
hpp::constraints::ExplicitConstraintSet::errorSize
size_type errorSize() const
Definition explicit-constraint-set.hh:121
hpp::constraints::ExplicitConstraintSet::rightHandSideFromInput
bool rightHandSideFromInput(const ExplicitPtr_t &constraint, vectorIn_t p)
hpp::constraints::ExplicitConstraintSet::isConstraintSatisfied
bool isConstraintSatisfied(const ImplicitPtr_t &constraint, vectorIn_t arg, vectorOut_t error, bool &constraintFound) const
hpp::constraints::ExplicitConstraintSet::add
size_type add(const ExplicitPtr_t &constraint)
hpp::constraints::ExplicitConstraintSet::rightHandSide
void rightHandSide(const size_type &i, vectorIn_t rhs)
hpp::constraints::ExplicitConstraintSet::isSatisfied
bool isSatisfied(vectorIn_t arg, value_type errorThreshold=-1) const
hpp::constraints::ExplicitConstraintSet::MatrixBlockView
Eigen::MatrixBlockView< matrix_t, Eigen::Dynamic, Eigen::Dynamic, false, false > MatrixBlockView
Definition explicit-constraint-set.hh:105
hpp::constraints::ExplicitConstraintSet::ExplicitConstraintSet
ExplicitConstraintSet(const ExplicitConstraintSet &other)
Definition explicit-constraint-set.hh:196
hpp::constraints::ExplicitConstraintSet::rightHandSide
void rightHandSide(vectorIn_t rhs)
hpp::constraints::ExplicitConstraintSet::jacobianNotOutToOut
MatrixBlockView jacobianNotOutToOut(matrix_t &jacobian) const
Definition explicit-constraint-set.hh:306
hpp::constraints::ExplicitConstraintSet::rightHandSideFromInput
void rightHandSideFromInput(const size_type &i, vectorIn_t p)
hpp::constraints::ExplicitConstraintSet::inArgs
const RowBlockIndices & inArgs() const
Set of input configuration variables.
Definition explicit-constraint-set.hh:236
hpp::constraints::ExplicitConstraintSet::inDers
const ColBlockIndices & inDers() const
Set of input velocity variables.
Definition explicit-constraint-set.hh:239
hpp::constraints::ExplicitConstraintSet::inOutDependencies
const Eigen::MatrixXi & inOutDependencies() const
Definition explicit-constraint-set.hh:262
hpp::constraints::ExplicitConstraintSet::RowBlockIndices
Eigen::RowBlockIndices RowBlockIndices
Definition explicit-constraint-set.hh:101
hpp::constraints::ExplicitConstraintSet::errorThreshold
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition explicit-constraint-set.hh:220
hpp::constraints::ExplicitConstraintSet::rightHandSide
vector_t rightHandSide() const
hpp::constraints::ExplicitConstraintSet::nq
std::size_t nq() const
The number of variables.
Definition explicit-constraint-set.hh:287
hpp::constraints::ExplicitConstraintSet::activeDerivativeParameters
const ColBlockIndices & activeDerivativeParameters() const
Same as inDers.
hpp::constraints::ExplicitConstraintSet::rightHandSideFromInput
vector_t rightHandSideFromInput(vectorIn_t p)
hpp::constraints::ExplicitConstraintSet::inOutDofDependencies
Eigen::MatrixXi inOutDofDependencies() const
Same as inOutDependencies except that cols correpond to DoFs.
hpp::constraints::ExplicitConstraintSet::errorThreshold
value_type errorThreshold() const
Get error threshold.
Definition explicit-constraint-set.hh:224
hpp::constraints::ExplicitConstraintSet::derFunction
const Eigen::VectorXi & derFunction() const
Definition explicit-constraint-set.hh:269
hpp::constraints::ExplicitConstraintSet::getRightHandSide
bool getRightHandSide(const ExplicitPtr_t &constraint, vectorOut_t rhs) const
hpp::constraints::ExplicitConstraintSet::notOutDers
const ColBlockIndices & notOutDers() const
Definition explicit-constraint-set.hh:249
hpp::constraints::ExplicitConstraintSet::squaredErrorThreshold
value_type squaredErrorThreshold() const
Get error threshold.
Definition explicit-constraint-set.hh:226
hpp::constraints::ExplicitConstraintSet::nv
std::size_t nv() const
The number of derivative variables.
Definition explicit-constraint-set.hh:290
hpp::constraints::ExplicitConstraintSet::rightHandSideSize
size_type rightHandSideSize() const
hpp::constraints::ExplicitConstraintSet::outDers
const RowBlockIndices & outDers() const
Definition explicit-constraint-set.hh:281
hpp::constraints::ExplicitConstraintSet::solve
bool solve(vectorOut_t arg) const
hpp::constraints::ExplicitConstraintSet::notOutArgs
const RowBlockIndices & notOutArgs() const
Definition explicit-constraint-set.hh:244
hpp::constraints::ExplicitConstraintSet::configSpace
LiegroupSpacePtr_t configSpace() const
The Lie group on which constraints are defined.
Definition explicit-constraint-set.hh:284
hpp::constraints::ExplicitConstraintSet::rightHandSide
bool rightHandSide(const ExplicitPtr_t &constraint, vectorIn_t rhs)
hpp::constraints::solver::BySubstitution
Definition by-substitution.hh:73
hpp::pinocchio::LiegroupElementBase
config.hh
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
differentiable-function-set.hh
fwd.hh
matrix-view.hh
Eigen
Definition fwd.hh:39
hpp::constraints::ExplicitPtr_t
shared_ptr< Explicit > ExplicitPtr_t
Definition fwd.hh:185
hpp::constraints::LiegroupSpacePtr_t
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition fwd.hh:70
hpp::constraints::ImplicitPtr_t
shared_ptr< Implicit > ImplicitPtr_t
Definition fwd.hh:175
hpp::constraints::size_type
pinocchio::size_type size_type
Definition fwd.hh:48
hpp::constraints::value_type
pinocchio::value_type value_type
Definition fwd.hh:49
hpp::constraints::ConstraintReplacement_t
std::map< ImplicitPtr_t, ImplicitPtr_t > ConstraintReplacement_t
Definition fwd.hh:189
hpp::constraints::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition fwd.hh:61
hpp::constraints::matrix_t
pinocchio::matrix_t matrix_t
Definition fwd.hh:57
hpp::constraints::matrixOut_t
Eigen::Ref< matrix_t > matrixOut_t
Definition fwd.hh:59
hpp::constraints::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition fwd.hh:62
hpp::constraints::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:60
hpp