| Directory: | ./ |
|---|---|
| File: | include/hpp/constraints/implicit.hh |
| Date: | 2025-05-05 12:19:30 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 7 | 9 | 77.8% |
| Branches: | 5 | 10 | 50.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright (c) 2015 - 2018 CNRS | ||
| 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr), Florent Lamiraux | ||
| 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_IMPLICIT_HH | ||
| 30 | #define HPP_CONSTRAINTS_IMPLICIT_HH | ||
| 31 | |||
| 32 | #include <hpp/constraints/comparison-types.hh> | ||
| 33 | #include <hpp/constraints/config.hh> | ||
| 34 | #include <hpp/constraints/fwd.hh> | ||
| 35 | #include <hpp/constraints/matrix-view.hh> | ||
| 36 | #include <hpp/pinocchio/liegroup-element.hh> | ||
| 37 | #include <hpp/util/serialization-fwd.hh> | ||
| 38 | |||
| 39 | namespace hpp { | ||
| 40 | namespace constraints { | ||
| 41 | /** | ||
| 42 | \addtogroup constraints | ||
| 43 | \{ | ||
| 44 | */ | ||
| 45 | /** | ||
| 46 | This class represents a parameterizable numerical constraint that | ||
| 47 | compares the output of a function \f$h\f$ to a right hand side Lie | ||
| 48 | group element. | ||
| 49 | |||
| 50 | \par Definition | ||
| 51 | |||
| 52 | \li The function \f$h\f$ takes input in a configuration space | ||
| 53 | \f$\mathcal{C}\f$ and output in a Lie group \f$\mathbf{L}\f$, | ||
| 54 | \li the dimensions of \f$\mathbf{L}\f$ and of its tangent space are | ||
| 55 | respectively \f$(n_q,n_v)\f$. | ||
| 56 | \li The comparison is represented by a vector \f$\mathbf{c}\f$ of | ||
| 57 | dimension \f$n_v\f$ with values in | ||
| 58 | enum hpp::constraints::ComparisonType = { | ||
| 59 | \f$\mathbf{Equality}\f$, \f$\mathbf{EqualToZero}\f$, | ||
| 60 | \f$\mathbf{Inferior}\f$, \f$\mathbf{Superior}\f$ }. | ||
| 61 | \li The right hand side is Lie group element of dimension \f$n_q\f$. | ||
| 62 | |||
| 63 | \par Error | ||
| 64 | |||
| 65 | A configuration \f$\mathbf{q}\f$ is said to satisfy the constraint for | ||
| 66 | a given right hand side if and only if the error \f$e\f$ as computed | ||
| 67 | below is | ||
| 68 | smaller in norm than a given threshold. | ||
| 69 | |||
| 70 | Let | ||
| 71 | \f[ | ||
| 72 | \Delta = h (\mathbf{q}) - rhs \in \mathbf{R}^{n_v}, | ||
| 73 | \f] | ||
| 74 | for each component \f$i\in\{0,\cdots,n_v-1\}\f$, | ||
| 75 | \li if \f$c_i\f$ is \f$\mathbf{Inferior}\f$, | ||
| 76 | \f$e_i = \max (0,\Delta_i)\f$, | ||
| 77 | \li if \f$c_i\f$ is \f$\mathbf{Superior}\f$, | ||
| 78 | \f$e_i = \min (0,\Delta_i)\f$, | ||
| 79 | \li if \f$c_i\f$ is \f$\mathbf{Equality}\f$, | ||
| 80 | \f$e_i = \Delta_i\f$, | ||
| 81 | \li if \f$c_i\f$ is \f$\mathbf{EqualToZero}\f$, \f$e_i = \Delta_i\f$. | ||
| 82 | |||
| 83 | \par Mask | ||
| 84 | |||
| 85 | A mask is a vector of Boolean values of size \f$n_v\f$. Values set to | ||
| 86 | false means that the corresponding component of the error defined above | ||
| 87 | is not taken into account to determine whether the constraint is | ||
| 88 | satisfied. The active rows of the constraint may be accessed via | ||
| 89 | method activeRows. | ||
| 90 | |||
| 91 | \par Parameterizable right hand side | ||
| 92 | |||
| 93 | Lines with \f$\mathbf{Equality}\f$ comparator in the above definition | ||
| 94 | of the error need a parameter, while lines with comparators | ||
| 95 | \f$\mathbf{Inferior}\f$, \f$\mathbf{Superior}\f$, or | ||
| 96 | \f$\mathbf{EqualToZero}\f$ do not. As a consequence, the right hand | ||
| 97 | side of the constraint is defined by a vector \f$\lambda\f$ | ||
| 98 | of parameters of size the number of \f$\mathbf{Equality}\f$ occurences | ||
| 99 | in vector \f$\mathbf{c}\f$. The right hand side is then defined as in | ||
| 100 | the following example: | ||
| 101 | \f[ | ||
| 102 | rhs = \exp\left(\begin{array}{c}\lambda_1 \\ 0 \\ 0 \\ \lambda_2 \\ | ||
| 103 | \vdots \end{array}\right) | ||
| 104 | \ \ \ \ \mathbf{c} = \left(\begin{array}{c}\mathbf{Equality} \\ | ||
| 105 | \mathbf{EqualToZero} \\ \mathbf{Inferior} \\ \mathbf{Equality} \\ | ||
| 106 | \vdots \end{array}\right) | ||
| 107 | \f] | ||
| 108 | To retrieve the size of vector \f$\lambda\f$, call method | ||
| 109 | Implicit::parameterSize (). To set and get the right hand side value, | ||
| 110 | use method Implicit::rightHandSide. | ||
| 111 | |||
| 112 | \par Time varying right hand side | ||
| 113 | |||
| 114 | The right hand side of the constraint may depend on time, for instance | ||
| 115 | if the constraint is associated to a trajectory following task. | ||
| 116 | In this case, the right hand side is a function from \f$\mathbf{R}\f$ | ||
| 117 | to \f$\mathbf{L}\f$. | ||
| 118 | */ | ||
| 119 | class HPP_CONSTRAINTS_DLLAPI Implicit { | ||
| 120 | public: | ||
| 121 | /// Operator equality | ||
| 122 | bool operator==(const Implicit& other) const; | ||
| 123 | /// Copy object and return shared pointer to copy | ||
| 124 | virtual ImplicitPtr_t copy() const; | ||
| 125 | /// Create a shared pointer to a new instance. | ||
| 126 | /// \sa constructors | ||
| 127 | static ImplicitPtr_t create(const DifferentiableFunctionPtr_t& func, | ||
| 128 | ComparisonTypes_t comp, | ||
| 129 | std::vector<bool> mask = std::vector<bool>()); | ||
| 130 | |||
| 131 | /// Create a copy and return shared pointer | ||
| 132 | static ImplicitPtr_t createCopy(const ImplicitPtr_t& other); | ||
| 133 | |||
| 134 | 8592 | virtual ~Implicit() {}; | |
| 135 | |||
| 136 | /// \name Right hand side | ||
| 137 | /// \{ | ||
| 138 | |||
| 139 | /// Computes the right hand side from a configuration | ||
| 140 | /// | ||
| 141 | /// in such a way that the configuration satisfies the numerical | ||
| 142 | /// constraints | ||
| 143 | /// \param config the input configuration. | ||
| 144 | /// \retval rhs right hand side as a Lie group element. | ||
| 145 | /// \precond rhs should be initialized with the right LiegroupSpace | ||
| 146 | /// (ie the output space of the function). | ||
| 147 | /// | ||
| 148 | /// \warning Only values of the right hand side corresponding to | ||
| 149 | /// \link Equality "equality constraints" \endlink are set. As a | ||
| 150 | /// result, the input configuration may not satisfy the other | ||
| 151 | /// constraints. | ||
| 152 | void rightHandSideFromConfig(ConfigurationIn_t config, | ||
| 153 | LiegroupElementRef rhs); | ||
| 154 | |||
| 155 | /// Check right hand side with regard to comparison types | ||
| 156 | /// \param rhs right hand side, | ||
| 157 | /// \return true if right hand side is correct | ||
| 158 | /// | ||
| 159 | /// This method checks that elements of the right hand side log | ||
| 160 | /// corresponding to comparison types different from Equality are equal | ||
| 161 | /// to 0. | ||
| 162 | bool checkRightHandSide(LiegroupElementConstRef rhs) const; | ||
| 163 | |||
| 164 | /// Get size of parameter defining the right hand side of the constraint | ||
| 165 | /// | ||
| 166 | /// See class documentation for details. | ||
| 167 | size_type parameterSize() const; | ||
| 168 | |||
| 169 | /// Get size of right hand side of the constraint | ||
| 170 | /// | ||
| 171 | /// This is the dimension (nq) of the output space of the function | ||
| 172 | size_type rightHandSideSize() const; | ||
| 173 | |||
| 174 | /// Set time-varying right hand side | ||
| 175 | /// \param rhsF Mapping from \f$\mathbf{R}\f$ to output space | ||
| 176 | /// \f$\mathbf{L}\f$ of \f$h\f$ defining the variation along time | ||
| 177 | /// of the right hand side. | ||
| 178 | void rightHandSideFunction(const DifferentiableFunctionPtr_t& rhsF); | ||
| 179 | |||
| 180 | /// Get time-varying right hand side | ||
| 181 | /// \return the mapping from \f$\mathbf{R}\f$ to output space | ||
| 182 | /// \f$\mathbf{L}\f$ of \f$h\f$ defining the variation along time | ||
| 183 | /// of the right hand side. | ||
| 184 | ✗ | const DifferentiableFunctionPtr_t& rightHandSideFunction() const { | |
| 185 | ✗ | return rhsFunction_; | |
| 186 | } | ||
| 187 | |||
| 188 | /// Evaluate and set right hand side at given time | ||
| 189 | /// \param s time | ||
| 190 | vectorIn_t rightHandSideAt(const value_type& s); | ||
| 191 | |||
| 192 | /// \} | ||
| 193 | |||
| 194 | /// Return the ComparisonType | ||
| 195 | const ComparisonTypes_t& comparisonType() const; | ||
| 196 | |||
| 197 | /// Set the comparison type | ||
| 198 | void comparisonType(const ComparisonTypes_t& comp); | ||
| 199 | |||
| 200 | /// Return the active rows taken into account | ||
| 201 | /// to determine whether the constraint is satisfied | ||
| 202 | 1095 | const segments_t& activeRows() const { return activeRows_; } | |
| 203 | |||
| 204 | /// Check if all rows are active (no inactive rows) | ||
| 205 | 5 | bool checkAllRowsActive() const { return inactiveRows_.nbRows() == 0; } | |
| 206 | |||
| 207 | /// Get indices of constraint coordinates that are equality | ||
| 208 | const Eigen::RowBlockIndices& equalityIndices() const { | ||
| 209 | return equalityIndices_; | ||
| 210 | } | ||
| 211 | |||
| 212 | /// Set inactive components of error to 0 | ||
| 213 | /// \retval error error vector computed by substracting a right hand | ||
| 214 | /// side to the output to the value of the function. | ||
| 215 | void setInactiveRowsToZero(vectorOut_t error) const; | ||
| 216 | |||
| 217 | /// Return a reference to function \f$h\f$. | ||
| 218 | 95168 | DifferentiableFunction& function() const { return *function_; } | |
| 219 | |||
| 220 | /// Return a reference to function \f$h\f$. | ||
| 221 | 28173 | const DifferentiableFunctionPtr_t& functionPtr() const { return function_; } | |
| 222 | |||
| 223 | /// Get pair of joints whose relative pose is fully constrained | ||
| 224 | /// | ||
| 225 | /// If the function value depends on the relative pose between j1 and j2 | ||
| 226 | /// and if the relative pose between j1 and j2 is fully constrained | ||
| 227 | /// because of Implicit constraint (relative transformation may still | ||
| 228 | /// differ from one path to another), return these two joints. | ||
| 229 | /// \param robot the device that this constraint is applied to | ||
| 230 | /// \return the pair of joints constrained, in order of increasing | ||
| 231 | /// joint index, or a pair of empty shared pointers | ||
| 232 | /// \note absolute pose is considered relative pose with respect to | ||
| 233 | /// "universe". "universe" is returned as a nullpointer | ||
| 234 | /// as the first element of the pair, if applicable. | ||
| 235 | virtual std::pair<JointConstPtr_t, JointConstPtr_t> | ||
| 236 | doesConstrainRelPoseBetween(DeviceConstPtr_t robot) const; | ||
| 237 | |||
| 238 | protected: | ||
| 239 | /// Constructor | ||
| 240 | /// \param function the differentiable function | ||
| 241 | /// \param comp vector of comparison \f$\mathbf{c}\f$. | ||
| 242 | /// \param mask mask defining which components of the error are | ||
| 243 | /// taken into account to determine whether the constraint | ||
| 244 | /// is satisfied. | ||
| 245 | /// \precond sizes of comp and of mask should be equal to size of | ||
| 246 | /// tangent space to | ||
| 247 | /// function output space \f$\mathbf{L}\f$. | ||
| 248 | Implicit(const DifferentiableFunctionPtr_t& function, ComparisonTypes_t comp, | ||
| 249 | std::vector<bool> mask); | ||
| 250 | |||
| 251 | /// Copy constructor | ||
| 252 | Implicit(const Implicit& other); | ||
| 253 | |||
| 254 | /// Test equality with other instance | ||
| 255 | /// \param other object to copy | ||
| 256 | /// \param swapAndTest whether we should also check other == this | ||
| 257 | virtual bool isEqual(const Implicit& other, bool swapAndTest) const; | ||
| 258 | |||
| 259 | // Store weak pointer to itself | ||
| 260 | 1147 | void init(const ImplicitWkPtr_t& weak) { weak_ = weak; } | |
| 261 | |||
| 262 | friend class ImplicitConstraintSet; | ||
| 263 | |||
| 264 | private: | ||
| 265 | void computeIndices(); | ||
| 266 | void computeActiveRows(); | ||
| 267 | ComparisonTypes_t comparison_; | ||
| 268 | vector_t rhs_; | ||
| 269 | size_type parameterSize_; | ||
| 270 | DifferentiableFunctionPtr_t function_; | ||
| 271 | DifferentiableFunctionPtr_t rhsFunction_; | ||
| 272 | std::vector<bool> mask_; | ||
| 273 | segments_t activeRows_; | ||
| 274 | Eigen::RowBlockIndices inactiveRows_; | ||
| 275 | std::vector<std::size_t> inequalityIndices_; | ||
| 276 | Eigen::RowBlockIndices equalityIndices_; | ||
| 277 | ImplicitWkPtr_t weak_; | ||
| 278 | // To avoid dynamic memory allocation | ||
| 279 | mutable LiegroupElement output_; | ||
| 280 | mutable vector_t logOutput_; | ||
| 281 | |||
| 282 | protected: | ||
| 283 |
5/10✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
|
3 | Implicit() {} |
| 284 | |||
| 285 | private: | ||
| 286 | HPP_SERIALIZABLE(); | ||
| 287 | }; // class Implicit | ||
| 288 | /// \} | ||
| 289 | } // namespace constraints | ||
| 290 | } // namespace hpp | ||
| 291 | |||
| 292 | #endif // HPP_CONSTRAINTS_IMPLICIT_HH | ||
| 293 |