| Directory: | ./ |
|---|---|
| File: | include/hpp/constraints/solver/by-substitution.hh |
| Date: | 2025-05-05 12:19:30 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 33 | 34 | 97.1% |
| Branches: | 46 | 99 | 46.5% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright (c) 2017, 2018 CNRS | ||
| 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_BY_SUBSTITUTION_HH | ||
| 30 | #define HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH | ||
| 31 | |||
| 32 | #include <hpp/constraints/config.hh> | ||
| 33 | #include <hpp/constraints/explicit-constraint-set.hh> | ||
| 34 | #include <hpp/constraints/fwd.hh> | ||
| 35 | #include <hpp/constraints/locked-joint.hh> | ||
| 36 | #include <hpp/constraints/solver/hierarchical-iterative.hh> | ||
| 37 | #include <vector> | ||
| 38 | |||
| 39 | namespace hpp { | ||
| 40 | namespace constraints { | ||
| 41 | namespace solver { | ||
| 42 | /// \addtogroup solvers | ||
| 43 | /// \{ | ||
| 44 | |||
| 45 | /// Solve a non-linear system equations with explicit and implicit constraints | ||
| 46 | /// | ||
| 47 | /// This solver is defined in paper | ||
| 48 | /// https://hal.archives-ouvertes.fr/hal-01804774/file/paper.pdf. We | ||
| 49 | /// give here only a brief description | ||
| 50 | /// | ||
| 51 | /// The unknows (denoted by \f$\mathbf{q}\f$) of the system of equations | ||
| 52 | /// is a Lie group. It is usually a robot configuration space or | ||
| 53 | /// the Cartesian product of robot configuration spaces. | ||
| 54 | /// | ||
| 55 | /// The solver stores a set of implicit numerical constraints: | ||
| 56 | /// \f$g_1 (\mathbf{q}) = 0, g_2 (\mathbf{q}) = 0, \cdots\f$. These implicit | ||
| 57 | /// constraints are added using method HierarchicalIterative::add. | ||
| 58 | /// | ||
| 59 | /// The solver also stores explicit numerical constraints (constraints where | ||
| 60 | /// some configuration variables depend on others) in an instance of class | ||
| 61 | /// ExplicitConstraintSet. This instance is accessible via method | ||
| 62 | /// BySubstitution::explicitConstraintSet. | ||
| 63 | /// | ||
| 64 | /// When an explicit constraint is added using method | ||
| 65 | /// ExplicitConstraintSet::add, this method checks that the explicit | ||
| 66 | /// constraint is compatible with the previously added ones. If so, | ||
| 67 | /// the constraint is stored in the explicit constraint set. Otherwise, | ||
| 68 | /// it has to be added as an implicit constraint. | ||
| 69 | /// | ||
| 70 | /// See Section III of the above mentioned paper for the description of | ||
| 71 | /// the constraint resolution. | ||
| 72 | class HPP_CONSTRAINTS_DLLAPI BySubstitution | ||
| 73 | : public solver::HierarchicalIterative { | ||
| 74 | public: | ||
| 75 | BySubstitution(const LiegroupSpacePtr_t& configSpace); | ||
| 76 | BySubstitution(const BySubstitution& other); | ||
| 77 | |||
| 78 | 2054 | virtual ~BySubstitution() {} | |
| 79 | |||
| 80 | /// \copydoc HierarchicalIterative::add | ||
| 81 | /// | ||
| 82 | /// If the constraint is explicit and compatible with previously | ||
| 83 | /// inserted constraints, it is added as explicit. Otherwise, it is | ||
| 84 | /// added as implicit. | ||
| 85 | bool add(const ImplicitPtr_t& numericalConstraint, | ||
| 86 |
18/35✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 1 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 1 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 1 times.
✗ Branch 51 not taken.
✓ Branch 53 taken 1000 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 1000 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 1 times.
✗ Branch 60 not taken.
✓ Branch 68 taken 1 times.
✗ Branch 69 not taken.
✓ Branch 72 taken 1 times.
✗ Branch 73 not taken.
✓ Branch 76 taken 1 times.
✗ Branch 77 not taken.
|
1062 | const std::size_t& priority = 0); |
| 87 | |||
| 88 | /// Get the numerical constraints implicit and explicit | ||
| 89 | 1 | const NumericalConstraints_t& numericalConstraints() const { | |
| 90 | 1 | return constraints_; | |
| 91 | } | ||
| 92 | |||
| 93 | /// Get explicit constraint set | ||
| 94 | 39 | ExplicitConstraintSet& explicitConstraintSet() { return explicit_; } | |
| 95 | |||
| 96 | /// Set explicit constraint set | ||
| 97 | const ExplicitConstraintSet& explicitConstraintSet() const { | ||
| 98 | return explicit_; | ||
| 99 | } | ||
| 100 | |||
| 101 | /// Return the number of free variables | ||
| 102 | size_type numberFreeVariables() const { | ||
| 103 | return explicitConstraintSet().notOutDers().nbIndices(); | ||
| 104 | } | ||
| 105 | |||
| 106 | /// Should be called whenever explicit solver is modified | ||
| 107 | void explicitConstraintSetHasChanged(); | ||
| 108 | |||
| 109 | template <typename LineSearchType> | ||
| 110 | 2894 | Status solve(vectorOut_t arg, LineSearchType ls = LineSearchType()) const { | |
| 111 |
2/4✓ Branch 2 taken 1647 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
|
2894 | return solve<LineSearchType>(arg, false, ls); |
| 112 | } | ||
| 113 | |||
| 114 | template <typename LineSearchType> | ||
| 115 | 2894 | Status solve(vectorOut_t arg, bool optimize, | |
| 116 | LineSearchType ls = LineSearchType()) const { | ||
| 117 | // TODO when there are only locked joint explicit constraints, | ||
| 118 | // there is no need for this intricated loop. | ||
| 119 | // if (explicit_.isConstant()) { | ||
| 120 | // explicit_.solve(arg); | ||
| 121 | // iterative_.solve(arg, ls); | ||
| 122 | // } else { | ||
| 123 |
2/4✓ Branch 2 taken 1647 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
|
2894 | return impl_solve(arg, optimize, ls); |
| 124 | // } | ||
| 125 | } | ||
| 126 | |||
| 127 | /// Project velocity on constraint tangent space in "from" | ||
| 128 | /// | ||
| 129 | /// \param from configuration, | ||
| 130 | /// \param velocity velocity to project | ||
| 131 | /// | ||
| 132 | /// \f[ | ||
| 133 | /// \textbf{q}_{res} = \left(I_n - | ||
| 134 | /// J^{+}J(\textbf{q}_{from})\right) (\textbf{v}) | ||
| 135 | /// \f] | ||
| 136 | void projectVectorOnKernel(ConfigurationIn_t from, vectorIn_t velocity, | ||
| 137 | vectorOut_t result) const; | ||
| 138 | |||
| 139 | /// Project configuration "to" on constraint tangent space in "from" | ||
| 140 | /// | ||
| 141 | /// \param from configuration, | ||
| 142 | /// \param to configuration to project | ||
| 143 | /// | ||
| 144 | /// \f[ | ||
| 145 | /// \textbf{q}_{res} = \textbf{q}_{from} + \left(I_n - | ||
| 146 | /// J^{+}J(\textbf{q}_{from})\right) (\textbf{q}_{to} - | ||
| 147 | /// \textbf{q}_{from}) | ||
| 148 | /// \f] | ||
| 149 | virtual void projectOnKernel(ConfigurationIn_t from, ConfigurationIn_t to, | ||
| 150 | ConfigurationOut_t result); | ||
| 151 | |||
| 152 | 1400 | inline Status solve(vectorOut_t arg) const { | |
| 153 |
2/4✓ Branch 2 taken 1400 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1400 times.
✗ Branch 6 not taken.
|
1400 | return solve(arg, DefaultLineSearch()); |
| 154 | } | ||
| 155 | |||
| 156 | /// \name Right hand side accessors | ||
| 157 | /// \{ | ||
| 158 | |||
| 159 | /// \copydoc HierarchicalIterative::rightHandSideFromConfig(ConfigurationIn_t) | ||
| 160 | vector_t rightHandSideFromConfig(ConfigurationIn_t config); | ||
| 161 | |||
| 162 | /// \copydoc HierarchicalIterative::rightHandSideFromConfig(const | ||
| 163 | /// ImplicitPtr_t&, ConfigurationIn_t) | ||
| 164 | bool rightHandSideFromConfig(const ImplicitPtr_t& constraint, | ||
| 165 | ConfigurationIn_t config); | ||
| 166 | |||
| 167 | /// \copydoc HierarchicalIterative::rightHandSide(const ImplicitPtr_t&, | ||
| 168 | /// vectorIn_t) | ||
| 169 | bool rightHandSide(const ImplicitPtr_t& constraint, vectorIn_t rhs); | ||
| 170 | |||
| 171 | /// \copydoc HierarchicalIterative::getRightHandSide(const ImplicitPtr_t&, | ||
| 172 | /// vectorOut_t) | ||
| 173 | bool getRightHandSide(const ImplicitPtr_t& constraint, vectorOut_t rhs) const; | ||
| 174 | |||
| 175 | /// \copydoc HierarchicalIterative::rightHandSide(vectorIn_t) | ||
| 176 | void rightHandSide(vectorIn_t rhs); | ||
| 177 | |||
| 178 | /// Get the right hand side | ||
| 179 | /// \return the right hand side | ||
| 180 | /// \note size of result is equal to total dimension of parameterizable | ||
| 181 | /// constraints (type Equality). | ||
| 182 | vector_t rightHandSide() const; | ||
| 183 | |||
| 184 | /// \copydoc HierarchicalIterative::rightHandSideSize() | ||
| 185 | size_type rightHandSideSize() const; | ||
| 186 | |||
| 187 | /// \} | ||
| 188 | |||
| 189 | /// Return the size of the error as computed by isSatisfied | ||
| 190 | /// | ||
| 191 | /// Size of error is different from size of right hand side | ||
| 192 | /// (rightHandSideSize) if the solver contains constraints with | ||
| 193 | /// values in a Lie group. | ||
| 194 | 1 | size_type errorSize() const { | |
| 195 | 1 | return dimension() + explicit_.outDers().nbIndices(); | |
| 196 | } | ||
| 197 | |||
| 198 | /// Whether input vector satisfies the constraints of the solver | ||
| 199 | /// \param arg input vector. | ||
| 200 | /// Compares to internal error threshold. | ||
| 201 | 5 | bool isSatisfied(vectorIn_t arg) const { | |
| 202 |
3/8✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
15 | return solver::HierarchicalIterative::isSatisfied(arg) && |
| 203 |
5/12✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
15 | explicit_.isSatisfied(arg); |
| 204 | } | ||
| 205 | |||
| 206 | /// Whether input vector satisfies the constraints of the solver | ||
| 207 | /// \param arg input vector | ||
| 208 | /// \param errorThreshold threshold to use instead of the value | ||
| 209 | /// stored in the solver. | ||
| 210 | bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const { | ||
| 211 | return solver::HierarchicalIterative::isSatisfied(arg, errorThreshold) && | ||
| 212 | explicit_.isSatisfied(arg, errorThreshold); | ||
| 213 | } | ||
| 214 | /// Whether input vector satisfies the constraints of the solver | ||
| 215 | /// \param arg input vector | ||
| 216 | /// \retval error the constraint errors dispatched in a vector, | ||
| 217 | /// the head of the vector corresponds to implicit constraints, | ||
| 218 | /// the tail of the vector corresponds to explicit constraints. | ||
| 219 | 1006 | bool isSatisfied(vectorIn_t arg, vectorOut_t error) const { | |
| 220 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 1006 times.
|
1006 | assert(error.size() == dimension() + explicit_.errorSize()); |
| 221 |
1/2✓ Branch 2 taken 1006 times.
✗ Branch 3 not taken.
|
1006 | bool iterative = solver::HierarchicalIterative::isSatisfied(arg); |
| 222 |
2/4✓ Branch 3 taken 1006 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1006 times.
✗ Branch 7 not taken.
|
1006 | residualError(error.head(dimension())); |
| 223 | bool _explicit = | ||
| 224 |
3/6✓ Branch 3 taken 1006 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1006 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1006 times.
✗ Branch 10 not taken.
|
1006 | explicit_.isSatisfied(arg, error.tail(explicit_.errorSize())); |
| 225 |
3/4✓ Branch 0 taken 1004 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1004 times.
✗ Branch 3 not taken.
|
1006 | return iterative && _explicit; |
| 226 | } | ||
| 227 | |||
| 228 | /// Whether a constraint is satisfied for an input vector | ||
| 229 | /// | ||
| 230 | /// \param constraint, the constraint in the solver, | ||
| 231 | /// \param arg the input vector | ||
| 232 | /// \retval error the error of the constraint. | ||
| 233 | /// \retval constraintFound whether the constraint belongs to the | ||
| 234 | /// solver, | ||
| 235 | /// \return true if constraint belongs to solver and error is below | ||
| 236 | /// the threshold, false otherwise. | ||
| 237 | bool isConstraintSatisfied(const ImplicitPtr_t& constraint, vectorIn_t arg, | ||
| 238 | vectorOut_t error, bool& constraintFound) const; | ||
| 239 | |||
| 240 | template <typename LineSearchType> | ||
| 241 | bool oneStep(vectorOut_t arg, LineSearchType& lineSearch) const { | ||
| 242 | computeValue<true>(arg); | ||
| 243 | updateJacobian(arg); | ||
| 244 | computeDescentDirection(); | ||
| 245 | lineSearch(*this, arg, dq_); | ||
| 246 | explicit_.solve(arg); | ||
| 247 | return solver::HierarchicalIterative::isSatisfied(arg); | ||
| 248 | } | ||
| 249 | |||
| 250 | /// Computes the jacobian of the explicit functions and | ||
| 251 | /// updates the jacobian of the problem using the chain rule. | ||
| 252 | void updateJacobian(vectorIn_t arg) const; | ||
| 253 | |||
| 254 | /// Set error threshold | ||
| 255 | 1015 | void errorThreshold(const value_type& threshold) { | |
| 256 | 1015 | solver::HierarchicalIterative::errorThreshold(threshold); | |
| 257 | 1015 | explicit_.errorThreshold(threshold); | |
| 258 | 1015 | } | |
| 259 | /// Get error threshold | ||
| 260 | 400 | value_type errorThreshold() const { | |
| 261 | 400 | return solver::HierarchicalIterative::errorThreshold(); | |
| 262 | } | ||
| 263 | |||
| 264 | /// Return the indices in the input vector which are solved implicitely. | ||
| 265 | /// | ||
| 266 | /// The other dof which are modified are solved explicitely. | ||
| 267 | segments_t implicitDof() const; | ||
| 268 | |||
| 269 | virtual std::ostream& print(std::ostream& os) const; | ||
| 270 | |||
| 271 | 13390 | bool integrate(vectorIn_t from, vectorIn_t velocity, | |
| 272 | vectorOut_t result) const { | ||
| 273 |
3/6✓ Branch 2 taken 13390 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13390 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13390 times.
✗ Branch 9 not taken.
|
13390 | bool res = solver::HierarchicalIterative::integrate(from, velocity, result); |
| 274 |
1/2✓ Branch 2 taken 13390 times.
✗ Branch 3 not taken.
|
13390 | explicit_.solve(result); |
| 275 | 13390 | return res; | |
| 276 | } | ||
| 277 | |||
| 278 | protected: | ||
| 279 | void computeActiveRowsOfJ(std::size_t iStack); | ||
| 280 | |||
| 281 | private: | ||
| 282 | typedef solver::HierarchicalIterative parent_t; | ||
| 283 | |||
| 284 | template <typename LineSearchType> | ||
| 285 | Status impl_solve(vectorOut_t arg, bool optimize, LineSearchType ls) const; | ||
| 286 | |||
| 287 | ExplicitConstraintSet explicit_; | ||
| 288 | mutable matrix_t Je_, JeExpanded_; | ||
| 289 | |||
| 290 | ✗ | BySubstitution() {} | |
| 291 | 4 | HPP_SERIALIZABLE_SPLIT(); | |
| 292 | }; // class BySubstitution | ||
| 293 | /// \} | ||
| 294 | |||
| 295 | } // namespace solver | ||
| 296 | } // namespace constraints | ||
| 297 | } // namespace hpp | ||
| 298 | |||
| 299 | #endif // HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH | ||
| 300 |