hpp-constraints 4.15.1
Definition of basic geometric constraints for motion planning
Loading...
Searching...
No Matches
hierarchical-iterative.hh
Go to the documentation of this file.
1// Copyright (c) 2017, 2018
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_SOLVER_HIERARCHICAL_ITERATIVE_HH
30#define HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
31
32#include <functional>
37#include <hpp/util/serialization-fwd.hh>
38#include <map>
39
40namespace hpp {
41namespace constraints {
42namespace solver {
45namespace lineSearch {
47struct Constant {
48 template <typename SolverType>
49 bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
50};
51
56
57 template <typename SolverType>
58 bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
59
60 template <typename SolverType>
61 inline value_type computeLocalSlope(const SolverType& solver) const;
62
63 value_type c, tau, smallAlpha; // 0.8 ^ 7 = 0.209, 0.8 ^ 8 = 0.1677
65};
66
72
73 template <typename SolverType>
74 bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
75
78};
79
87 ErrorNormBased(value_type alphaMin = 0.2);
88
89 template <typename SolverType>
90 bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
91
93};
94} // namespace lineSearch
95
96namespace saturation {
104struct Base {
116 virtual bool saturate(vectorIn_t q, vectorOut_t qSat,
117 Eigen::VectorXi& saturation);
118 virtual ~Base() {}
119};
121struct Function : Base {
122 typedef std::function<bool(vectorIn_t, vectorOut_t, Eigen::VectorXi&)>
124 bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation) {
125 return function(q, qSat, saturation);
126 }
130};
132struct Bounds : Base {
133 bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation);
135 Bounds(const vector_t& lb, const vector_t& ub) : lb(lb), ub(ub) {}
137};
139struct Device : Base {
141 bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation);
145};
146} // namespace saturation
147
233 public:
236
237 enum Status { ERROR_INCREASED, MAX_ITERATION_REACHED, INFEASIBLE, SUCCESS };
238 typedef shared_ptr<saturation::Base> Saturation_t;
239
241
243
245
248
250 const LiegroupSpacePtr_t& configSpace() const { return configSpace_; }
254 virtual bool contains(const ImplicitPtr_t& numericalConstraint) const;
255
261 virtual bool add(const ImplicitPtr_t& constraint,
262 const std::size_t& priority);
263
269 virtual void merge(const HierarchicalIterative& other);
270
272 void saturation(const Saturation_t& saturate) { saturate_ = saturate; }
273
275 const Saturation_t& saturation() const { return saturate_; }
276
278
281
296 template <typename LineSearchType>
297 Status solve(vectorOut_t arg, LineSearchType ls = LineSearchType()) const;
298
313 inline Status solve(vectorOut_t arg) const {
314 return solve(arg, DefaultLineSearch());
315 }
316
320 bool isSatisfied(vectorIn_t arg) const {
321 computeValue<false>(arg);
322 computeError();
323 return squaredNorm_ < squaredErrorThreshold_;
324 }
325
330 bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const {
331 computeValue<false>(arg);
332 computeError();
333 return squaredNorm_ < errorThreshold * errorThreshold;
334 }
335
345 bool isConstraintSatisfied(const ImplicitPtr_t& constraint, vectorIn_t arg,
346 vectorOut_t error, bool& constraintFound) const;
347
352 const value_type& sigma() const { return sigma_; }
353
355
358
365 void freeVariables(const segments_t intervals) {
366 freeVariables_ = Indices_t();
367 for (std::size_t i = 0; i < intervals.size(); ++i)
368 freeVariables_.addRow(intervals[i].first, intervals[i].second);
369 freeVariables_.updateIndices<true, true, true>();
370 update();
371 }
372
377 void freeVariables(const Indices_t& indices) {
378 freeVariables_ = indices;
379 update();
380 }
381
383 const Indices_t& freeVariables() const { return freeVariables_; }
384
386 void maxIterations(size_type iterations) { maxIterations_ = iterations; }
388 size_type maxIterations() const { return maxIterations_; }
389
391 void errorThreshold(const value_type& threshold) {
392 squaredErrorThreshold_ = threshold * threshold;
393 }
395 value_type errorThreshold() const { return sqrt(squaredErrorThreshold_); }
397 value_type squaredErrorThreshold() const { return squaredErrorThreshold_; }
398
400 value_type inequalityThreshold() const { return inequalityThreshold_; }
402 void inequalityThreshold(const value_type& it) { inequalityThreshold_ = it; }
403
404 void lastIsOptional(bool optional) { lastIsOptional_ = optional; }
405
406 bool lastIsOptional() const { return lastIsOptional_; }
407
408 // Set whether the constraints are solved level by level
409 //
410 // If true, the solver will consider only the higher level of
411 // priority that is not satisfied when computing the direction
412 // of descent.
413 void solveLevelByLevel(bool solveLevelByLevel) {
414 solveLevelByLevel_ = solveLevelByLevel;
415 }
416
417 // Get whether the constraints are solved level by level
418 bool solveLevelByLevel() const { return solveLevelByLevel_; }
420
423
425 const ImplicitConstraintSet& constraints(const std::size_t priority) {
426 assert(priority < stacks_.size());
427 return stacks_[priority];
428 }
429
431 const NumericalConstraints_t& constraints() const { return constraints_; }
432
433 std::size_t numberStacks() const { return stacks_.size(); }
434
435 const size_type& dimension() const { return dimension_; }
436
439 const size_type& reducedDimension() const { return reducedDimension_; }
440
443
446
448
450 value_type residualError() const { return squaredNorm_; }
451
453 void residualError(vectorOut_t error) const;
454
464
467
475
482 virtual bool rightHandSideFromConfig(const ImplicitPtr_t& constraint,
483 ConfigurationIn_t config);
489 virtual bool rightHandSide(const ImplicitPtr_t& constraint, vectorIn_t rhs);
490
492 virtual bool getRightHandSide(const ImplicitPtr_t& constraint,
493 vectorOut_t rhs) const;
494
499 virtual void rightHandSide(vectorIn_t rhs);
500
504
510
515
517
521
523 template <bool ComputeJac>
524 void computeValue(vectorIn_t arg) const;
526 void getValue(vectorOut_t v) const;
530 void computeError() const;
531
533 const vector_t& lastStep() const { return dq_; }
534
535 virtual bool integrate(vectorIn_t from, vectorIn_t velocity,
536 vectorOut_t result) const;
538
539 virtual std::ostream& print(std::ostream& os) const;
540
541 protected:
542 typedef Eigen::JacobiSVD<matrix_t> SVD_t;
543
544 struct Data {
546 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
548 LiegroupElement output, rightHandSide;
551
554
556
558 std::vector<std::size_t> inequalityIndices;
561 };
562
565 void update();
566
570 virtual void computeActiveRowsOfJ(std::size_t iStack);
571
580 void expandDqSmall() const;
581 void saturate(vectorOut_t arg) const;
582
583 value_type squaredErrorThreshold_, inequalityThreshold_;
585
586 std::vector<ImplicitConstraintSet> stacks_;
588 size_type dimension_, reducedDimension_;
597 std::map<DifferentiableFunctionPtr_t, size_type> iq_;
599 std::map<DifferentiableFunctionPtr_t, size_type> iv_;
601 std::map<DifferentiableFunctionPtr_t, std::size_t> priority_;
602
605
606 mutable vector_t dq_, dqSmall_;
608 mutable Eigen::VectorXi saturation_, reducedSaturation_;
612 mutable std::vector<Data> datas_;
613 mutable SVD_t svd_;
614 mutable vector_t OM_;
615 mutable vector_t OP_;
616
618
619 protected:
621
622 private:
623 HPP_SERIALIZABLE_SPLIT();
624}; // class HierarchicalIterative
626inline std::ostream& operator<<(std::ostream& os,
627 const HierarchicalIterative& hs) {
628 return hs.print(os);
629}
630} // namespace solver
631} // namespace constraints
632} // namespace hpp
633
634#endif // HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
Definition: implicit-constraint-set.hh:45
Definition: hierarchical-iterative.hh:232
const vector_t & lastStep() const
Accessor to the last step done.
Definition: hierarchical-iterative.hh:533
Configuration_t qSat_
Definition: hierarchical-iterative.hh:609
virtual ~HierarchicalIterative()
Definition: hierarchical-iterative.hh:244
virtual bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
virtual bool rightHandSideFromConfig(const ImplicitPtr_t &constraint, ConfigurationIn_t config)
std::vector< ImplicitConstraintSet > stacks_
Definition: hierarchical-iterative.hh:586
lineSearch::FixedSequence DefaultLineSearch
Definition: hierarchical-iterative.hh:235
Status solve(vectorOut_t arg) const
Definition: hierarchical-iterative.hh:313
Saturation_t saturate_
Definition: hierarchical-iterative.hh:593
virtual bool getRightHandSide(const ImplicitPtr_t &constraint, vectorOut_t rhs) const
Get right hand side of a constraints.
bool solveLevelByLevel() const
Definition: hierarchical-iterative.hh:418
virtual void computeActiveRowsOfJ(std::size_t iStack)
size_type maxIterations_
Definition: hierarchical-iterative.hh:584
void saturation(const Saturation_t &saturate)
Set the saturation function.
Definition: hierarchical-iterative.hh:272
vector_t rightHandSideFromConfig(ConfigurationIn_t config)
bool solveLevelByLevel_
Definition: hierarchical-iterative.hh:590
Eigen::VectorXi reducedSaturation_
Definition: hierarchical-iterative.hh:608
virtual std::ostream & print(std::ostream &os) const
const ImplicitConstraintSet & constraints(const std::size_t priority)
Get set of constraints for a give priority level.
Definition: hierarchical-iterative.hh:425
void freeVariables(const Indices_t &indices)
Definition: hierarchical-iterative.hh:377
bool isConstraintSatisfied(const ImplicitPtr_t &constraint, vectorIn_t arg, vectorOut_t error, bool &constraintFound) const
vector_t OM_
Definition: hierarchical-iterative.hh:614
HierarchicalIterative(const LiegroupSpacePtr_t &configSpace)
value_type residualError() const
Returns the squared norm of the error vector.
Definition: hierarchical-iterative.hh:450
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: hierarchical-iterative.hh:391
HierarchicalIterative(const HierarchicalIterative &other)
value_type squaredNorm_
Definition: hierarchical-iterative.hh:611
void computeValue(vectorIn_t arg) const
Compute the value of each level, and the jacobian if ComputeJac is true.
std::size_t numberStacks() const
Definition: hierarchical-iterative.hh:433
value_type inequalityThreshold_
Definition: hierarchical-iterative.hh:583
virtual bool add(const ImplicitPtr_t &constraint, const std::size_t &priority)
std::map< DifferentiableFunctionPtr_t, std::size_t > priority_
Priority level of constraint.
Definition: hierarchical-iterative.hh:601
LiegroupSpacePtr_t configSpace_
Definition: hierarchical-iterative.hh:587
const value_type & sigma() const
Definition: hierarchical-iterative.hh:352
Status solve(vectorOut_t arg, LineSearchType ls=LineSearchType()) const
void computeSaturation(vectorIn_t arg) const
vector_t dq_
Definition: hierarchical-iterative.hh:606
virtual void rightHandSide(vectorIn_t rhs)
Eigen::JacobiSVD< matrix_t > SVD_t
Definition: hierarchical-iterative.hh:542
HierarchicalIterative()
Definition: hierarchical-iterative.hh:620
const Saturation_t & saturation() const
Get the saturation function.
Definition: hierarchical-iterative.hh:275
Status
Definition: hierarchical-iterative.hh:237
@ ERROR_INCREASED
Definition: hierarchical-iterative.hh:237
ArrayXb activeDerivativeParameters() const
Velocity parameters involved in the constraint resolution.
void freeVariables(const segments_t intervals)
Definition: hierarchical-iterative.hh:365
const size_type & reducedDimension() const
Definition: hierarchical-iterative.hh:439
vector_t OP_
Definition: hierarchical-iterative.hh:615
std::map< DifferentiableFunctionPtr_t, size_type > iq_
Value rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:597
void lastIsOptional(bool optional)
Definition: hierarchical-iterative.hh:404
value_type sigma_
The smallest non-zero singular value.
Definition: hierarchical-iterative.hh:604
std::vector< Data > datas_
Definition: hierarchical-iterative.hh:612
void inequalityThreshold(const value_type &it)
set the inequality threshold
Definition: hierarchical-iterative.hh:402
ArrayXb tmpSat_
Definition: hierarchical-iterative.hh:610
NumericalConstraints_t constraints_
Members moved from core::ConfigProjector.
Definition: hierarchical-iterative.hh:595
void residualError(vectorOut_t error) const
Returns the error vector.
const Indices_t & freeVariables() const
Get free velocity variables.
Definition: hierarchical-iterative.hh:383
bool lastIsOptional() const
Definition: hierarchical-iterative.hh:406
bool isSatisfied(vectorIn_t arg) const
Definition: hierarchical-iterative.hh:320
virtual void merge(const HierarchicalIterative &other)
const NumericalConstraints_t & constraints() const
Get constraints (implicit and explicit)
Definition: hierarchical-iterative.hh:431
virtual bool contains(const ImplicitPtr_t &numericalConstraint) const
ArrayXb activeParameters() const
Configuration parameters involved in the constraint resolution.
bool definesSubmanifoldOf(const HierarchicalIterative &solver) const
void solveLevelByLevel(bool solveLevelByLevel)
Definition: hierarchical-iterative.hh:413
Eigen::RowBlockIndices Indices_t
Definition: hierarchical-iterative.hh:234
size_type dimension_
Definition: hierarchical-iterative.hh:588
shared_ptr< saturation::Base > Saturation_t
Definition: hierarchical-iterative.hh:238
std::map< DifferentiableFunctionPtr_t, size_type > iv_
Derivative rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:599
size_type maxIterations() const
Get maximal number of iterations in config projector.
Definition: hierarchical-iterative.hh:388
void maxIterations(size_type iterations)
Set maximal number of iterations.
Definition: hierarchical-iterative.hh:386
bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const
Definition: hierarchical-iterative.hh:330
SVD_t svd_
Definition: hierarchical-iterative.hh:613
value_type inequalityThreshold() const
Get the inequality threshold.
Definition: hierarchical-iterative.hh:400
value_type errorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:395
Indices_t freeVariables_
Unknown of the set of implicit constraints.
Definition: hierarchical-iterative.hh:592
const LiegroupSpacePtr_t & configSpace() const
Get configuration space on which constraints are defined.
Definition: hierarchical-iterative.hh:250
matrix_t reducedJ_
Definition: hierarchical-iterative.hh:607
bool lastIsOptional_
Definition: hierarchical-iterative.hh:589
value_type squaredErrorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:397
virtual bool rightHandSide(const ImplicitPtr_t &constraint, vectorIn_t rhs)
const size_type & dimension() const
Definition: hierarchical-iterative.hh:435
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
std::ostream & operator<<(std::ostream &os, const HierarchicalIterative &hs)
Definition: hierarchical-iterative.hh:626
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:65
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:105
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:182
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:109
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:69
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:175
pinocchio::size_type size_type
Definition: fwd.hh:47
pinocchio::ArrayXb ArrayXb
Definition: fwd.hh:80
pinocchio::value_type value_type
Definition: fwd.hh:48
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:60
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
pinocchio::matrix_t matrix_t
Definition: fwd.hh:56
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:58
std::vector< segment_t > segments_t
Definition: fwd.hh:84
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:61
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition: fwd.hh:177
pinocchio::vector_t vector_t
Definition: fwd.hh:59
Definition: active-set-differentiable-function.hh:36
Definition: hierarchical-iterative.hh:544
std::vector< std::size_t > inequalityIndices
Definition: hierarchical-iterative.hh:558
LiegroupElement output
Definition: hierarchical-iterative.hh:548
size_type maxRank
Definition: hierarchical-iterative.hh:555
ComparisonTypes_t comparison
Definition: hierarchical-iterative.hh:557
Eigen::RowBlockIndices equalityIndices
Definition: hierarchical-iterative.hh:559
matrix_t jacobian
Definition: hierarchical-iterative.hh:550
SVD_t svd
Definition: hierarchical-iterative.hh:552
vector_t error
Definition: hierarchical-iterative.hh:549
matrix_t PK
Definition: hierarchical-iterative.hh:553
Eigen::MatrixBlocks< false, false > activeRowsOfJ
Definition: hierarchical-iterative.hh:560
Definition: hierarchical-iterative.hh:54
vector_t df
Definition: hierarchical-iterative.hh:64
vector_t arg_darg
Definition: hierarchical-iterative.hh:64
value_type smallAlpha
Definition: hierarchical-iterative.hh:63
value_type tau
Definition: hierarchical-iterative.hh:63
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:48
value_type computeLocalSlope(const SolverType &solver) const
Definition: hierarchical-iterative.hh:94
value_type c
Definition: hierarchical-iterative.hh:63
vector_t darg
Definition: hierarchical-iterative.hh:64
No line search. Use .
Definition: hierarchical-iterative.hh:47
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:41
Definition: hierarchical-iterative.hh:85
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:118
value_type b
Definition: hierarchical-iterative.hh:92
value_type K
Definition: hierarchical-iterative.hh:92
ErrorNormBased(value_type alphaMin, value_type _a, value_type _b)
value_type a
Definition: hierarchical-iterative.hh:92
value_type C
Definition: hierarchical-iterative.hh:92
Definition: hierarchical-iterative.hh:70
value_type alphaMax
Definition: hierarchical-iterative.hh:77
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:109
value_type K
Definition: hierarchical-iterative.hh:77
value_type alpha
Definition: hierarchical-iterative.hh:76
Base class for box constraints. To prevent configuration variables to get out of joint limits during ...
Definition: hierarchical-iterative.hh:104
virtual ~Base()
Definition: hierarchical-iterative.hh:118
virtual bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
simple box constraints
Definition: hierarchical-iterative.hh:132
Bounds(const vector_t &lb, const vector_t &ub)
Definition: hierarchical-iterative.hh:135
vector_t ub
Definition: hierarchical-iterative.hh:136
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
vector_t lb
Definition: hierarchical-iterative.hh:136
Bounds()
Definition: hierarchical-iterative.hh:134
Box constraints use a Device joint limits.
Definition: hierarchical-iterative.hh:139
DevicePtr_t device
Definition: hierarchical-iterative.hh:144
Device()
Definition: hierarchical-iterative.hh:142
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
Device(const DevicePtr_t &device)
Definition: hierarchical-iterative.hh:143
saturation from a std::function.
Definition: hierarchical-iterative.hh:121
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
Definition: hierarchical-iterative.hh:124
function_t function
Definition: hierarchical-iterative.hh:129
Function()
Definition: hierarchical-iterative.hh:127
Function(const function_t &function)
Definition: hierarchical-iterative.hh:128
std::function< bool(vectorIn_t, vectorOut_t, Eigen::VectorXi &)> function_t
Definition: hierarchical-iterative.hh:123