| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/liegroup/vector-space.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 98 | 122 | 80.3% |
| Branches: | 45 | 134 | 33.6% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2016-2020 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__ | ||
| 6 | #define __pinocchio_multibody_liegroup_vector_space_operation_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/multibody/liegroup/liegroup-base.hpp" | ||
| 9 | |||
| 10 | #include <stdexcept> | ||
| 11 | #include <boost/integer/static_min_max.hpp> | ||
| 12 | |||
| 13 | namespace pinocchio | ||
| 14 | { | ||
| 15 | template<int Dim, typename Scalar, int Options = context::Options> | ||
| 16 | struct VectorSpaceOperationTpl; | ||
| 17 | |||
| 18 | template<int Dim, typename _Scalar, int _Options> | ||
| 19 | struct traits<VectorSpaceOperationTpl<Dim, _Scalar, _Options>> | ||
| 20 | { | ||
| 21 | typedef _Scalar Scalar; | ||
| 22 | enum | ||
| 23 | { | ||
| 24 | Options = _Options, | ||
| 25 | NQ = Dim, | ||
| 26 | NV = Dim | ||
| 27 | }; | ||
| 28 | }; | ||
| 29 | |||
| 30 | template<int Dim, typename _Scalar, int _Options> | ||
| 31 | struct VectorSpaceOperationTpl | ||
| 32 | : public LieGroupBase<VectorSpaceOperationTpl<Dim, _Scalar, _Options>> | ||
| 33 | { | ||
| 34 | PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(VectorSpaceOperationTpl); | ||
| 35 | |||
| 36 | /// Constructor | ||
| 37 | /// \param size size of the vector space: should be the equal to template | ||
| 38 | /// argument for static sized vector-spaces. | ||
| 39 | 701896 | VectorSpaceOperationTpl(int size = boost::static_signed_max<0, Dim>::value) | |
| 40 | 701896 | : size_(size) | |
| 41 | { | ||
| 42 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
|
10 | assert(size_.value() >= 0); |
| 43 | 701896 | } | |
| 44 | |||
| 45 | /// Constructor | ||
| 46 | /// \param other other VectorSpaceOperationTpl from which to retrieve size | ||
| 47 | 1102 | VectorSpaceOperationTpl(const VectorSpaceOperationTpl & other) | |
| 48 | : Base() | ||
| 49 | 1102 | , size_(other.size_.value()) | |
| 50 | { | ||
| 51 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 10 times.
|
20 | assert(size_.value() >= 0); |
| 52 | 1102 | } | |
| 53 | |||
| 54 | ✗ | VectorSpaceOperationTpl & operator=(const VectorSpaceOperationTpl & other) | |
| 55 | { | ||
| 56 | ✗ | size_.setValue(other.size_.value()); | |
| 57 | ✗ | assert(size_.value() >= 0); | |
| 58 | ✗ | return *this; | |
| 59 | } | ||
| 60 | |||
| 61 | 752174 | Index nq() const | |
| 62 | { | ||
| 63 | 752174 | return size_.value(); | |
| 64 | } | ||
| 65 | 76474 | Index nv() const | |
| 66 | { | ||
| 67 | 152894 | return size_.value(); | |
| 68 | } | ||
| 69 | |||
| 70 | 2642 | ConfigVector_t neutral() const | |
| 71 | { | ||
| 72 |
1/2✓ Branch 3 taken 1321 times.
✗ Branch 4 not taken.
|
2642 | return ConfigVector_t::Zero(size_.value()); |
| 73 | } | ||
| 74 | |||
| 75 | 706 | std::string name() const | |
| 76 | { | ||
| 77 |
1/2✓ Branch 1 taken 353 times.
✗ Branch 2 not taken.
|
706 | std::ostringstream oss; |
| 78 |
2/4✓ Branch 1 taken 353 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 353 times.
✗ Branch 6 not taken.
|
706 | oss << "R^" << nq(); |
| 79 |
1/2✓ Branch 1 taken 353 times.
✗ Branch 2 not taken.
|
1412 | return oss.str(); |
| 80 | 706 | } | |
| 81 | |||
| 82 | template<class ConfigL_t, class ConfigR_t, class Tangent_t> | ||
| 83 | 145636 | static void difference_impl( | |
| 84 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
| 85 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
| 86 | const Eigen::MatrixBase<Tangent_t> & d) | ||
| 87 | { | ||
| 88 |
1/2✓ Branch 3 taken 72818 times.
✗ Branch 4 not taken.
|
145636 | PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d) = q1 - q0; |
| 89 | 145636 | } | |
| 90 | |||
| 91 | template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t> | ||
| 92 | 1120 | void dDifference_impl( | |
| 93 | const Eigen::MatrixBase<ConfigL_t> &, | ||
| 94 | const Eigen::MatrixBase<ConfigR_t> &, | ||
| 95 | const Eigen::MatrixBase<JacobianOut_t> & J) const | ||
| 96 | { | ||
| 97 | if (arg == ARG0) | ||
| 98 |
2/4✓ Branch 2 taken 240 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 240 times.
✗ Branch 7 not taken.
|
480 | PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J) = |
| 99 | 480 | -JacobianMatrix_t::Identity(size_.value(), size_.value()); | |
| 100 | else if (arg == ARG1) | ||
| 101 | 640 | PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setIdentity(); | |
| 102 | 1120 | } | |
| 103 | |||
| 104 | template< | ||
| 105 | ArgumentPosition arg, | ||
| 106 | class ConfigL_t, | ||
| 107 | class ConfigR_t, | ||
| 108 | class JacobianIn_t, | ||
| 109 | class JacobianOut_t> | ||
| 110 | 16 | static void dDifference_product_impl( | |
| 111 | const ConfigL_t &, | ||
| 112 | const ConfigR_t &, | ||
| 113 | const JacobianIn_t & Jin, | ||
| 114 | JacobianOut_t & Jout, | ||
| 115 | bool, | ||
| 116 | const AssignmentOperatorType op) | ||
| 117 | { | ||
| 118 |
1/4✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
16 | switch (op) |
| 119 | { | ||
| 120 | 16 | case SETTO: | |
| 121 | if (arg == ARG0) | ||
| 122 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
8 | Jout = -Jin; |
| 123 | else | ||
| 124 | 8 | Jout = Jin; | |
| 125 | 16 | return; | |
| 126 | ✗ | case ADDTO: | |
| 127 | if (arg == ARG0) | ||
| 128 | ✗ | Jout -= Jin; | |
| 129 | else | ||
| 130 | ✗ | Jout += Jin; | |
| 131 | ✗ | return; | |
| 132 | ✗ | case RMTO: | |
| 133 | if (arg == ARG0) | ||
| 134 | ✗ | Jout += Jin; | |
| 135 | else | ||
| 136 | ✗ | Jout -= Jin; | |
| 137 | ✗ | return; | |
| 138 | } | ||
| 139 | } | ||
| 140 | |||
| 141 | template<class ConfigIn_t, class Velocity_t, class ConfigOut_t> | ||
| 142 | 287064 | static void integrate_impl( | |
| 143 | const Eigen::MatrixBase<ConfigIn_t> & q, | ||
| 144 | const Eigen::MatrixBase<Velocity_t> & v, | ||
| 145 | const Eigen::MatrixBase<ConfigOut_t> & qout) | ||
| 146 | { | ||
| 147 |
1/2✓ Branch 3 taken 143532 times.
✗ Branch 4 not taken.
|
287064 | PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout) = q + v; |
| 148 | 287064 | } | |
| 149 | |||
| 150 | template<class Config_t, class Jacobian_t> | ||
| 151 | 212 | static void integrateCoeffWiseJacobian_impl( | |
| 152 | const Eigen::MatrixBase<Config_t> &, const Eigen::MatrixBase<Jacobian_t> & J) | ||
| 153 | { | ||
| 154 | 212 | PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).setIdentity(); | |
| 155 | 212 | } | |
| 156 | |||
| 157 | template<class Config_t, class Tangent_t, class JacobianOut_t> | ||
| 158 | 632 | static void dIntegrate_dq_impl( | |
| 159 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
| 160 | const Eigen::MatrixBase<Tangent_t> & /*v*/, | ||
| 161 | const Eigen::MatrixBase<JacobianOut_t> & J, | ||
| 162 | const AssignmentOperatorType op = SETTO) | ||
| 163 | { | ||
| 164 | 632 | Eigen::MatrixBase<JacobianOut_t> & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J); | |
| 165 |
3/4✓ Branch 0 taken 300 times.
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
632 | switch (op) |
| 166 | { | ||
| 167 | 600 | case SETTO: | |
| 168 | 600 | Jout.setIdentity(); | |
| 169 | 600 | break; | |
| 170 | 16 | case ADDTO: | |
| 171 |
3/9✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
16 | Jout.diagonal().array() += Scalar(1); |
| 172 | 16 | break; | |
| 173 | 16 | case RMTO: | |
| 174 |
3/9✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
16 | Jout.diagonal().array() -= Scalar(1); |
| 175 | 16 | break; | |
| 176 | ✗ | default: | |
| 177 | ✗ | assert(false && "Wrong Op requesed value"); | |
| 178 | break; | ||
| 179 | } | ||
| 180 | 632 | } | |
| 181 | |||
| 182 | template<class Config_t, class Tangent_t, class JacobianOut_t> | ||
| 183 | 792 | static void dIntegrate_dv_impl( | |
| 184 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
| 185 | const Eigen::MatrixBase<Tangent_t> & /*v*/, | ||
| 186 | const Eigen::MatrixBase<JacobianOut_t> & J, | ||
| 187 | const AssignmentOperatorType op = SETTO) | ||
| 188 | { | ||
| 189 | 792 | Eigen::MatrixBase<JacobianOut_t> & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J); | |
| 190 |
3/4✓ Branch 0 taken 380 times.
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
792 | switch (op) |
| 191 | { | ||
| 192 | 760 | case SETTO: | |
| 193 | 760 | Jout.setIdentity(); | |
| 194 | 760 | break; | |
| 195 | 16 | case ADDTO: | |
| 196 |
3/9✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
16 | Jout.diagonal().array() += Scalar(1); |
| 197 | 16 | break; | |
| 198 | 16 | case RMTO: | |
| 199 |
3/9✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
16 | Jout.diagonal().array() -= Scalar(1); |
| 200 | 16 | break; | |
| 201 | ✗ | default: | |
| 202 | ✗ | assert(false && "Wrong Op requesed value"); | |
| 203 | break; | ||
| 204 | } | ||
| 205 | 792 | } | |
| 206 | |||
| 207 | template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> | ||
| 208 | 16 | static void dIntegrate_product_impl( | |
| 209 | const Config_t &, | ||
| 210 | const Tangent_t &, | ||
| 211 | const JacobianIn_t & Jin, | ||
| 212 | JacobianOut_t & Jout, | ||
| 213 | bool, | ||
| 214 | const ArgumentPosition, | ||
| 215 | const AssignmentOperatorType op) | ||
| 216 | { | ||
| 217 |
1/4✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
16 | switch (op) |
| 218 | { | ||
| 219 | 16 | case SETTO: | |
| 220 | 16 | Jout = Jin; | |
| 221 | 16 | break; | |
| 222 | ✗ | case ADDTO: | |
| 223 | ✗ | Jout += Jin; | |
| 224 | ✗ | break; | |
| 225 | ✗ | case RMTO: | |
| 226 | ✗ | Jout -= Jin; | |
| 227 | ✗ | break; | |
| 228 | ✗ | default: | |
| 229 | ✗ | assert(false && "Wrong Op requesed value"); | |
| 230 | break; | ||
| 231 | } | ||
| 232 | } | ||
| 233 | |||
| 234 | template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> | ||
| 235 | 728 | static void dIntegrateTransport_dq_impl( | |
| 236 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
| 237 | const Eigen::MatrixBase<Tangent_t> & /*v*/, | ||
| 238 | const Eigen::MatrixBase<JacobianIn_t> & Jin, | ||
| 239 | const Eigen::MatrixBase<JacobianOut_t> & Jout) | ||
| 240 | { | ||
| 241 | 728 | PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin; | |
| 242 | 728 | } | |
| 243 | |||
| 244 | template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> | ||
| 245 | 72 | static void dIntegrateTransport_dv_impl( | |
| 246 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
| 247 | const Eigen::MatrixBase<Tangent_t> & /*v*/, | ||
| 248 | const Eigen::MatrixBase<JacobianIn_t> & Jin, | ||
| 249 | const Eigen::MatrixBase<JacobianOut_t> & Jout) | ||
| 250 | { | ||
| 251 | 72 | PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin; | |
| 252 | } | ||
| 253 | |||
| 254 | template<class Config_t, class Tangent_t, class Jacobian_t> | ||
| 255 | 16 | static void dIntegrateTransport_dq_impl( | |
| 256 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
| 257 | const Eigen::MatrixBase<Tangent_t> & /*v*/, | ||
| 258 | const Eigen::MatrixBase<Jacobian_t> & /*J*/) | ||
| 259 | { | ||
| 260 | } | ||
| 261 | |||
| 262 | template<class Config_t, class Tangent_t, class Jacobian_t> | ||
| 263 | 16 | static void dIntegrateTransport_dv_impl( | |
| 264 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
| 265 | const Eigen::MatrixBase<Tangent_t> & /*v*/, | ||
| 266 | const Eigen::MatrixBase<Jacobian_t> & /*J*/) | ||
| 267 | { | ||
| 268 | } | ||
| 269 | |||
| 270 | // template <class ConfigL_t, class ConfigR_t> | ||
| 271 | // static context::Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0, | ||
| 272 | // const Eigen::MatrixBase<ConfigR_t> & q1) | ||
| 273 | |||
| 274 | template<class Config_t> | ||
| 275 | 20818 | static void normalize_impl(const Eigen::MatrixBase<Config_t> & /*qout*/) | |
| 276 | { | ||
| 277 | 20818 | } | |
| 278 | |||
| 279 | template<class Config_t> | ||
| 280 | static bool | ||
| 281 | 82032 | isNormalized_impl(const Eigen::MatrixBase<Config_t> & /*qout*/, const Scalar & /*prec*/) | |
| 282 | { | ||
| 283 | 82032 | return true; | |
| 284 | } | ||
| 285 | |||
| 286 | template<class Config_t> | ||
| 287 | 1859 | static void random_impl(const Eigen::MatrixBase<Config_t> & qout) | |
| 288 | { | ||
| 289 | 1859 | PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).setRandom(); | |
| 290 | 1859 | } | |
| 291 | |||
| 292 | template<class ConfigL_t, class ConfigR_t, class ConfigOut_t> | ||
| 293 | 224814 | void randomConfiguration_impl( | |
| 294 | const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit, | ||
| 295 | const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit, | ||
| 296 | const Eigen::MatrixBase<ConfigOut_t> & qout) const | ||
| 297 | { | ||
| 298 | 224814 | ConfigOut_t & res = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).derived(); | |
| 299 |
2/2✓ Branch 1 taken 138819 times.
✓ Branch 2 taken 112414 times.
|
502434 | for (int i = 0; i < nq(); ++i) |
| 300 | { | ||
| 301 |
0/16✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
|
555244 | if (check_expression_if_real<Scalar, false>( |
| 302 | 277622 | lower_pos_limit[i] == -std::numeric_limits<typename ConfigL_t::Scalar>::infinity() | |
| 303 |
6/8✓ Branch 0 taken 138818 times.
✓ Branch 1 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 138818 times.
✓ Branch 7 taken 138819 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 138818 times.
|
277622 | || upper_pos_limit[i] == std::numeric_limits<typename ConfigR_t::Scalar>::infinity())) |
| 304 | { | ||
| 305 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::ostringstream error; |
| 306 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | error << "non bounded limit. Cannot uniformly sample joint at rank " << i; |
| 307 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | throw std::range_error(error.str()); |
| 308 | } | ||
| 309 |
0/20✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
|
277620 | res[i] = |
| 310 | 277620 | lower_pos_limit[i] + ((upper_pos_limit[i] - lower_pos_limit[i]) * rand()) / RAND_MAX; | |
| 311 | } | ||
| 312 | 224812 | } | |
| 313 | |||
| 314 | 5 | bool isEqual_impl(const VectorSpaceOperationTpl & other) const | |
| 315 | { | ||
| 316 | 5 | return size_.value() == other.size_.value(); | |
| 317 | } | ||
| 318 | |||
| 319 | private: | ||
| 320 | Eigen::internal::variable_if_dynamic<Index, Dim> size_; | ||
| 321 | }; // struct VectorSpaceOperationTpl | ||
| 322 | |||
| 323 | } // namespace pinocchio | ||
| 324 | |||
| 325 | #endif // ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__ | ||
| 326 |