GCC Code Coverage Report

Directory: ./
File: include/hpp/constraints/solver/hierarchical-iterative.hh
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 40 42 95.2%
Branches: 13 52 25.0%
Line Branch Exec Source
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>
33 #include <hpp/constraints/config.hh>
34 #include <hpp/constraints/fwd.hh>
35 #include <hpp/constraints/implicit-constraint-set.hh>
36 #include <hpp/constraints/matrix-view.hh>
37 #include <hpp/util/serialization-fwd.hh>
38 #include <map>
39
40 namespace hpp {
41 namespace constraints {
42 namespace solver {
43 /// \addtogroup solvers
44 /// \{
45 namespace lineSearch {
46 /// No line search. Use \f$\alpha_i = 1\f$
47 struct Constant {
48 template <typename SolverType>
49 bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
50 };
51
52 /// Implements the backtracking line search algorithm.
53 /// See https://en.wikipedia.org/wiki/Backtracking_line_search.
54 struct Backtracking {
55 Backtracking();
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
64 mutable vector_t arg_darg, df, darg;
65 };
66
67 /// The step size is computed using the recursion
68 /// \f$ \alpha_{i+1} = \alpha - K \times (\alpha_{max} - \alpha_i) \f$
69 /// where \f$K\f$ and \f$\alpha_{max}\f$ are some constant values.
70 struct FixedSequence {
71 FixedSequence();
72
73 template <typename SolverType>
74 bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
75
76 value_type alpha;
77 value_type alphaMax, K;
78 };
79
80 /// The step size is computed using the formula
81 /// \f$ \alpha_{i} = C - K \times \text{tanh}(a
82 /// \frac{\|f(\mathbf{q}_i)\|}{\epsilon^2} + b) \f$, where \li \f$\epsilon\f$ is
83 /// the error threshold: if \f$\|f(\mathbf{q}_i)\|<\epsilon\f$,
84 /// \f$\mathbf{q}_i\f$ is considered to satisfy the constraint.
85 struct ErrorNormBased {
86 ErrorNormBased(value_type alphaMin, value_type _a, value_type _b);
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
92 value_type C, K, a, b;
93 };
94 } // namespace lineSearch
95
96 namespace saturation {
97 /// \brief Base class for box constraints.
98 /// To prevent configuration variables to get out of joint limits during
99 /// Newton Raphson iterations, the user may provide an object derived
100 /// from this type to HierarchicalIterative using setter and getter
101 /// HierarchicalIterative::saturation.
102 ///
103 /// This class implements no saturation.
104 struct Base {
105 /// This function checks which degrees of freedom are saturated.
106 ///
107 /// \param q a configuration,
108 /// \retval qSat configuration after saturing values out of bounds
109 /// \retval saturation vector: for each degree of freedom, saturation
110 /// is set to
111 /// \li -1 if the lower bound is reached,
112 /// \li 1 if the upper bound is reached,
113 /// \li 0 otherwise.
114 /// \return true if and only if at least one degree of freedom has been
115 /// saturated
116 virtual bool saturate(vectorIn_t q, vectorOut_t qSat,
117 Eigen::VectorXi& saturation);
118 4174 virtual ~Base() {}
119 };
120 /// \brief saturation from a std::function.
121 struct Function : Base {
122 typedef std::function<bool(vectorIn_t, vectorOut_t, Eigen::VectorXi&)>
123 function_t;
124 264 bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation) {
125
2/4
✓ Branch 2 taken 264 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 264 times.
✗ Branch 6 not taken.
 264 return function(q, qSat, saturation);
126 }
127 Function() {}
128
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 Function(const function_t& function) : function(function) {}
129 function_t function;
130 };
131 /// \brief simple box constraints
132 struct Bounds : Base {
133 bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation);
134 ✗ Bounds() {}
135
3/6
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
 12 Bounds(const vector_t& lb, const vector_t& ub) : lb(lb), ub(ub), iq2iv_() {
136
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 iq2iv_.resize(ub.size());
137
2/2
✓ Branch 1 taken 85 times.
✓ Branch 2 taken 12 times.
 97 for (size_type i = 0; i < ub.size(); ++i) {
138
1/2
✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
 85 iq2iv_[i] = i;
139 }
140 12 }
141 Bounds(const vector_t& lb, const vector_t& ub, const Eigen::VectorXi& iq2iv)
142 : lb(lb), ub(ub), iq2iv_(iq2iv) {}
143 vector_t lb, ub;
144 Eigen::VectorXi iq2iv_;
145 };
146 /// \brief Box constraints use a Device joint limits.
147 struct Device : Base {
148 /// \todo write a visitor.
149 bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation);
150 1 Device() {}
151 4 Device(const DevicePtr_t& device) : device(device) {}
152 DevicePtr_t device;
153 };
154 } // namespace saturation
155
156 /// Solve a system of non-linear equations on a robot configuration
157 ///
158 /// The non-linear system of equations is built by adding equations with
159 /// method HierarchicalIterative::add.
160 ///
161 /// Note that a hierarchy between the equations can be
162 /// provided. In this case, the solver will try to solve the
163 /// highest priority equations first, and then to solve the lower priority
164 /// equations. Note that priorities are in decreasing order: 0 has higher
165 /// priority than 1.
166 ///
167 /// The algorithm used is a Newton-Raphson like algorithm that works as
168 /// follows: for a single level of priority, let \f$f (\mathbf{q}) = 0\f$
169 /// be the system of equations where
170 /// \f$f\f$ is a \f$C^1\f$ mapping from the robot configuration space to
171 /// a Lie group space \f$\mathcal{L}\f$.
172 ///
173 /// Starting from initial guess \f$\mathbf{q}_0\f$, the method
174 /// HierarchicalIterative::solve builds a sequence of configurations
175 /// \f$\mathbf{q}_i\f$ as follows:
176 /// \f{eqnarray*}
177 /// \mathbf{q}_{i+1} = \mathbf{q}_i -
178 /// \alpha_i \frac{\partial f}{\partial \mathbf{q}}(\mathbf{q}_i)^{+}
179 /// f (\mathbf{q}_i)
180 /// \f}
181 /// where
182 /// \li \f$\frac{\partial f}{\partial \mathbf{q}}(\mathbf{q}_i)^{+}\f$ is
183 /// the Moore-Penrose pseudo-inverse of the system Jacobian,
184 /// \li \f$\alpha_i\f$ is a sequence of real numbers depending on the
185 /// line search strategy. Possible line-search strategies are
186 /// lineSearch::Constant, lineSearch::Backtracking,
187 /// lineSearch::FixedSequence, lineSearch::ErrorNormBased.
188 /// until
189 /// \li the residual \f$\|f(\mathbf{q})\|\f$ is below an error threshold, or
190 /// \li the maximal number of iterations has been reached.
191 ///
192 /// The computation of the direction of descent in the case of multiple
193 /// level of hierarchy is described in
194 /// <a href="https://hal.archives-ouvertes.fr/lirmm-00796736">this paper</a>.
195 ///
196 /// The error threshold can be accessed by methods
197 /// HierarchicalIterative::errorThreshold. The maximal number of
198 /// iterations can be accessed by methods
199 /// HierarchicalIterative::maxIterations.
200 ///
201 /// \note Solving equations one after the other
202 ///
203 /// For some applications, it can be more efficient to solve a set of
204 /// equations one after the other. In other words, an equation is ignored
205 /// until the previous one is solved (norm below the threshold). To do so,
206 /// introduce the equations using method HierarchicalIterative::add with
207 /// increasing value of \c priority, and call method
208 /// \c HierarchicalIterative::solveLevelByLevel(true).
209 /// \note Lie group
210 ///
211 /// The unknowns \f$\mathbf{q}\f$ may take values in a more general set
212 /// than the configuration space of a robot. This set should be a
213 /// Cartesian product of Lie groups: hpp::pinocchio::LiegroupSpace.
214 ///
215 /// \note Saturation
216 ///
217 /// \copydoc saturate::Base
218 ///
219 /// \note Right hand side and comparison types
220 ///
221 /// Instead of \f$f(\mathbf{q}) = 0\f$, other constraints can be defined.
222 /// Several comparison types are available:
223 /// \li Equality: \f$f(\mathbf{q}) = rhs\f$, where \f$rhs\f$ is a
224 /// parameterizable right hand side,
225 /// \li EqualToZero: \f$f(\mathbf{q}) = 0\f$,
226 /// \li Superior: \f$f(\mathbf{q}) > 0\f$
227 /// \li Inferior: \f$f(\mathbf{q}) < 0\f$
228 /// If several constraint are of type equality, the right hand side of the
229 /// system of equations can be modified by methods
230 /// HierarchicalIterative::rightHandSideFromInput,
231 /// HierarchicalIterative::rightHandSide.
232 ///
233 /// \note Free variables
234 ///
235 /// Some variables can be locked, or computed explicitely. In this case,
236 /// the iterative resolution will only change the other variables called
237 /// free variables. \sa methods
238 /// \li \ref freeVariables (const Indices_t& indices) and
239 /// \li \ref freeVariables (const Indices_t& indices).
240 class HPP_CONSTRAINTS_DLLAPI HierarchicalIterative {
241 public:
242 typedef Eigen::RowBlockIndices Indices_t;
243 typedef lineSearch::FixedSequence DefaultLineSearch;
244
245 enum Status { ERROR_INCREASED, MAX_ITERATION_REACHED, INFEASIBLE, SUCCESS };
246 typedef shared_ptr<saturation::Base> Saturation_t;
247
248 HierarchicalIterative(const LiegroupSpacePtr_t& configSpace);
249
250 HierarchicalIterative(const HierarchicalIterative& other);
251
252 2076 virtual ~HierarchicalIterative() {}
253
254 /// \name Problem definition
255 /// \{
256
257 /// Get configuration space on which constraints are defined
258 const LiegroupSpacePtr_t& configSpace() const { return configSpace_; }
259 /// Check whether a numerical constraint has been added
260 /// \param numericalConstraint numerical constraint
261 /// \return true if numerical constraint is already in the solver
262 virtual bool contains(const ImplicitPtr_t& numericalConstraint) const;
263
264 /// Add an implicit constraint
265 ///
266 /// \param constraint implicit constraint
267 /// \param priority level of priority of the constraint: priority are
268 /// in decreasing order: 0 is the highest priority level,
269 virtual bool add(const ImplicitPtr_t& constraint,
270 const std::size_t& priority);
271
272 /// add constraints of another solver
273 /// \param other other solver
274 ///
275 /// Add constraints of other to this solver.
276 /// \note right hand side of other is not copied.
277 virtual void merge(const HierarchicalIterative& other);
278
279 /// Set the saturation function
280 18 void saturation(const Saturation_t& saturate) { saturate_ = saturate; }
281
282 /// Get the saturation function
283 const Saturation_t& saturation() const { return saturate_; }
284
285 /// \}
286
287 /// \name Problem resolution
288 /// \{
289
290 /// Solve the system of non linear equations
291 ///
292 /// \param arg initial guess,
293 /// \param ls line search method used.
294 ///
295 /// Use Newton Rhapson like iterative method until the error is below
296 /// the threshold, or until the maximal number of iterations has been
297 /// reached.
298 ///
299 /// \note Explicit constraints are expressed in their implicit
300 /// form: \f$\mathbf{q}_2 = f (\mathbf{q}_1)\f$ is replaced by
301 /// \f$\mathbf{q}_2 - f (\mathbf{q}_1) = 0\f$,
302 /// where \f$\mathbf{q}_1\f$ and \f$\mathbf{q}_2\f$ are vectors
303 /// composed of the components of \f$\mathbf{q}\f$.
304 template <typename LineSearchType>
305 Status solve(vectorOut_t arg, LineSearchType ls = LineSearchType()) const;
306
307 /// Solve the system of non linear equations
308 ///
309 /// \param arg initial guess,
310 ///
311 /// Use Newton Rhapson like iterative method until the error is below
312 /// the threshold, or until the maximal number of iterations has been
313 /// reached. Use the default line search method (fixed sequence of
314 /// \f$\alpha_i\f$).
315 ///
316 /// \note Explicit constraints are expressed in their implicit
317 /// form: \f$\mathbf{q}_2 = f (\mathbf{q}_1)\f$ is replaced by
318 /// \f$\mathbf{q}_2 - f (\mathbf{q}_1) = 0\f$,
319 /// where \f$\mathbf{q}_1\f$ and \f$\mathbf{q}_2\f$ are vectors
320 /// composed of the components of \f$\mathbf{q}\f$.
321 400 inline Status solve(vectorOut_t arg) const {
322
2/4
✓ Branch 2 taken 400 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
 400 return solve(arg, DefaultLineSearch());
323 }
324
325 /// Whether input vector satisfies the constraints of the solver
326 /// \param arg input vector
327 /// Compares to internal error threshold.
328 1803 bool isSatisfied(vectorIn_t arg) const {
329
1/2
✓ Branch 2 taken 1803 times.
✗ Branch 3 not taken.
 1803 computeValue<false>(arg);
330 1803 computeError();
331 1803 return squaredNorm_ < squaredErrorThreshold_;
332 }
333
334 /// Whether input vector satisfies the constraints of the solver
335 /// \param arg input vector
336 /// \param errorThreshold threshold to use instead of the value
337 /// stored in the solver.
338 bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const {
339 computeValue<false>(arg);
340 computeError();
341 return squaredNorm_ < errorThreshold * errorThreshold;
342 }
343
344 /// Whether a constraint is satisfied for an input vector
345 ///
346 /// \param constraint, the constraint in the solver,
347 /// \param arg the input vector
348 /// \retval error the error of the constraint.
349 /// \retval constraintFound whether the constraint belongs to the
350 /// solver,
351 /// \return true if constraint belongs to solver and error is below
352 /// the threshold, false otherwise.
353 bool isConstraintSatisfied(const ImplicitPtr_t& constraint, vectorIn_t arg,
354 vectorOut_t error, bool& constraintFound) const;
355
356 /// Returns the lowest singular value.
357 /// If the jacobian has maximum rank r, then it corresponds to r-th
358 /// greatest singular value. This value is zero when the jacobian is
359 /// singular.
360 const value_type& sigma() const { return sigma_; }
361
362 /// \}
363
364 /// \name Parameters
365 /// \{
366
367 /// Set free velocity variables
368 ///
369 /// The other variables will be left unchanged by the iterative
370 /// resolution.
371 ///
372 /// \param intervals set of index intervals
373 void freeVariables(const segments_t intervals) {
374 freeVariables_ = Indices_t();
375 for (std::size_t i = 0; i < intervals.size(); ++i)
376 freeVariables_.addRow(intervals[i].first, intervals[i].second);
377 freeVariables_.updateIndices<true, true, true>();
378 update();
379 }
380
381 /// Set free velocity variables
382 ///
383 /// The other variables will be left unchanged by the iterative
384 /// resolution.
385 29 void freeVariables(const Indices_t& indices) {
386 29 freeVariables_ = indices;
387 29 update();
388 29 }
389
390 /// Get free velocity variables
391 const Indices_t& freeVariables() const { return freeVariables_; }
392
393 /// Set maximal number of iterations
394 1022 void maxIterations(size_type iterations) { maxIterations_ = iterations; }
395 /// Get maximal number of iterations in config projector
396 3 size_type maxIterations() const { return maxIterations_; }
397
398 /// Set error threshold
399 1024 void errorThreshold(const value_type& threshold) {
400 1024 squaredErrorThreshold_ = threshold * threshold;
401 1024 }
402 /// Get error threshold
403 417 value_type errorThreshold() const { return sqrt(squaredErrorThreshold_); }
404 /// Get error threshold
405 48 value_type squaredErrorThreshold() const { return squaredErrorThreshold_; }
406
407 /// Get the inequality threshold
408 value_type inequalityThreshold() const { return inequalityThreshold_; }
409 /// set the inequality threshold
410 2 void inequalityThreshold(const value_type& it) { inequalityThreshold_ = it; }
411
412 13 void lastIsOptional(bool optional) { lastIsOptional_ = optional; }
413
414 bool lastIsOptional() const { return lastIsOptional_; }
415
416 // Set whether the constraints are solved level by level
417 //
418 // If true, the solver will consider only the higher level of
419 // priority that is not satisfied when computing the direction
420 // of descent.
421 void solveLevelByLevel(bool solveLevelByLevel) {
422 solveLevelByLevel_ = solveLevelByLevel;
423 }
424
425 // Get whether the constraints are solved level by level
426 bool solveLevelByLevel() const { return solveLevelByLevel_; }
427 /// \}
428
429 /// \name Stack
430 /// \{
431
432 /// Get set of constraints for a give priority level.
433 const ImplicitConstraintSet& constraints(const std::size_t priority) {
434 assert(priority < stacks_.size());
435 return stacks_[priority];
436 }
437
438 /// Get constraints (implicit and explicit)
439 21 const NumericalConstraints_t& constraints() const { return constraints_; }
440
441 9 std::size_t numberStacks() const { return stacks_.size(); }
442
443 2020 const size_type& dimension() const { return dimension_; }
444
445 /// Dimension of the problem after removing the rows of the jacobian
446 /// which do not influence the error (only zeros along those lines).
447 6 const size_type& reducedDimension() const { return reducedDimension_; }
448
449 /// Configuration parameters involved in the constraint resolution.
450 ArrayXb activeParameters() const;
451
452 /// Velocity parameters involved in the constraint resolution.
453 ArrayXb activeDerivativeParameters() const;
454
455 /// \}
456
457 /// Returns the squared norm of the error vector
458 520 value_type residualError() const { return squaredNorm_; }
459
460 /// Returns the error vector
461 void residualError(vectorOut_t error) const;
462
463 /// Inclusion of manifolds
464 ///
465 /// \param solver another solver
466 ///
467 /// This function returns true if the solution manifold of the solver
468 /// given as input is a submanifold of the solution manifold of this
469 /// solver. The function tests whether constraints of the input solver
470 /// are also in this solver.
471 bool definesSubmanifoldOf(const HierarchicalIterative& solver) const;
472
473 /// \name Right hand side accessors
474 /// \{
475
476 /// Compute right hand side of equality constraints from a configuration
477 /// \param config a configuration.
478 ///
479 /// for each constraint of type Equality, set right hand side as
480 /// \f$rhs = f(\mathbf{q})\f$.
481 /// \note Only parameterizable constraints (type Equality) are set
482 vector_t rightHandSideFromConfig(ConfigurationIn_t config);
483
484 /// Compute right hand side of a constraint from a configuration
485 /// \param constraint the constraint,
486 /// \param config a configuration.
487 ///
488 /// Set right hand side as \f$rhs = f(\mathbf{q})\f$.
489 /// \note Only parameterizable constraints (type Equality) are set
490 virtual bool rightHandSideFromConfig(const ImplicitPtr_t& constraint,
491 ConfigurationIn_t config);
492 /// Set right hand side of a constraints
493 /// \param constraint the constraint,
494 /// \param rhs right hand side.
495 /// \note Size of rhs should be equal to the total dimension of
496 /// parameterizable constraints (type Equality) .
497 virtual bool rightHandSide(const ImplicitPtr_t& constraint, vectorIn_t rhs);
498
499 /// Get right hand side of a constraints
500 virtual bool getRightHandSide(const ImplicitPtr_t& constraint,
501 vectorOut_t rhs) const;
502
503 /// Set the right hand side
504 /// \param rhs the right hand side
505 /// \note Size of rhs should be equal to the total dimension of
506 /// parameterizable constraints (type Equality).
507 virtual void rightHandSide(vectorIn_t rhs);
508
509 /// Set the right hand side at a given parameter.
510 /// \param s parameter passed to Implicit::rightHandSideAt
511 void rightHandSideAt(const value_type& s);
512
513 /// Get the right hand side
514 /// \return the right hand side
515 /// \note size of result is equal to total dimension of parameterizable
516 /// constraints (type Equality).
517 vector_t rightHandSide() const;
518
519 /// Get size of the right hand side
520 /// \return sum of dimensions of parameterizable constraints
521 /// (type Equality)
522 size_type rightHandSideSize() const;
523
524 /// \}
525
526 /// \name Access to internal data
527 /// You should know what you do when you call these functions
528 /// \{
529
530 /// Compute the value of each level, and the jacobian if ComputeJac is true.
531 template <bool ComputeJac>
532 void computeValue(vectorIn_t arg) const;
533 void computeSaturation(vectorIn_t arg) const;
534 void getValue(vectorOut_t v) const;
535 void getReducedJacobian(matrixOut_t J) const;
536 /// If lastIsOptional() is true, then the last level is ignored.
537 /// \warning computeValue must have been called first.
538 void computeError() const;
539
540 /// Accessor to the last step done
541 const vector_t& lastStep() const { return dq_; }
542
543 virtual bool integrate(vectorIn_t from, vectorIn_t velocity,
544 vectorOut_t result) const;
545 /// \}
546
547 virtual std::ostream& print(std::ostream& os) const;
548
549 protected:
550 typedef Eigen::JacobiSVD<matrix_t> SVD_t;
551
552 struct Data {
553 /// \cond
554 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
555 /// \endcond
556 LiegroupElement output, rightHandSide;
557 vector_t error;
558 matrix_t jacobian, reducedJ;
559
560 SVD_t svd;
561 matrix_t PK;
562
563 mutable size_type maxRank;
564
565 ComparisonTypes_t comparison;
566 std::vector<std::size_t> inequalityIndices;
567 Eigen::RowBlockIndices equalityIndices;
568 Eigen::MatrixBlocks<false, false> activeRowsOfJ;
569 };
570
571 /// Allocate datas and update sizes of the problem
572 /// Should be called whenever the stack is modified.
573 void update();
574
575 /// Compute which rows of the jacobian of stack_[iStack]
576 /// are not zero, using the activeDerivativeParameters of the functions.
577 /// The result is stored in datas_[i].activeRowsOfJ
578 virtual void computeActiveRowsOfJ(std::size_t iStack);
579
580 /// Compute a SVD decomposition of each level and find the best descent
581 /// direction at the first order.
582 /// Linearization of the system of equations
583 /// rhs - v_{i} = J (q_i) (dq_{i+1} - q_{i})
584 /// q_{i+1} - q_{i} = J(q_i)^{+} ( rhs - v_{i} )
585 /// dq = J(q_i)^{+} ( rhs - v_{i} )
586 /// \warning computeValue<true> must have been called first.
587 void computeDescentDirection() const;
588 void expandDqSmall() const;
589 void saturate(vectorOut_t arg) const;
590
591 value_type squaredErrorThreshold_, inequalityThreshold_;
592 size_type maxIterations_;
593
594 std::vector<ImplicitConstraintSet> stacks_;
595 LiegroupSpacePtr_t configSpace_;
596 size_type dimension_, reducedDimension_;
597 bool lastIsOptional_;
598 bool solveLevelByLevel_;
599 /// Unknown of the set of implicit constraints
600 Indices_t freeVariables_;
601 Saturation_t saturate_;
602 /// Members moved from core::ConfigProjector
603 NumericalConstraints_t constraints_;
604 /// Value rank of constraint in its priority level
605 std::map<DifferentiableFunctionPtr_t, size_type> iq_;
606 /// Derivative rank of constraint in its priority level
607 std::map<DifferentiableFunctionPtr_t, size_type> iv_;
608 /// Priority level of constraint
609 std::map<DifferentiableFunctionPtr_t, std::size_t> priority_;
610
611 /// The smallest non-zero singular value
612 mutable value_type sigma_;
613
614 mutable vector_t dq_, dqSmall_;
615 mutable matrix_t reducedJ_;
616 mutable Eigen::VectorXi saturation_, reducedSaturation_;
617 mutable Configuration_t qSat_;
618 mutable ArrayXb tmpSat_;
619 mutable value_type squaredNorm_;
620 mutable std::vector<Data> datas_;
621 mutable SVD_t svd_;
622 mutable vector_t OM_;
623 mutable vector_t OP_;
624
625 friend struct lineSearch::Backtracking;
626
627 protected:
628 ✗ HierarchicalIterative() {}
629
630 private:
631 4 HPP_SERIALIZABLE_SPLIT();
632 }; // class HierarchicalIterative
633 /// \}
634 3 inline std::ostream& operator<<(std::ostream& os,
635 const HierarchicalIterative& hs) {
636 3 return hs.print(os);
637 }
638 } // namespace solver
639 } // namespace constraints
640 } // namespace hpp
641
642 #endif // HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
643