| Directory: | ./ |
|---|---|
| File: | include/pinocchio/spatial/inertia.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 517 | 522 | 99.0% |
| Branches: | 620 | 2224 | 27.9% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2021 CNRS INRIA | ||
| 3 | // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
| 4 | // | ||
| 5 | |||
| 6 | #ifndef __pinocchio_spatial_inertia_hpp__ | ||
| 7 | #define __pinocchio_spatial_inertia_hpp__ | ||
| 8 | |||
| 9 | #include "pinocchio/math/fwd.hpp" | ||
| 10 | #include "pinocchio/spatial/symmetric3.hpp" | ||
| 11 | #include "pinocchio/spatial/force.hpp" | ||
| 12 | #include "pinocchio/spatial/motion.hpp" | ||
| 13 | #include "pinocchio/spatial/skew.hpp" | ||
| 14 | |||
| 15 | namespace pinocchio | ||
| 16 | { | ||
| 17 | template<class Derived> | ||
| 18 | struct InertiaBase : NumericalBase<Derived> | ||
| 19 | { | ||
| 20 | SPATIAL_TYPEDEF_TEMPLATE(Derived); | ||
| 21 | |||
| 22 | 105492 | Derived & derived() | |
| 23 | { | ||
| 24 | 105492 | return *static_cast<Derived *>(this); | |
| 25 | } | ||
| 26 | 385786 | const Derived & derived() const | |
| 27 | { | ||
| 28 | 386071 | return *static_cast<const Derived *>(this); | |
| 29 | } | ||
| 30 | |||
| 31 | Derived & const_cast_derived() const | ||
| 32 | { | ||
| 33 | return *const_cast<Derived *>(&derived()); | ||
| 34 | } | ||
| 35 | |||
| 36 | Scalar mass() const | ||
| 37 | { | ||
| 38 | return static_cast<const Derived *>(this)->mass(); | ||
| 39 | } | ||
| 40 | Scalar & mass() | ||
| 41 | { | ||
| 42 | return static_cast<const Derived *>(this)->mass(); | ||
| 43 | } | ||
| 44 | const Vector3 & lever() const | ||
| 45 | { | ||
| 46 | return static_cast<const Derived *>(this)->lever(); | ||
| 47 | } | ||
| 48 | Vector3 & lever() | ||
| 49 | { | ||
| 50 | return static_cast<const Derived *>(this)->lever(); | ||
| 51 | } | ||
| 52 | const Symmetric3 & inertia() const | ||
| 53 | { | ||
| 54 | return static_cast<const Derived *>(this)->inertia(); | ||
| 55 | } | ||
| 56 | Symmetric3 & inertia() | ||
| 57 | { | ||
| 58 | return static_cast<const Derived *>(this)->inertia(); | ||
| 59 | } | ||
| 60 | |||
| 61 | template<typename Matrix6Like> | ||
| 62 | void matrix(const Eigen::MatrixBase<Matrix6Like> & mat) const | ||
| 63 | { | ||
| 64 | derived().matrix_impl(mat); | ||
| 65 | } | ||
| 66 | 9694 | Matrix6 matrix() const | |
| 67 | { | ||
| 68 | 9694 | return derived().matrix_impl(); | |
| 69 | } | ||
| 70 | 862 | operator Matrix6() const | |
| 71 | { | ||
| 72 | 862 | return matrix(); | |
| 73 | } | ||
| 74 | |||
| 75 | template<typename Matrix6Like> | ||
| 76 | void inverse(const Eigen::MatrixBase<Matrix6Like> & mat) const | ||
| 77 | { | ||
| 78 | derived().inverse_impl(mat); | ||
| 79 | } | ||
| 80 | 1 | Matrix6 inverse() const | |
| 81 | { | ||
| 82 | 1 | return derived().inverse_impl(); | |
| 83 | } | ||
| 84 | |||
| 85 | Derived & operator=(const Derived & clone) | ||
| 86 | { | ||
| 87 | return derived().__equl__(clone); | ||
| 88 | } | ||
| 89 | 11748 | bool operator==(const Derived & other) const | |
| 90 | { | ||
| 91 | 11748 | return derived().isEqual(other); | |
| 92 | } | ||
| 93 | 647 | bool operator!=(const Derived & other) const | |
| 94 | { | ||
| 95 | 647 | return !(*this == other); | |
| 96 | } | ||
| 97 | |||
| 98 | 105489 | Derived & operator+=(const Derived & Yb) | |
| 99 | { | ||
| 100 | 105489 | return derived().__pequ__(Yb); | |
| 101 | } | ||
| 102 | 100007 | Derived operator+(const Derived & Yb) const | |
| 103 | { | ||
| 104 | 100007 | return derived().__plus__(Yb); | |
| 105 | } | ||
| 106 | 3 | Derived & operator-=(const Derived & Yb) | |
| 107 | { | ||
| 108 | 3 | return derived().__mequ__(Yb); | |
| 109 | } | ||
| 110 | 1 | Derived operator-(const Derived & Yb) const | |
| 111 | { | ||
| 112 | 1 | return derived().__minus__(Yb); | |
| 113 | } | ||
| 114 | |||
| 115 | template<typename MotionDerived> | ||
| 116 | ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options> | ||
| 117 | 167614 | operator*(const MotionDense<MotionDerived> & v) const | |
| 118 | { | ||
| 119 | 167614 | return derived().__mult__(v); | |
| 120 | } | ||
| 121 | |||
| 122 | template<typename MotionDerived> | ||
| 123 | 7523 | Scalar vtiv(const MotionDense<MotionDerived> & v) const | |
| 124 | { | ||
| 125 | 7523 | return derived().vtiv_impl(v); | |
| 126 | } | ||
| 127 | |||
| 128 | template<typename MotionDerived> | ||
| 129 | Matrix6 variation(const MotionDense<MotionDerived> & v) const | ||
| 130 | { | ||
| 131 | return derived().variation_impl(v); | ||
| 132 | } | ||
| 133 | |||
| 134 | /// \brief Time variation operator. | ||
| 135 | /// It computes the time derivative of an inertia I corresponding to the formula \f$ | ||
| 136 | /// \dot{I} = v \times^{*} I \f$. | ||
| 137 | /// | ||
| 138 | /// \param[in] v The spatial velocity of the frame supporting the inertia. | ||
| 139 | /// \param[in] I The spatial inertia in motion. | ||
| 140 | /// \param[out] Iout The time derivative of the inertia I. | ||
| 141 | /// | ||
| 142 | template<typename MotionDerived, typename M6> | ||
| 143 | static void | ||
| 144 | 131 | vxi(const MotionDense<MotionDerived> & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout) | |
| 145 | { | ||
| 146 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6); | ||
| 147 | 131 | Derived::vxi_impl(v, I, Iout); | |
| 148 | 131 | } | |
| 149 | |||
| 150 | template<typename MotionDerived> | ||
| 151 | 129 | Matrix6 vxi(const MotionDense<MotionDerived> & v) const | |
| 152 | { | ||
| 153 | 129 | Matrix6 Iout; | |
| 154 |
0/2✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
129 | vxi(v, derived(), Iout); |
| 155 | 129 | return Iout; | |
| 156 | } | ||
| 157 | |||
| 158 | /// \brief Time variation operator. | ||
| 159 | /// It computes the time derivative of an inertia I corresponding to the formula \f$ | ||
| 160 | /// \dot{I} = v \times^{*} I \f$. | ||
| 161 | /// | ||
| 162 | /// \param[in] v The spatial velocity of the frame supporting the inertia. | ||
| 163 | /// \param[in] I The spatial inertia in motion. | ||
| 164 | /// \param[out] Iout The time derivative of the inertia I. | ||
| 165 | /// | ||
| 166 | template<typename MotionDerived, typename M6> | ||
| 167 | static void | ||
| 168 | 3 | ivx(const MotionDense<MotionDerived> & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout) | |
| 169 | { | ||
| 170 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6); | ||
| 171 | 3 | Derived::ivx_impl(v, I, Iout); | |
| 172 | 3 | } | |
| 173 | |||
| 174 | template<typename MotionDerived> | ||
| 175 | 1 | Matrix6 ivx(const MotionDense<MotionDerived> & v) const | |
| 176 | { | ||
| 177 | 1 | Matrix6 Iout; | |
| 178 |
0/2✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
1 | ivx(v, derived(), Iout); |
| 179 | 1 | return Iout; | |
| 180 | } | ||
| 181 | |||
| 182 | void setZero() | ||
| 183 | { | ||
| 184 | derived().setZero(); | ||
| 185 | } | ||
| 186 | void setIdentity() | ||
| 187 | { | ||
| 188 | derived().setIdentity(); | ||
| 189 | } | ||
| 190 | void setRandom() | ||
| 191 | { | ||
| 192 | derived().setRandom(); | ||
| 193 | } | ||
| 194 | |||
| 195 | 1307 | bool isApprox( | |
| 196 | const Derived & other, | ||
| 197 | const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | ||
| 198 | { | ||
| 199 | 1307 | return derived().isApprox_impl(other, prec); | |
| 200 | } | ||
| 201 | |||
| 202 | 2334 | bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | |
| 203 | { | ||
| 204 | 2334 | return derived().isZero_impl(prec); | |
| 205 | } | ||
| 206 | |||
| 207 | /// aI = aXb.act(bI) | ||
| 208 | template<typename S2, int O2> | ||
| 209 | 106157 | Derived se3Action(const SE3Tpl<S2, O2> & M) const | |
| 210 | { | ||
| 211 | 106157 | return derived().se3Action_impl(M); | |
| 212 | } | ||
| 213 | |||
| 214 | /// bI = aXb.actInv(aI) | ||
| 215 | template<typename S2, int O2> | ||
| 216 | 9 | Derived se3ActionInverse(const SE3Tpl<S2, O2> & M) const | |
| 217 | { | ||
| 218 | 9 | return derived().se3ActionInverse_impl(M); | |
| 219 | } | ||
| 220 | |||
| 221 | 100009 | void disp(std::ostream & os) const | |
| 222 | { | ||
| 223 | 100009 | static_cast<const Derived *>(this)->disp_impl(os); | |
| 224 | 100009 | } | |
| 225 | 100009 | friend std::ostream & operator<<(std::ostream & os, const InertiaBase<Derived> & X) | |
| 226 | { | ||
| 227 | 100009 | X.disp(os); | |
| 228 | 100009 | return os; | |
| 229 | } | ||
| 230 | }; | ||
| 231 | |||
| 232 | // class InertiaBase | ||
| 233 | template<typename T, int U> | ||
| 234 | struct traits<InertiaTpl<T, U>> | ||
| 235 | { | ||
| 236 | typedef T Scalar; | ||
| 237 | typedef Eigen::Matrix<T, 3, 1, U> Vector3; | ||
| 238 | typedef Eigen::Matrix<T, 4, 1, U> Vector4; | ||
| 239 | typedef Eigen::Matrix<T, 6, 1, U> Vector6; | ||
| 240 | typedef Eigen::Matrix<T, 3, 3, U> Matrix3; | ||
| 241 | typedef Eigen::Matrix<T, 4, 4, U> Matrix4; | ||
| 242 | typedef Eigen::Matrix<T, 6, 6, U> Matrix6; | ||
| 243 | typedef Eigen::Matrix<T, 10, 10, U> Matrix10; | ||
| 244 | typedef Matrix6 ActionMatrix_t; | ||
| 245 | typedef Vector3 Angular_t; | ||
| 246 | typedef Vector3 Linear_t; | ||
| 247 | typedef const Vector3 ConstAngular_t; | ||
| 248 | typedef const Vector3 ConstLinear_t; | ||
| 249 | typedef Eigen::Quaternion<T, U> Quaternion_t; | ||
| 250 | typedef SE3Tpl<T, U> SE3; | ||
| 251 | typedef ForceTpl<T, U> Force; | ||
| 252 | typedef MotionTpl<T, U> Motion; | ||
| 253 | typedef Symmetric3Tpl<T, U> Symmetric3; | ||
| 254 | typedef PseudoInertiaTpl<T, U> PseudoInertia; | ||
| 255 | typedef LogCholeskyParametersTpl<T, U> LogCholeskyParameters; | ||
| 256 | enum | ||
| 257 | { | ||
| 258 | LINEAR = 0, | ||
| 259 | ANGULAR = 3 | ||
| 260 | }; | ||
| 261 | }; // traits InertiaTpl | ||
| 262 | |||
| 263 | template<typename _Scalar, int _Options> | ||
| 264 | struct InertiaTpl : public InertiaBase<InertiaTpl<_Scalar, _Options>> | ||
| 265 | { | ||
| 266 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 267 | |||
| 268 | SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl); | ||
| 269 | enum | ||
| 270 | { | ||
| 271 | Options = _Options | ||
| 272 | }; | ||
| 273 | |||
| 274 | typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare; | ||
| 275 | typedef Eigen::Matrix<Scalar, 10, 1, Options> Vector10; | ||
| 276 | typedef Eigen::Matrix<Scalar, 10, 10, Options> Matrix10; | ||
| 277 | typedef PseudoInertiaTpl<Scalar, Options> PseudoInertia; | ||
| 278 | typedef LogCholeskyParametersTpl<Scalar, Options> LogCholeskyParameters; | ||
| 279 | |||
| 280 | // Constructors | ||
| 281 | 1779 | InertiaTpl() | |
| 282 |
2/4✓ Branch 2 taken 411 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 411 times.
✗ Branch 6 not taken.
|
1779 | { |
| 283 | 1779 | } | |
| 284 | |||
| 285 | 3042 | InertiaTpl(const Scalar & mass, const Vector3 & com, const Matrix3 & rotational_inertia) | |
| 286 | 3042 | : m_mass(mass) | |
| 287 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
3042 | , m_com(com) |
| 288 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
3042 | , m_inertia(rotational_inertia) |
| 289 | { | ||
| 290 | 3042 | } | |
| 291 | |||
| 292 | 1 | explicit InertiaTpl(const Matrix6 & I6) | |
| 293 | 1 | { | |
| 294 |
5/10✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
|
1 | assert(check_expression_if_real<Scalar>(pinocchio::isZero(I6 - I6.transpose()))); |
| 295 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | mass() = I6(LINEAR, LINEAR); |
| 296 | const typename Matrix6::template ConstFixedBlockXpr<3, 3>::Type mc_cross = | ||
| 297 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | I6.template block<3, 3>(ANGULAR, LINEAR); |
| 298 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | lever() = unSkew(mc_cross); |
| 299 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | lever() /= mass(); |
| 300 | |||
| 301 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Matrix3 I3(mc_cross * mc_cross); |
| 302 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | I3 /= mass(); |
| 303 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | I3 += I6.template block<3, 3>(ANGULAR, ANGULAR); |
| 304 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | const Symmetric3 S3(I3); |
| 305 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | inertia() = S3; |
| 306 | 1 | } | |
| 307 | |||
| 308 | 479801 | InertiaTpl(const Scalar & mass, const Vector3 & com, const Symmetric3 & rotational_inertia) | |
| 309 | 479801 | : m_mass(mass) | |
| 310 |
1/2✓ Branch 1 taken 2317 times.
✗ Branch 2 not taken.
|
479801 | , m_com(com) |
| 311 |
1/2✓ Branch 1 taken 2317 times.
✗ Branch 2 not taken.
|
479801 | , m_inertia(rotational_inertia) |
| 312 | { | ||
| 313 | 479801 | } | |
| 314 | |||
| 315 | 581906 | InertiaTpl(const InertiaTpl & clone) // Copy constructor | |
| 316 | 581906 | : m_mass(clone.mass()) | |
| 317 |
1/2✓ Branch 2 taken 2393 times.
✗ Branch 3 not taken.
|
581906 | , m_com(clone.lever()) |
| 318 |
1/2✓ Branch 2 taken 2393 times.
✗ Branch 3 not taken.
|
581906 | , m_inertia(clone.inertia()) |
| 319 | { | ||
| 320 | 581906 | } | |
| 321 | |||
| 322 | 116323 | InertiaTpl & operator=(const InertiaTpl & clone) // Copy assignment operator | |
| 323 | { | ||
| 324 |
1/2✓ Branch 2 taken 1423 times.
✗ Branch 3 not taken.
|
116323 | m_mass = clone.mass(); |
| 325 | 116323 | m_com = clone.lever(); | |
| 326 | 116323 | m_inertia = clone.inertia(); | |
| 327 | 116323 | return *this; | |
| 328 | } | ||
| 329 | |||
| 330 | template<typename S2, int O2> | ||
| 331 | 1 | explicit InertiaTpl(const InertiaTpl<S2, O2> & clone) | |
| 332 | 1 | { | |
| 333 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | *this = clone.template cast<Scalar>(); |
| 334 | 1 | } | |
| 335 | |||
| 336 | // Initializers | ||
| 337 | 53107 | static InertiaTpl Zero() | |
| 338 | { | ||
| 339 |
4/8✓ Branch 2 taken 52050 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 52050 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 52050 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 930 times.
✗ Branch 12 not taken.
|
106214 | return InertiaTpl(Scalar(0), Vector3::Zero(), Symmetric3::Zero()); |
| 340 | } | ||
| 341 | |||
| 342 | 2402 | void setZero() | |
| 343 | { | ||
| 344 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
2402 | mass() = Scalar(0); |
| 345 | 2402 | lever().setZero(); | |
| 346 | 2402 | inertia().setZero(); | |
| 347 | 2402 | } | |
| 348 | |||
| 349 | 292 | static InertiaTpl Identity() | |
| 350 | { | ||
| 351 |
3/8✓ Branch 2 taken 292 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 292 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 292 times.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
|
584 | return InertiaTpl(Scalar(1), Vector3::Zero(), Symmetric3::Identity()); |
| 352 | } | ||
| 353 | |||
| 354 | 1 | void setIdentity() | |
| 355 | { | ||
| 356 |
0/2✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
1 | mass() = Scalar(1); |
| 357 | 1 | lever().setZero(); | |
| 358 | 1 | inertia().setIdentity(); | |
| 359 | 1 | } | |
| 360 | |||
| 361 | 211236 | static InertiaTpl Random() | |
| 362 | { | ||
| 363 | // We have to shoot "I" definite positive and not only symmetric. | ||
| 364 | return InertiaTpl( | ||
| 365 |
4/12✓ Branch 2 taken 211236 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 211236 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 211236 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 211236 times.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
|
422472 | Eigen::internal::random<Scalar>() + 1, Vector3::Random(), Symmetric3::RandomPositive()); |
| 366 | } | ||
| 367 | |||
| 368 | /// | ||
| 369 | /// \brief Computes the Inertia of a sphere defined by its mass and its radius. | ||
| 370 | /// | ||
| 371 | /// \param[in] mass of the sphere. | ||
| 372 | /// \param[in] radius of the sphere. | ||
| 373 | /// | ||
| 374 | 7 | static InertiaTpl FromSphere(const Scalar mass, const Scalar radius) | |
| 375 | { | ||
| 376 |
0/8✗ 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.
|
7 | return FromEllipsoid(mass, radius, radius, radius); |
| 377 | } | ||
| 378 | |||
| 379 | /// | ||
| 380 | /// \brief Computes the Inertia of an ellipsoid defined by its mass and main semi-axis | ||
| 381 | /// dimensions (x,y,z). | ||
| 382 | /// | ||
| 383 | /// \param[in] mass of the ellipsoid. | ||
| 384 | /// \param[in] x semi-axis dimension along the local X axis. | ||
| 385 | /// \param[in] y semi-axis dimension along the local Y axis. | ||
| 386 | /// \param[in] z semi-axis dimension along the local Z axis. | ||
| 387 | /// | ||
| 388 | static InertiaTpl | ||
| 389 | 14 | FromEllipsoid(const Scalar mass, const Scalar x, const Scalar y, const Scalar z) | |
| 390 | { | ||
| 391 |
0/12✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
14 | const Scalar a = mass * (y * y + z * z) / Scalar(5); |
| 392 |
0/12✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
14 | const Scalar b = mass * (x * x + z * z) / Scalar(5); |
| 393 |
0/12✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
14 | const Scalar c = mass * (y * y + x * x) / Scalar(5); |
| 394 | return InertiaTpl( | ||
| 395 |
4/14✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
14 | mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b, Scalar(0), Scalar(0), c)); |
| 396 | } | ||
| 397 | |||
| 398 | /// | ||
| 399 | /// \brief Computes the Inertia of a cylinder defined by its mass, radius and length along the Z | ||
| 400 | /// axis. | ||
| 401 | /// | ||
| 402 | /// \param[in] mass of the cylinder. | ||
| 403 | /// \param[in] radius of the cylinder. | ||
| 404 | /// \param[in] length of the cylinder. | ||
| 405 | /// | ||
| 406 | 10 | static InertiaTpl FromCylinder(const Scalar mass, const Scalar radius, const Scalar length) | |
| 407 | { | ||
| 408 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
10 | const Scalar radius_square = radius * radius; |
| 409 |
0/14✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
10 | const Scalar a = mass * (radius_square / Scalar(4) + length * length / Scalar(12)); |
| 410 |
0/6✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
10 | const Scalar c = mass * (radius_square / Scalar(2)); |
| 411 | return InertiaTpl( | ||
| 412 |
4/14✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
10 | mass, Vector3::Zero(), Symmetric3(a, Scalar(0), a, Scalar(0), Scalar(0), c)); |
| 413 | } | ||
| 414 | |||
| 415 | /// | ||
| 416 | /// \brief Computes the Inertia of a box defined by its mass and main dimensions (x,y,z). | ||
| 417 | /// | ||
| 418 | /// \param[in] mass of the box. | ||
| 419 | /// \param[in] x dimension along the local X axis. | ||
| 420 | /// \param[in] y dimension along the local Y axis. | ||
| 421 | /// \param[in] z dimension along the local Z axis. | ||
| 422 | /// | ||
| 423 | 12 | static InertiaTpl FromBox(const Scalar mass, const Scalar x, const Scalar y, const Scalar z) | |
| 424 | { | ||
| 425 |
0/12✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
12 | const Scalar a = mass * (y * y + z * z) / Scalar(12); |
| 426 |
0/12✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
12 | const Scalar b = mass * (x * x + z * z) / Scalar(12); |
| 427 |
0/12✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
12 | const Scalar c = mass * (y * y + x * x) / Scalar(12); |
| 428 | return InertiaTpl( | ||
| 429 |
4/14✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
12 | mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b, Scalar(0), Scalar(0), c)); |
| 430 | } | ||
| 431 | |||
| 432 | /// | ||
| 433 | /// \brief Computes the Inertia of a capsule defined by its mass, radius and length along the Z | ||
| 434 | /// axis. Assumes a uniform density. | ||
| 435 | /// | ||
| 436 | /// \param[in] mass of the capsule. | ||
| 437 | /// \param[in] radius of the capsule. | ||
| 438 | /// \param[in] height of the capsule. | ||
| 439 | 26 | static InertiaTpl FromCapsule(const Scalar mass, const Scalar radius, const Scalar height) | |
| 440 | { | ||
| 441 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
26 | const Scalar pi = boost::math::constants::pi<Scalar>(); |
| 442 | |||
| 443 | // first need to compute mass repartition between cylinder and halfsphere | ||
| 444 |
1/6✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
26 | const Scalar v_cyl = pi * math::pow(radius, 2) * height; |
| 445 |
1/12✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
26 | const Scalar v_hs = Scalar(2) / Scalar(3) * math::pow(radius, 3) * pi; |
| 446 |
0/6✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
26 | const Scalar total_v = v_cyl + Scalar(2) * v_hs; |
| 447 | |||
| 448 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
26 | const Scalar m_cyl = mass * (v_cyl / total_v); |
| 449 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
26 | const Scalar m_hs = mass * (v_hs / total_v); |
| 450 | |||
| 451 | // Then Distance between halfSphere center and cylindere center. | ||
| 452 |
0/10✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
26 | const Scalar dist_hc_cyl = height / Scalar(2) + Scalar(0.375) * radius; |
| 453 | |||
| 454 | // Computes inertia terms | ||
| 455 |
0/8✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
26 | const Scalar ix_c = |
| 456 |
2/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
26 | m_cyl * (math::pow(height, 2) / Scalar(12) + math::pow(radius, 2) / Scalar(4)); |
| 457 |
1/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
26 | const Scalar iz_c = m_cyl * math::pow(radius, 2) / Scalar(2); |
| 458 | |||
| 459 | // For halfsphere inertia see, "Dynamics: Theory and Applications" McGraw-Hill, New York, | ||
| 460 | // 1985, by T.R. Kane and D.A. Levinson, Appendix 23. | ||
| 461 |
1/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
26 | const Scalar ix_hs = m_hs * math::pow(radius, 2) * Scalar(0.259375); |
| 462 |
1/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
26 | const Scalar iz_hs = m_hs * math::pow(radius, 2) * Scalar(0.4); |
| 463 | |||
| 464 | // Put everything together using the parallel axis theorem | ||
| 465 |
1/12✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
26 | const Scalar ix = ix_c + Scalar(2) * (ix_hs + m_hs * math::pow(dist_hc_cyl, 2)); |
| 466 |
0/6✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
26 | const Scalar iz = iz_c + Scalar(2) * iz_hs; |
| 467 | |||
| 468 | return InertiaTpl( | ||
| 469 |
4/14✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
26 | mass, Vector3::Zero(), Symmetric3(ix, Scalar(0), ix, Scalar(0), Scalar(0), iz)); |
| 470 | } | ||
| 471 | |||
| 472 | 3 | void setRandom() | |
| 473 | { | ||
| 474 |
0/6✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
3 | mass() = static_cast<Scalar>(std::rand()) / static_cast<Scalar>(RAND_MAX); |
| 475 | 3 | lever().setRandom(); | |
| 476 | 3 | inertia().setRandom(); | |
| 477 | 3 | } | |
| 478 | |||
| 479 | template<typename Matrix6Like> | ||
| 480 | 9694 | void matrix_impl(const Eigen::MatrixBase<Matrix6Like> & M_) const | |
| 481 | { | ||
| 482 | 9694 | Matrix6Like & M = M_.const_cast_derived(); | |
| 483 | |||
| 484 |
1/2✓ Branch 2 taken 9579 times.
✗ Branch 3 not taken.
|
9694 | M.template block<3, 3>(LINEAR, LINEAR).setZero(); |
| 485 |
4/7✓ Branch 2 taken 9579 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 207 times.
✓ Branch 6 taken 9372 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 207 times.
✗ Branch 9 not taken.
|
9694 | M.template block<3, 3>(LINEAR, LINEAR).diagonal().fill(mass()); |
| 486 |
5/8✓ Branch 3 taken 207 times.
✓ Branch 4 taken 9372 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 207 times.
✓ Branch 7 taken 9372 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 207 times.
✗ Branch 10 not taken.
|
9694 | M.template block<3, 3>(ANGULAR, LINEAR) = alphaSkew(mass(), lever()); |
| 487 |
3/6✓ Branch 2 taken 9579 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9579 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9579 times.
✗ Branch 9 not taken.
|
9694 | M.template block<3, 3>(LINEAR, ANGULAR) = -M.template block<3, 3>(ANGULAR, LINEAR); |
| 488 |
4/8✓ Branch 1 taken 9579 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9579 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9579 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9579 times.
✗ Branch 11 not taken.
|
9694 | M.template block<3, 3>(ANGULAR, ANGULAR) = |
| 489 | 9962 | (inertia() - AlphaSkewSquare(mass(), lever())).matrix(); | |
| 490 | 9694 | } | |
| 491 | |||
| 492 | 9694 | Matrix6 matrix_impl() const | |
| 493 | { | ||
| 494 | 9694 | Matrix6 M; | |
| 495 |
1/2✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
|
9694 | matrix_impl(M); |
| 496 | 9694 | return M; | |
| 497 | } | ||
| 498 | |||
| 499 | template<typename Matrix6Like> | ||
| 500 | 1 | void inverse_impl(const Eigen::MatrixBase<Matrix6Like> & M_) const | |
| 501 | { | ||
| 502 | 1 | Matrix6Like & M = M_.const_cast_derived(); | |
| 503 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | inertia().inverse(M.template block<3, 3>(ANGULAR, ANGULAR)); |
| 504 | |||
| 505 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
1 | M.template block<3, 3>(LINEAR, ANGULAR).noalias() = |
| 506 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | -M.template block<3, 3>(ANGULAR, ANGULAR).colwise().cross(lever()); |
| 507 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | M.template block<3, 3>(ANGULAR, LINEAR) = M.template block<3, 3>(LINEAR, ANGULAR).transpose(); |
| 508 | |||
| 509 |
3/11✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
1 | const Scalar &cx = lever()[0], cy = lever()[1], cz = lever()[2]; |
| 510 | |||
| 511 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
|
2 | M.template block<3, 3>(LINEAR, LINEAR).col(0).noalias() = |
| 512 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | cy * M.template block<3, 3>(LINEAR, ANGULAR).col(2) |
| 513 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | - cz * M.template block<3, 3>(LINEAR, ANGULAR).col(1); |
| 514 | |||
| 515 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
|
2 | M.template block<3, 3>(LINEAR, LINEAR).col(1).noalias() = |
| 516 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | cz * M.template block<3, 3>(LINEAR, ANGULAR).col(0) |
| 517 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | - cx * M.template block<3, 3>(LINEAR, ANGULAR).col(2); |
| 518 | |||
| 519 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
|
2 | M.template block<3, 3>(LINEAR, LINEAR).col(2).noalias() = |
| 520 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | cx * M.template block<3, 3>(LINEAR, ANGULAR).col(1) |
| 521 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | - cy * M.template block<3, 3>(LINEAR, ANGULAR).col(0); |
| 522 | |||
| 523 |
0/6✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
1 | const Scalar m_inv = Scalar(1) / mass(); |
| 524 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | M.template block<3, 3>(LINEAR, LINEAR).diagonal().array() += m_inv; |
| 525 | 1 | } | |
| 526 | |||
| 527 | 1 | Matrix6 inverse_impl() const | |
| 528 | { | ||
| 529 | 1 | Matrix6 res; | |
| 530 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
1 | inverse_impl(res); |
| 531 | 1 | return res; | |
| 532 | } | ||
| 533 | |||
| 534 | /** Returns the representation of the matrix as a vector of dynamic parameters. | ||
| 535 | * The parameters are given as | ||
| 536 | * \f$ v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T \f$ | ||
| 537 | * where \f$ c \f$ is the center of mass, \f$ I = I_C + mS^T(c)S(c) \f$ and \f$ I_C \f$ has its | ||
| 538 | * origin at the barycenter and \f$ S(c) \f$ is the the skew matrix representation of the cross | ||
| 539 | * product operator. | ||
| 540 | */ | ||
| 541 | 122 | Vector10 toDynamicParameters() const | |
| 542 | { | ||
| 543 | 122 | Vector10 v; | |
| 544 | |||
| 545 |
0/6✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
122 | v[0] = mass(); |
| 546 |
4/14✗ Branch 2 not taken.
✓ Branch 3 taken 122 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 122 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 122 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 122 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
122 | v.template segment<3>(1).noalias() = mass() * lever(); |
| 547 |
3/13✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 5 taken 122 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 122 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 122 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
122 | v.template segment<6>(4) = (inertia() - AlphaSkewSquare(mass(), lever())).data(); |
| 548 | |||
| 549 | 122 | return v; | |
| 550 | } | ||
| 551 | |||
| 552 | /** Builds and inertia matrix from a vector of dynamic parameters. | ||
| 553 | * | ||
| 554 | * @param[in] params The dynamic parameters. | ||
| 555 | * | ||
| 556 | * The parameters are given as | ||
| 557 | * \f$ v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T \f$ | ||
| 558 | * where \f$ I = I_C + mS^T(c)S(c) \f$ and \f$ I_C \f$ has its origin at the barycenter. | ||
| 559 | */ | ||
| 560 | template<typename Vector10Like> | ||
| 561 | 11 | static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase<Vector10Like> & params) | |
| 562 | { | ||
| 563 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
11 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, params, 10, 1); |
| 564 | |||
| 565 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
11 | const Scalar & mass = params[0]; |
| 566 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
11 | Vector3 lever = params.template segment<3>(1); |
| 567 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
11 | lever /= mass; |
| 568 | |||
| 569 | return InertiaTpl( | ||
| 570 |
6/12✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
|
11 | mass, lever, Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass, lever)); |
| 571 | } | ||
| 572 | |||
| 573 | /** | ||
| 574 | |||
| 575 | * @brief Create an InertiaTpl object from a PseudoInertia object. | ||
| 576 | * | ||
| 577 | * @param pseudo_inertia A PseudoInertia object. | ||
| 578 | * @return An InertiaTpl object. | ||
| 579 | */ | ||
| 580 | |||
| 581 | ✗ | static InertiaTpl FromPseudoInertia(const PseudoInertia & pseudo_inertia) | |
| 582 | { | ||
| 583 | ✗ | return pseudo_inertia.toInertia(); | |
| 584 | } | ||
| 585 | |||
| 586 | /** | ||
| 587 | * @brief Convert the InertiaTpl object to a 4x4 pseudo inertia matrix. | ||
| 588 | * | ||
| 589 | * @return A 4x4 pseudo inertia matrix. | ||
| 590 | */ | ||
| 591 | 3 | PseudoInertia toPseudoInertia() const | |
| 592 | { | ||
| 593 | 3 | PseudoInertia pseudo_inertia = PseudoInertia::FromInertia(*this); | |
| 594 | 3 | return pseudo_inertia; | |
| 595 | } | ||
| 596 | |||
| 597 | /** | ||
| 598 | * @brief Create an InertiaTpl object from log Cholesky parameters. | ||
| 599 | * | ||
| 600 | * @param log_cholesky A log cholesky parameters object | ||
| 601 | * | ||
| 602 | * @return An InertiaTpl object. | ||
| 603 | */ | ||
| 604 | 2 | static InertiaTpl FromLogCholeskyParameters(const LogCholeskyParameters & log_cholesky) | |
| 605 | { | ||
| 606 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | Vector10 dynamic_params = log_cholesky.toDynamicParameters(); |
| 607 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | return FromDynamicParameters(dynamic_params); |
| 608 | } | ||
| 609 | |||
| 610 | // Arithmetic operators | ||
| 611 | InertiaTpl & __equl__(const InertiaTpl & clone) | ||
| 612 | { | ||
| 613 | mass() = clone.mass(); | ||
| 614 | lever() = clone.lever(); | ||
| 615 | inertia() = clone.inertia(); | ||
| 616 | return *this; | ||
| 617 | } | ||
| 618 | |||
| 619 | // Required by std::vector boost::python bindings. | ||
| 620 | 11749 | bool isEqual(const InertiaTpl & Y2) const | |
| 621 | { | ||
| 622 |
4/6✓ Branch 2 taken 8104 times.
✓ Branch 3 taken 2 times.
✓ Branch 7 taken 8104 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 8104 times.
✗ Branch 13 not taken.
|
11749 | return (mass() == Y2.mass()) && (lever() == Y2.lever()) && (inertia() == Y2.inertia()); |
| 623 | } | ||
| 624 | |||
| 625 | 1307 | bool isApprox_impl( | |
| 626 | const InertiaTpl & other, | ||
| 627 | const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | ||
| 628 | { | ||
| 629 | using math::fabs; | ||
| 630 | 1307 | return fabs(static_cast<Scalar>(mass() - other.mass())) <= prec | |
| 631 |
5/10✓ Branch 0 taken 1303 times.
✗ Branch 1 not taken.
✓ Branch 5 taken 1303 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1303 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 1303 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 1303 times.
✗ Branch 15 not taken.
|
1307 | && lever().isApprox(other.lever(), prec) && inertia().isApprox(other.inertia(), prec); |
| 632 | } | ||
| 633 | |||
| 634 | 2334 | bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | |
| 635 | { | ||
| 636 | using math::fabs; | ||
| 637 |
6/10✓ Branch 2 taken 392 times.
✓ Branch 3 taken 1942 times.
✓ Branch 6 taken 392 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 392 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 392 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 392 times.
✗ Branch 15 not taken.
|
2334 | return fabs(mass()) <= prec && lever().isZero(prec) && inertia().isZero(prec); |
| 638 | } | ||
| 639 | |||
| 640 | 100007 | InertiaTpl __plus__(const InertiaTpl & Yb) const | |
| 641 | { | ||
| 642 | /* Y_{a+b} = ( m_a+m_b, | ||
| 643 | * (m_a*c_a + m_b*c_b ) / (m_a + m_b), | ||
| 644 | * I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x ) | ||
| 645 | */ | ||
| 646 | |||
| 647 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
100007 | const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); |
| 648 | |||
| 649 |
0/6✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
100007 | const Scalar & mab = mass() + Yb.mass(); |
| 650 |
1/6✓ Branch 1 taken 100007 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
100007 | const Scalar mab_inv = Scalar(1) / math::max(mab, eps); |
| 651 |
2/4✓ Branch 3 taken 100007 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100007 times.
✗ Branch 7 not taken.
|
100007 | const Vector3 & AB = (lever() - Yb.lever()).eval(); |
| 652 | return InertiaTpl( | ||
| 653 | 200014 | mab, (mass() * lever() + Yb.mass() * Yb.lever()) * mab_inv, | |
| 654 | 100007 | inertia() + Yb.inertia() | |
| 655 |
9/39✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 100007 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 100007 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 100007 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 100007 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 100007 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 100007 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 100007 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 100007 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 100007 times.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
✗ Branch 41 not taken.
✗ Branch 42 not taken.
✗ Branch 44 not taken.
✗ Branch 45 not taken.
|
400028 | - (mass() * Yb.mass() * mab_inv) * typename Symmetric3::SkewSquare(AB)); |
| 656 | } | ||
| 657 | |||
| 658 | 105489 | InertiaTpl & __pequ__(const InertiaTpl & Yb) | |
| 659 | { | ||
| 660 |
4/8✓ Branch 0 taken 9 times.
✓ Branch 1 taken 89 times.
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
133 | static const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); |
| 661 | |||
| 662 | 105489 | const InertiaTpl & Ya = *this; | |
| 663 |
2/4✓ Branch 1 taken 98 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 98 times.
✗ Branch 6 not taken.
|
105489 | const Scalar & mab = mass() + Yb.mass(); |
| 664 |
3/6✓ Branch 1 taken 105427 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 98 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 98 times.
✗ Branch 8 not taken.
|
105622 | const Scalar mab_inv = Scalar(1) / math::max(mab, eps); |
| 665 |
2/4✓ Branch 3 taken 105427 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 105427 times.
✗ Branch 7 not taken.
|
105489 | const Vector3 & AB = (Ya.lever() - Yb.lever()).eval(); |
| 666 |
3/5✓ Branch 2 taken 98 times.
✓ Branch 3 taken 105329 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 98 times.
✗ Branch 7 not taken.
|
105489 | lever() *= (mass() * mab_inv); |
| 667 |
6/10✓ Branch 2 taken 98 times.
✓ Branch 3 taken 105329 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 98 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 105329 times.
✓ Branch 8 taken 98 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 98 times.
✗ Branch 13 not taken.
|
105489 | lever() += (Yb.mass() * mab_inv) * Yb.lever(); // c *= mab_inv; |
| 668 |
1/2✓ Branch 3 taken 105427 times.
✗ Branch 4 not taken.
|
105489 | inertia() += Yb.inertia(); |
| 669 |
7/13✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 105329 times.
✓ Branch 5 taken 98 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 105427 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 98 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 98 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 98 times.
✗ Branch 19 not taken.
|
105489 | inertia() -= (Ya.mass() * Yb.mass() * mab_inv) * typename Symmetric3::SkewSquare(AB); |
| 670 |
1/2✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
|
105489 | mass() = mab; |
| 671 | 105489 | return *this; | |
| 672 | 133 | } | |
| 673 | |||
| 674 | 1 | InertiaTpl __minus__(const InertiaTpl & Yb) const | |
| 675 | { | ||
| 676 | /* Y_{a} = ( m_{a+b}+m_b, | ||
| 677 | * (m_{a+b}*c_{a+b} - m_b*c_b ) / (m_a), | ||
| 678 | * I_{a+b} - I_b + (m_a*m_b)/(m_a+m_b) * AB_x * AB_x ) | ||
| 679 | */ | ||
| 680 | |||
| 681 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
1 | const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); |
| 682 | |||
| 683 |
0/6✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
1 | const Scalar ma = mass() - Yb.mass(); |
| 684 |
2/7✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
1 | assert(check_expression_if_real<Scalar>(ma >= Scalar(0))); |
| 685 | |||
| 686 |
1/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
1 | const Scalar ma_inv = Scalar(1) / math::max(ma, eps); |
| 687 |
5/19✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
|
1 | const Vector3 c_a((mass() * lever() - Yb.mass() * Yb.lever()) * ma_inv); |
| 688 | |||
| 689 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | const Vector3 AB = c_a - Yb.lever(); |
| 690 | |||
| 691 | return InertiaTpl( | ||
| 692 | ma, c_a, | ||
| 693 |
5/23✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
|
1 | inertia() - Yb.inertia() + (ma * Yb.mass() / mass()) * typename Symmetric3::SkewSquare(AB)); |
| 694 | } | ||
| 695 | |||
| 696 | 3 | InertiaTpl & __mequ__(const InertiaTpl & Yb) | |
| 697 | { | ||
| 698 | ✗ | static const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); | |
| 699 | |||
| 700 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
3 | const Scalar ma = mass() - Yb.mass(); |
| 701 |
2/7✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
3 | assert(check_expression_if_real<Scalar>(ma >= Scalar(0))); |
| 702 | |||
| 703 |
1/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
3 | const Scalar ma_inv = Scalar(1) / math::max(ma, eps); |
| 704 | |||
| 705 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
3 | lever() *= (mass() * ma_inv); |
| 706 |
3/14✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
|
3 | lever().noalias() -= (Yb.mass() * ma_inv) * Yb.lever(); |
| 707 | |||
| 708 |
2/4✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
|
3 | const Vector3 AB = lever() - Yb.lever(); |
| 709 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | inertia() -= Yb.inertia(); |
| 710 |
3/15✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
3 | inertia() += (ma * Yb.mass() / mass()) * typename Symmetric3::SkewSquare(AB); |
| 711 |
0/2✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
3 | mass() = ma; |
| 712 | |||
| 713 | 3 | return *this; | |
| 714 | } | ||
| 715 | |||
| 716 | template<typename MotionDerived> | ||
| 717 | ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options> | ||
| 718 | 167614 | __mult__(const MotionDense<MotionDerived> & v) const | |
| 719 | { | ||
| 720 | typedef ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options> | ||
| 721 | ReturnType; | ||
| 722 | 167614 | ReturnType f; | |
| 723 |
1/2✓ Branch 1 taken 492 times.
✗ Branch 2 not taken.
|
167614 | __mult__(v, f); |
| 724 | 167614 | return f; | |
| 725 | } | ||
| 726 | |||
| 727 | template<typename MotionDerived, typename ForceDerived> | ||
| 728 | 351182 | void __mult__(const MotionDense<MotionDerived> & v, ForceDense<ForceDerived> & f) const | |
| 729 | { | ||
| 730 |
12/20✓ Branch 3 taken 248608 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 248608 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 248608 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 886 times.
✓ Branch 13 taken 247722 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 886 times.
✓ Branch 16 taken 247722 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 886 times.
✓ Branch 19 taken 247722 times.
✗ Branch 20 not taken.
✓ Branch 21 taken 886 times.
✓ Branch 22 taken 247722 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 886 times.
✗ Branch 25 not taken.
|
351182 | f.linear().noalias() = mass() * (v.linear() - lever().cross(v.angular())); |
| 731 |
2/4✓ Branch 2 taken 248608 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 248608 times.
✗ Branch 7 not taken.
|
351182 | Symmetric3::rhsMult(inertia(), v.angular(), f.angular()); |
| 732 |
3/6✓ Branch 3 taken 248608 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 248608 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 248608 times.
✗ Branch 10 not taken.
|
351182 | f.angular() += lever().cross(f.linear()); |
| 733 | // f.angular().noalias() = c.cross(f.linear()) + I*v.angular(); | ||
| 734 | 351182 | } | |
| 735 | |||
| 736 | template<typename MotionDerived> | ||
| 737 | 7523 | Scalar vtiv_impl(const MotionDense<MotionDerived> & v) const | |
| 738 | { | ||
| 739 |
2/4✓ Branch 2 taken 7523 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7523 times.
✗ Branch 6 not taken.
|
7523 | const Vector3 cxw(lever().cross(v.angular())); |
| 740 |
13/22✓ Branch 1 taken 1 times.
✓ Branch 2 taken 7522 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 7522 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
✓ Branch 8 taken 7522 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✓ Branch 11 taken 7522 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
|
7524 | Scalar res = mass() * (v.linear().squaredNorm() - Scalar(2) * v.linear().dot(cxw)); |
| 741 |
7/12✓ Branch 2 taken 7523 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 7522 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 7522 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
|
7524 | const Vector3 mcxcxw(-mass() * lever().cross(cxw)); |
| 742 |
3/6✓ Branch 1 taken 7523 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7523 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
7523 | res += v.angular().dot(mcxcxw); |
| 743 |
4/8✓ Branch 2 taken 7523 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7523 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7523 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
|
7523 | res += inertia().vtiv(v.angular()); |
| 744 | |||
| 745 | 7524 | return res; | |
| 746 | 1 | } | |
| 747 | |||
| 748 | template<typename MotionDerived> | ||
| 749 | 15291 | Matrix6 variation(const MotionDense<MotionDerived> & v) const | |
| 750 | { | ||
| 751 |
1/2✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
|
15291 | Matrix6 res; |
| 752 |
3/5✓ Branch 1 taken 27 times.
✓ Branch 2 taken 14900 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
15291 | const Motion mv(v * mass()); |
| 753 | |||
| 754 |
8/16✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14927 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14927 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 14927 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 14927 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 14927 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 14927 times.
✗ Branch 23 not taken.
|
45873 | res.template block<3, 3>(LINEAR, ANGULAR) = |
| 755 |
3/6✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 14927 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 14927 times.
✗ Branch 10 not taken.
|
45900 | -skew(mv.linear()) - skewSquare(mv.angular(), lever()) + skewSquare(lever(), mv.angular()); |
| 756 |
2/4✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
|
15291 | res.template block<3, 3>(ANGULAR, LINEAR) = |
| 757 |
2/4✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
|
15291 | res.template block<3, 3>(LINEAR, ANGULAR).transpose(); |
| 758 | |||
| 759 | // res.template block<3,3>(LINEAR,LINEAR) = mv.linear()*c.transpose(); // use as | ||
| 760 | // temporary variable res.template block<3,3>(ANGULAR,ANGULAR) = res.template | ||
| 761 | // block<3,3>(LINEAR,LINEAR) - res.template block<3,3>(LINEAR,LINEAR).transpose(); | ||
| 762 |
6/12✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14927 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14927 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 14927 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 14927 times.
✗ Branch 17 not taken.
|
30582 | res.template block<3, 3>(ANGULAR, ANGULAR) = |
| 763 |
2/4✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 14927 times.
✗ Branch 7 not taken.
|
30609 | -skewSquare(mv.linear(), lever()) - skewSquare(lever(), mv.linear()); |
| 764 | |||
| 765 |
4/8✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14927 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14927 times.
✗ Branch 11 not taken.
|
15291 | res.template block<3, 3>(LINEAR, LINEAR) = |
| 766 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
15318 | (inertia() - AlphaSkewSquare(mass(), lever())).matrix(); |
| 767 | |||
| 768 |
2/4✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
|
15291 | res.template block<3, 3>(ANGULAR, ANGULAR) -= |
| 769 |
4/8✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14927 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14927 times.
✗ Branch 11 not taken.
|
15318 | res.template block<3, 3>(LINEAR, LINEAR) * skew(v.angular()); |
| 770 |
3/6✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14927 times.
✗ Branch 8 not taken.
|
15291 | res.template block<3, 3>(ANGULAR, ANGULAR) += |
| 771 |
2/4✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
|
15291 | cross(v.angular(), res.template block<3, 3>(LINEAR, LINEAR)); |
| 772 | |||
| 773 |
2/4✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
|
15291 | res.template block<3, 3>(LINEAR, LINEAR).setZero(); |
| 774 | 30582 | return res; | |
| 775 | 27 | } | |
| 776 | |||
| 777 | template<typename MotionDerived, typename M6> | ||
| 778 | 131 | static void vxi_impl( | |
| 779 | const MotionDense<MotionDerived> & v, | ||
| 780 | const InertiaTpl & I, | ||
| 781 | const Eigen::MatrixBase<M6> & Iout) | ||
| 782 | { | ||
| 783 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6, 6, 6); | ||
| 784 | 131 | M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, Iout); | |
| 785 | |||
| 786 | // Block 1,1 | ||
| 787 |
3/9✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 131 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
131 | alphaSkew(I.mass(), v.angular(), Iout_.template block<3, 3>(LINEAR, LINEAR)); |
| 788 | // Iout_.template block<3,3>(LINEAR,LINEAR) = alphaSkew(I.mass(),v.angular()); | ||
| 789 |
2/8✗ Branch 2 not taken.
✓ Branch 3 taken 131 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 131 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
131 | const Vector3 mc(I.mass() * I.lever()); |
| 790 | |||
| 791 | // Block 1,2 | ||
| 792 |
4/8✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 131 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 131 times.
✗ Branch 11 not taken.
|
131 | skewSquare(-v.angular(), mc, Iout_.template block<3, 3>(LINEAR, ANGULAR)); |
| 793 | |||
| 794 | //// Block 2,1 | ||
| 795 |
3/9✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 131 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
131 | alphaSkew(I.mass(), v.linear(), Iout_.template block<3, 3>(ANGULAR, LINEAR)); |
| 796 |
3/6✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 131 times.
✗ Branch 8 not taken.
|
131 | Iout_.template block<3, 3>(ANGULAR, LINEAR) -= Iout_.template block<3, 3>(LINEAR, ANGULAR); |
| 797 | |||
| 798 | //// Block 2,2 | ||
| 799 |
4/8✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 131 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 131 times.
✗ Branch 11 not taken.
|
131 | skewSquare(-v.linear(), mc, Iout_.template block<3, 3>(ANGULAR, ANGULAR)); |
| 800 | |||
| 801 | // TODO: I do not why, but depending on the CPU, these three lines can give | ||
| 802 | // wrong output. | ||
| 803 | // typename Symmetric3::AlphaSkewSquare mcxcx(I.mass(),I.lever()); | ||
| 804 | // const Symmetric3 I_mcxcx(I.inertia() - mcxcx); | ||
| 805 | // Iout_.template block<3,3>(ANGULAR,ANGULAR) += I_mcxcx.vxs(v.angular()); | ||
| 806 |
1/6✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
131 | Symmetric3 mcxcx(typename Symmetric3::AlphaSkewSquare(I.mass(), I.lever())); |
| 807 |
4/8✓ Branch 2 taken 131 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 131 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 131 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 131 times.
✗ Branch 12 not taken.
|
131 | Iout_.template block<3, 3>(ANGULAR, ANGULAR) += I.inertia().vxs(v.angular()); |
| 808 |
4/8✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 131 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 131 times.
✗ Branch 11 not taken.
|
131 | Iout_.template block<3, 3>(ANGULAR, ANGULAR) -= mcxcx.vxs(v.angular()); |
| 809 | 131 | } | |
| 810 | |||
| 811 | template<typename MotionDerived, typename M6> | ||
| 812 | 3 | static void ivx_impl( | |
| 813 | const MotionDense<MotionDerived> & v, | ||
| 814 | const InertiaTpl & I, | ||
| 815 | const Eigen::MatrixBase<M6> & Iout) | ||
| 816 | { | ||
| 817 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6, 6, 6); | ||
| 818 | 3 | M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, Iout); | |
| 819 | |||
| 820 | // Block 1,1 | ||
| 821 |
3/9✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
3 | alphaSkew(I.mass(), v.angular(), Iout_.template block<3, 3>(LINEAR, LINEAR)); |
| 822 | |||
| 823 | // Block 2,1 | ||
| 824 |
2/8✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
3 | const Vector3 mc(I.mass() * I.lever()); |
| 825 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
|
3 | skewSquare(mc, v.angular(), Iout_.template block<3, 3>(ANGULAR, LINEAR)); |
| 826 | |||
| 827 | // Block 1,2 | ||
| 828 |
3/9✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
3 | alphaSkew(I.mass(), v.linear(), Iout_.template block<3, 3>(LINEAR, ANGULAR)); |
| 829 | |||
| 830 | // Block 2,2 | ||
| 831 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | cross( |
| 832 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | -I.lever(), Iout_.template block<3, 3>(ANGULAR, LINEAR), |
| 833 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | Iout_.template block<3, 3>(ANGULAR, ANGULAR)); |
| 834 |
4/8✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
|
3 | Iout_.template block<3, 3>(ANGULAR, ANGULAR) += I.inertia().svx(v.angular()); |
| 835 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 3 times.
|
12 | for (int k = 0; k < 3; ++k) |
| 836 |
4/8✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
|
9 | Iout_.template block<3, 3>(ANGULAR, ANGULAR).col(k) += |
| 837 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
18 | I.lever().cross(Iout_.template block<3, 3>(LINEAR, ANGULAR).col(k)); |
| 838 | |||
| 839 | // Block 1,2 | ||
| 840 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
|
3 | Iout_.template block<3, 3>(LINEAR, ANGULAR) -= Iout_.template block<3, 3>(ANGULAR, LINEAR); |
| 841 | 3 | } | |
| 842 | |||
| 843 | // Getters | ||
| 844 | 2340835 | Scalar mass() const | |
| 845 | { | ||
| 846 | 2340835 | return m_mass; | |
| 847 | } | ||
| 848 | 2282674 | const Vector3 & lever() const | |
| 849 | { | ||
| 850 | 2314805 | return m_com; | |
| 851 | } | ||
| 852 | 1505765 | const Symmetric3 & inertia() const | |
| 853 | { | ||
| 854 | 1535908 | return m_inertia; | |
| 855 | } | ||
| 856 | |||
| 857 | 328077 | Scalar & mass() | |
| 858 | { | ||
| 859 | 328077 | return m_mass; | |
| 860 | } | ||
| 861 | 226502 | Vector3 & lever() | |
| 862 | { | ||
| 863 | 226639 | return m_com; | |
| 864 | } | ||
| 865 | 217923 | Symmetric3 & inertia() | |
| 866 | { | ||
| 867 | 218058 | return m_inertia; | |
| 868 | } | ||
| 869 | |||
| 870 | /// aI = aXb.act(bI) | ||
| 871 | template<typename S2, int O2> | ||
| 872 | 106157 | InertiaTpl se3Action_impl(const SE3Tpl<S2, O2> & M) const | |
| 873 | { | ||
| 874 | /* The multiplication RIR' has a particular form that could be used, however it | ||
| 875 | * does not seems to be more efficient, see http://stackoverflow.m_comom/questions/ | ||
| 876 | * 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/ | ||
| 877 | return InertiaTpl( | ||
| 878 |
8/15✓ Branch 5 taken 106034 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 106034 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 106034 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 106034 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 106034 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 126 times.
✓ Branch 21 taken 105908 times.
✗ Branch 22 not taken.
✓ Branch 23 taken 126 times.
✗ Branch 24 not taken.
|
212125 | mass(), M.translation() + M.rotation() * lever(), inertia().rotate(M.rotation())); |
| 879 | } | ||
| 880 | |||
| 881 | /// bI = aXb.actInv(aI) | ||
| 882 | template<typename S2, int O2> | ||
| 883 | 9 | InertiaTpl se3ActionInverse_impl(const SE3Tpl<S2, O2> & M) const | |
| 884 | { | ||
| 885 | return InertiaTpl( | ||
| 886 |
3/6✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
|
27 | mass(), M.rotation().transpose() * (lever() - M.translation()), |
| 887 |
5/12✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
45 | inertia().rotate(M.rotation().transpose())); |
| 888 | } | ||
| 889 | |||
| 890 | template<typename MotionDerived> | ||
| 891 | 652 | Force vxiv(const MotionDense<MotionDerived> & v) const | |
| 892 | { | ||
| 893 |
4/12✓ Branch 2 taken 652 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 652 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 652 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 652 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
652 | const Vector3 & mcxw = mass() * lever().cross(v.angular()); |
| 894 |
4/13✓ Branch 1 taken 652 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 652 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 652 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 652 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
652 | const Vector3 & mv_mcxw = mass() * v.linear() - mcxw; |
| 895 | return Force( | ||
| 896 |
1/2✓ Branch 1 taken 652 times.
✗ Branch 2 not taken.
|
652 | v.angular().cross(mv_mcxw), |
| 897 |
2/4✓ Branch 1 taken 652 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 652 times.
✗ Branch 5 not taken.
|
1304 | v.angular().cross(lever().cross(mv_mcxw) + inertia() * v.angular()) |
| 898 |
9/18✓ Branch 1 taken 652 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 652 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 652 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 652 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 652 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 652 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 652 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 652 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 652 times.
✗ Branch 26 not taken.
|
2608 | - v.linear().cross(mcxw)); |
| 899 | } | ||
| 900 | |||
| 901 | 100009 | void disp_impl(std::ostream & os) const | |
| 902 | { | ||
| 903 |
0/2✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
100009 | os << " m = " << mass() << "\n" |
| 904 |
1/9✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 100009 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
|
100009 | << " c = " << lever().transpose() << "\n" |
| 905 |
2/4✓ Branch 1 taken 100009 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100009 times.
✗ Branch 5 not taken.
|
100009 | << " I = \n" |
| 906 |
3/6✓ Branch 2 taken 100009 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100009 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100009 times.
✗ Branch 9 not taken.
|
100009 | << inertia().matrix() << ""; |
| 907 | 100009 | } | |
| 908 | |||
| 909 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 910 | template<typename NewScalar> | ||
| 911 | 6978 | InertiaTpl<NewScalar, Options> cast() const | |
| 912 | { | ||
| 913 | return InertiaTpl<NewScalar, Options>( | ||
| 914 |
4/6✓ Branch 2 taken 4779 times.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4779 times.
✗ Branch 7 not taken.
|
8924 | pinocchio::cast<NewScalar>(mass()), lever().template cast<NewScalar>(), |
| 915 |
4/8✓ Branch 3 taken 6045 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1265 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1265 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1261 times.
✗ Branch 13 not taken.
|
21278 | inertia().template cast<NewScalar>()); |
| 916 | } | ||
| 917 | |||
| 918 | // TODO: adjust code | ||
| 919 | // /// \brief Check whether *this is a valid inertia, resulting from a positive mass | ||
| 920 | // distribution bool isValid() const | ||
| 921 | // { | ||
| 922 | // return | ||
| 923 | // (m_mass > Scalar(0) && m_inertia.isValid()) | ||
| 924 | // || (m_mass == Scalar(0) && (m_inertia.data().array() == Scalar(0)).all()); | ||
| 925 | // } | ||
| 926 | |||
| 927 | protected: | ||
| 928 | Scalar m_mass; | ||
| 929 | Vector3 m_com; | ||
| 930 | Symmetric3 m_inertia; | ||
| 931 | |||
| 932 | }; // class InertiaTpl | ||
| 933 | |||
| 934 | /** | ||
| 935 | * @brief A structure representing a pseudo inertia matrix. | ||
| 936 | * | ||
| 937 | * References: | ||
| 938 | * - Wensing, Patrick M., Sangbae Kim, and Jean-Jacques E. Slotine. "Linear matrix inequalities | ||
| 939 | * for physically consistent inertial parameter identification: A statistical perspective on the | ||
| 940 | * mass distribution." IEEE Robotics and Automation Letters 3.1 (2017): 60-67. | ||
| 941 | */ | ||
| 942 | template<typename _Scalar, int _Options> | ||
| 943 | struct PseudoInertiaTpl | ||
| 944 | { | ||
| 945 | typedef _Scalar Scalar; | ||
| 946 | enum | ||
| 947 | { | ||
| 948 | Options = _Options, | ||
| 949 | }; | ||
| 950 | typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4; | ||
| 951 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
| 952 | typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3; | ||
| 953 | typedef Eigen::Matrix<Scalar, 10, 1, Options> Vector10; | ||
| 954 | typedef LogCholeskyParametersTpl<Scalar, Options> LogCholeskyParameters; | ||
| 955 | |||
| 956 | Scalar mass; ///< Mass of the pseudo inertia | ||
| 957 | Vector3 h; ///< Vector part of the pseudo inertia | ||
| 958 | Matrix3 sigma; ///< 3x3 matrix part of the pseudo inertia | ||
| 959 | |||
| 960 | 10 | PseudoInertiaTpl(Scalar mass, const Vector3 & h, const Matrix3 & sigma) | |
| 961 | 10 | : mass(mass) | |
| 962 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
10 | , h(h) |
| 963 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
10 | , sigma(sigma) |
| 964 | { | ||
| 965 | 10 | } | |
| 966 | |||
| 967 | /** | ||
| 968 | * @brief Converts the PseudoInertiaTpl object to a 4x4 matrix. | ||
| 969 | * @return A 4x4 pseudo inertia matrix. | ||
| 970 | */ | ||
| 971 | 6 | Matrix4 toMatrix() const | |
| 972 | { | ||
| 973 |
1/2✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
|
6 | Matrix4 pseudo_inertia = Matrix4::Zero(); |
| 974 |
1/5✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
6 | pseudo_inertia.template block<3, 3>(0, 0) = sigma; |
| 975 |
1/5✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
6 | pseudo_inertia.template block<3, 1>(0, 3) = h; |
| 976 |
2/8✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
6 | pseudo_inertia.template block<1, 3>(3, 0) = h.transpose(); |
| 977 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
6 | pseudo_inertia(3, 3) = mass; |
| 978 | 6 | return pseudo_inertia; | |
| 979 | } | ||
| 980 | |||
| 981 | /** | ||
| 982 | * @brief Constructs a PseudoInertiaTpl object from a 4x4 pseudo inertia matrix. | ||
| 983 | * @param pseudo_inertia A 4x4 pseudo inertia matrix. | ||
| 984 | * @return A PseudoInertiaTpl object. | ||
| 985 | */ | ||
| 986 | 2 | static PseudoInertiaTpl FromMatrix(const Matrix4 & pseudo_inertia) | |
| 987 | { | ||
| 988 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | Scalar mass = pseudo_inertia(3, 3); |
| 989 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | Vector3 h = pseudo_inertia.template block<3, 1>(0, 3); |
| 990 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | Matrix3 sigma = pseudo_inertia.template block<3, 3>(0, 0); |
| 991 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
4 | return PseudoInertiaTpl(mass, h, sigma); |
| 992 | } | ||
| 993 | |||
| 994 | /** | ||
| 995 | * @brief Constructs a PseudoInertiaTpl object from dynamic parameters. | ||
| 996 | * @param dynamic_params A 10-dimensional vector of dynamic parameters. | ||
| 997 | * @return A PseudoInertiaTpl object. | ||
| 998 | */ | ||
| 999 | template<typename Vector10Like> | ||
| 1000 | static PseudoInertiaTpl | ||
| 1001 | 12 | FromDynamicParameters(const Eigen::MatrixBase<Vector10Like> & dynamic_params) | |
| 1002 | { | ||
| 1003 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
12 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, dynamic_params, 10, 1); |
| 1004 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
12 | Scalar mass = dynamic_params[0]; |
| 1005 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
12 | Vector3 h = dynamic_params.template segment<3>(1); |
| 1006 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
12 | Matrix3 I_bar; |
| 1007 | // clang-format off | ||
| 1008 |
6/12✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
|
24 | I_bar << dynamic_params[4], dynamic_params[5], dynamic_params[7], |
| 1009 |
6/12✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
|
12 | dynamic_params[5], dynamic_params[6], dynamic_params[8], |
| 1010 |
6/12✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
|
12 | dynamic_params[7], dynamic_params[8], dynamic_params[9]; |
| 1011 | // clang-format on | ||
| 1012 | |||
| 1013 |
5/14✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
12 | Matrix3 sigma = 0.5 * I_bar.trace() * Matrix3::Identity() - I_bar; |
| 1014 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
24 | return PseudoInertiaTpl(mass, h, sigma); |
| 1015 | } | ||
| 1016 | |||
| 1017 | /** | ||
| 1018 | * @brief Converts the PseudoInertiaTpl object to dynamic parameters. | ||
| 1019 | * @return A 10-dimensional vector of dynamic parameters. | ||
| 1020 | */ | ||
| 1021 | 4 | Vector10 toDynamicParameters() const | |
| 1022 | { | ||
| 1023 |
5/10✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
|
4 | Matrix3 I_bar = sigma.trace() * Matrix3::Identity() - sigma; |
| 1024 | |||
| 1025 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | Vector10 dynamic_params; |
| 1026 |
1/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
4 | dynamic_params[0] = mass; |
| 1027 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | dynamic_params.template segment<3>(1) = h; |
| 1028 |
2/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
4 | dynamic_params[4] = I_bar(0, 0); |
| 1029 |
2/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
4 | dynamic_params[5] = I_bar(0, 1); |
| 1030 |
2/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
4 | dynamic_params[6] = I_bar(1, 1); |
| 1031 |
2/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
4 | dynamic_params[7] = I_bar(0, 2); |
| 1032 |
2/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
4 | dynamic_params[8] = I_bar(1, 2); |
| 1033 |
2/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
4 | dynamic_params[9] = I_bar(2, 2); |
| 1034 | |||
| 1035 | 8 | return dynamic_params; | |
| 1036 | } | ||
| 1037 | |||
| 1038 | /** | ||
| 1039 | * @brief Constructs a PseudoInertiaTpl object from an InertiaTpl object. | ||
| 1040 | * @param inertia An InertiaTpl object. | ||
| 1041 | * @return A PseudoInertiaTpl object. | ||
| 1042 | */ | ||
| 1043 | 4 | static PseudoInertiaTpl FromInertia(const InertiaTpl<Scalar, Options> & inertia) | |
| 1044 | { | ||
| 1045 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | Vector10 dynamic_params = inertia.toDynamicParameters(); |
| 1046 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | return FromDynamicParameters(dynamic_params); |
| 1047 | } | ||
| 1048 | |||
| 1049 | /** | ||
| 1050 | * @brief Converts the PseudoInertiaTpl object to an InertiaTpl object. | ||
| 1051 | * @return An InertiaTpl object. | ||
| 1052 | */ | ||
| 1053 | 2 | InertiaTpl<Scalar, Options> toInertia() const | |
| 1054 | { | ||
| 1055 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | Vector10 dynamic_params = toDynamicParameters(); |
| 1056 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | return InertiaTpl<Scalar, Options>::FromDynamicParameters(dynamic_params); |
| 1057 | } | ||
| 1058 | |||
| 1059 | /** | ||
| 1060 | * @brief Constructs a PseudoInertiaTpl object from log Cholesky parameters. | ||
| 1061 | * @param log_cholesky A 10-dimensional vector of log Cholesky parameters. | ||
| 1062 | * @return A PseudoInertiaTpl object. | ||
| 1063 | */ | ||
| 1064 | static PseudoInertiaTpl FromLogCholeskyParameters(const LogCholeskyParameters & log_cholesky) | ||
| 1065 | { | ||
| 1066 | Vector10 dynamic_params = log_cholesky.toDynamicParameters(); | ||
| 1067 | return FromDynamicParameters(dynamic_params); | ||
| 1068 | } | ||
| 1069 | |||
| 1070 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 1071 | template<typename NewScalar> | ||
| 1072 | 1 | PseudoInertiaTpl<NewScalar, Options> cast() const | |
| 1073 | { | ||
| 1074 | return PseudoInertiaTpl<NewScalar, Options>( | ||
| 1075 | 2 | pinocchio::cast<NewScalar>(mass), h.template cast<NewScalar>(), | |
| 1076 |
0/10✗ 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.
|
1 | sigma.template cast<NewScalar>()); |
| 1077 | } | ||
| 1078 | |||
| 1079 | 1 | void disp_impl(std::ostream & os) const | |
| 1080 | { | ||
| 1081 | 1 | os << " m = " << mass << "\n" | |
| 1082 |
1/2✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | << " h = " << h.transpose() << "\n" |
| 1083 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | << " sigma = \n" |
| 1084 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | << sigma << ""; |
| 1085 | 1 | } | |
| 1086 | |||
| 1087 | 1 | friend std::ostream & operator<<(std::ostream & os, const PseudoInertiaTpl & pi) | |
| 1088 | { | ||
| 1089 | 1 | pi.disp_impl(os); | |
| 1090 | 1 | return os; | |
| 1091 | } | ||
| 1092 | }; | ||
| 1093 | |||
| 1094 | /** | ||
| 1095 | * @brief A structure representing log Cholesky parameters. | ||
| 1096 | * | ||
| 1097 | * References: | ||
| 1098 | * - Rucker, Caleb, and Patrick M. Wensing. "Smooth parameterization of rigid-body inertia." | ||
| 1099 | * IEEE Robotics and Automation Letters 7.2 (2022): 2771-2778. | ||
| 1100 | */ | ||
| 1101 | template<typename _Scalar, int _Options> | ||
| 1102 | struct LogCholeskyParametersTpl | ||
| 1103 | { | ||
| 1104 | typedef _Scalar Scalar; | ||
| 1105 | enum | ||
| 1106 | { | ||
| 1107 | Options = _Options, | ||
| 1108 | }; | ||
| 1109 | typedef Eigen::Matrix<Scalar, 10, 1, Options> Vector10; | ||
| 1110 | typedef Eigen::Matrix<Scalar, 10, 10, Options> Matrix10; | ||
| 1111 | typedef PseudoInertiaTpl<Scalar, Options> PseudoInertia; | ||
| 1112 | |||
| 1113 | Vector10 parameters; ///< 10-dimensional vector of log Cholesky parameters | ||
| 1114 | /** | ||
| 1115 | * @brief Constructor for LogCholeskyParametersTpl. | ||
| 1116 | * @param log_cholesky A 10-dimensional vector of log Cholesky parameters. | ||
| 1117 | */ | ||
| 1118 | 2 | explicit LogCholeskyParametersTpl(const Vector10 & log_cholesky) | |
| 1119 | 2 | : parameters(log_cholesky) | |
| 1120 | { | ||
| 1121 | 2 | } | |
| 1122 | /** | ||
| 1123 | * @brief Converts the LogCholeskyParametersTpl object to dynamic parameters. | ||
| 1124 | * @return A 10-dimensional vector of dynamic parameters. | ||
| 1125 | */ | ||
| 1126 | 7 | Vector10 toDynamicParameters() const | |
| 1127 | { | ||
| 1128 | using math::exp; | ||
| 1129 | using math::pow; | ||
| 1130 | |||
| 1131 | // clang-format off | ||
| 1132 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | Vector10 dynamic_params; |
| 1133 | |||
| 1134 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar alpha = parameters[0]; |
| 1135 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar d1 = parameters[1]; |
| 1136 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar d2 = parameters[2]; |
| 1137 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar d3 = parameters[3]; |
| 1138 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar s12 = parameters[4]; |
| 1139 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar s23 = parameters[5]; |
| 1140 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar s13 = parameters[6]; |
| 1141 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar t1 = parameters[7]; |
| 1142 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar t2 = parameters[8]; |
| 1143 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar t3 = parameters[9]; |
| 1144 | |||
| 1145 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
7 | const Scalar exp_d1 = exp(d1); |
| 1146 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
7 | const Scalar exp_d2 = exp(d2); |
| 1147 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
7 | const Scalar exp_d3 = exp(d3); |
| 1148 | |||
| 1149 |
1/6✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
7 | dynamic_params[0] = 1; |
| 1150 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | dynamic_params[1] = t1; |
| 1151 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | dynamic_params[2] = t2; |
| 1152 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | dynamic_params[3] = t3; |
| 1153 |
6/22✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
|
7 | dynamic_params[4] = pow(s23, 2) + pow(t2, 2) + pow(t3, 2) + pow(exp_d2, 2) + pow(exp_d3, 2); |
| 1154 |
1/16✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
|
7 | dynamic_params[5] = -s12 * exp_d2 - s13 * s23 - t1 * t2; |
| 1155 |
7/26✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
|
7 | dynamic_params[6] = pow(s12, 2) + pow(s13, 2) + pow(t1, 2) + pow(t3, 2) + pow(exp_d1, 2) + pow(exp_d3, 2); |
| 1156 |
1/12✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
7 | dynamic_params[7] = -s13 * exp_d3 - t1 * t3; |
| 1157 |
1/12✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
7 | dynamic_params[8] = -s23 * exp_d3 - t2 * t3; |
| 1158 |
8/30✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
|
7 | dynamic_params[9] = pow(s12, 2) + pow(s13, 2) + pow(s23, 2) + pow(t1, 2) + pow(t2, 2) + pow(exp_d1, 2) + pow(exp_d2, 2); |
| 1159 | |||
| 1160 |
0/6✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
7 | const Scalar exp_2_alpha = exp(2 * alpha); |
| 1161 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | dynamic_params *= exp_2_alpha; |
| 1162 | // clang-format on | ||
| 1163 | |||
| 1164 | 14 | return dynamic_params; | |
| 1165 | } | ||
| 1166 | |||
| 1167 | /** | ||
| 1168 | * @brief Converts the LogCholeskyParametersTpl object to a PseudoInertiaTpl object. | ||
| 1169 | * @return A PseudoInertiaTpl object. | ||
| 1170 | */ | ||
| 1171 | 2 | PseudoInertia toPseudoInertia() const | |
| 1172 | { | ||
| 1173 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | Vector10 dynamic_params = toDynamicParameters(); |
| 1174 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | return PseudoInertia::FromDynamicParameters(dynamic_params); |
| 1175 | } | ||
| 1176 | |||
| 1177 | /** | ||
| 1178 | * @brief Converts the LogCholeskyParametersTpl object to an InertiaTpl object. | ||
| 1179 | * @return An InertiaTpl object. | ||
| 1180 | */ | ||
| 1181 | 1 | InertiaTpl<Scalar, Options> toInertia() const | |
| 1182 | { | ||
| 1183 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Vector10 dynamic_params = toDynamicParameters(); |
| 1184 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | return InertiaTpl<Scalar, Options>::FromDynamicParameters(dynamic_params); |
| 1185 | } | ||
| 1186 | |||
| 1187 | /** | ||
| 1188 | * @brief Calculates the Jacobian of the log Cholesky parameters. | ||
| 1189 | * @return A 10x10 matrix representing the Jacobian. | ||
| 1190 | */ | ||
| 1191 | 2 | Matrix10 calculateJacobian() const | |
| 1192 | { | ||
| 1193 | using math::exp; | ||
| 1194 | using math::pow; | ||
| 1195 | |||
| 1196 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | Matrix10 jacobian = Matrix10::Zero(); |
| 1197 | |||
| 1198 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar alpha = parameters[0]; |
| 1199 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar d1 = parameters[1]; |
| 1200 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar d2 = parameters[2]; |
| 1201 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar d3 = parameters[3]; |
| 1202 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar s12 = parameters[4]; |
| 1203 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar s23 = parameters[5]; |
| 1204 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar s13 = parameters[6]; |
| 1205 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar t1 = parameters[7]; |
| 1206 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar t2 = parameters[8]; |
| 1207 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar t3 = parameters[9]; |
| 1208 | |||
| 1209 |
0/6✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
2 | const Scalar exp_2alpha = exp(2 * alpha); |
| 1210 |
0/6✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
2 | const Scalar exp_2d1 = exp(2 * d1); |
| 1211 |
0/6✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
2 | const Scalar exp_2d2 = exp(2 * d2); |
| 1212 |
0/6✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
2 | const Scalar exp_2d3 = exp(2 * d3); |
| 1213 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
2 | const Scalar exp_d1 = exp(d1); |
| 1214 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
2 | const Scalar exp_d2 = exp(d2); |
| 1215 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
2 | const Scalar exp_d3 = exp(d3); |
| 1216 | |||
| 1217 | // clang-format off | ||
| 1218 |
1/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | jacobian(0, 0) = 2 * exp_2alpha; |
| 1219 | |||
| 1220 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(1, 0) = 2 * t1 * exp_2alpha; |
| 1221 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | jacobian(1, 7) = exp_2alpha; |
| 1222 | |||
| 1223 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(2, 0) = 2 * t2 * exp_2alpha; |
| 1224 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | jacobian(2, 8) = exp_2alpha; |
| 1225 | |||
| 1226 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(3, 0) = 2 * t3 * exp_2alpha; |
| 1227 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | jacobian(3, 9) = exp_2alpha; |
| 1228 | |||
| 1229 |
4/24✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
|
2 | jacobian(4, 0) = 2 * (pow(s23, 2) + pow(t2, 2) + pow(t3, 2) + exp_2d2 + exp_2d3) * exp_2alpha; |
| 1230 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(4, 2) = 2 * exp_2alpha * exp_2d2; |
| 1231 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(4, 3) = 2 * exp_2alpha * exp_2d3; |
| 1232 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(4, 5) = 2 * s23 * exp_2alpha; |
| 1233 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(4, 8) = 2 * t2 * exp_2alpha; |
| 1234 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(4, 9) = 2 * t3 * exp_2alpha; |
| 1235 | |||
| 1236 |
1/20✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
|
2 | jacobian(5, 0) = -2 * (s12 * exp_d2 + s13 * s23 + t1 * t2) * exp_2alpha; |
| 1237 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(5, 2) = -s12 * exp_2alpha * exp_d2; |
| 1238 |
1/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | jacobian(5, 4) = -exp_2alpha * exp_d2; |
| 1239 |
1/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | jacobian(5, 5) = -s13 * exp_2alpha; |
| 1240 |
1/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | jacobian(5, 6) = -s23 * exp_2alpha; |
| 1241 |
1/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | jacobian(5, 7) = -t2 * exp_2alpha; |
| 1242 |
1/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | jacobian(5, 8) = -t1 * exp_2alpha; |
| 1243 | |||
| 1244 |
5/28✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
|
2 | jacobian(6, 0) = 2 * (pow(s12, 2) + pow(s13, 2) + pow(t1, 2) + pow(t3, 2) + exp_2d1 + exp_2d3) * exp_2alpha; |
| 1245 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(6, 1) = 2 * exp_2alpha * exp_2d1; |
| 1246 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(6, 3) = 2 * exp_2alpha * exp_2d3; |
| 1247 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(6, 4) = 2 * s12 * exp_2alpha; |
| 1248 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(6, 6) = 2 * s13 * exp_2alpha; |
| 1249 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(6, 7) = 2 * t1 * exp_2alpha; |
| 1250 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(6, 9) = 2 * t3 * exp_2alpha; |
| 1251 | |||
| 1252 |
1/16✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
|
2 | jacobian(7, 0) = -2 * (s13 * exp_d3 + t1 * t3) * exp_2alpha; |
| 1253 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(7, 3) = -s13 * exp_2alpha * exp_d3; |
| 1254 |
1/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | jacobian(7, 6) = -exp_2alpha * exp_d3; |
| 1255 |
1/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | jacobian(7, 7) = -t3 * exp_2alpha; |
| 1256 |
1/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | jacobian(7, 9) = -t1 * exp_2alpha; |
| 1257 | |||
| 1258 |
1/16✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
|
2 | jacobian(8, 0) = -2 * (s23 * exp_d3 + t2 * t3) * exp_2alpha; |
| 1259 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(8, 3) = -s23 * exp_2alpha * exp_d3; |
| 1260 |
1/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | jacobian(8, 5) = -exp_2alpha * exp_d3; |
| 1261 |
1/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | jacobian(8, 8) = -t3 * exp_2alpha; |
| 1262 |
1/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | jacobian(8, 9) = -t2 * exp_2alpha; |
| 1263 | |||
| 1264 |
6/32✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
|
2 | jacobian(9, 0) = 2 * (pow(s12, 2) + pow(s13, 2) + pow(s23, 2) + pow(t1, 2) + pow(t2, 2) + exp_2d1 + exp_2d2) * exp_2alpha; |
| 1265 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(9, 1) = 2 * exp_2alpha * exp_2d1; |
| 1266 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(9, 2) = 2 * exp_2alpha * exp_2d2; |
| 1267 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(9, 4) = 2 * s12 * exp_2alpha; |
| 1268 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(9, 5) = 2 * s23 * exp_2alpha; |
| 1269 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(9, 6) = 2 * s13 * exp_2alpha; |
| 1270 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(9, 7) = 2 * t1 * exp_2alpha; |
| 1271 |
1/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | jacobian(9, 8) = 2 * t2 * exp_2alpha; |
| 1272 | // clang-format on | ||
| 1273 | |||
| 1274 | 4 | return jacobian; | |
| 1275 | } | ||
| 1276 | |||
| 1277 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 1278 | template<typename NewScalar> | ||
| 1279 | ✗ | LogCholeskyParametersTpl<NewScalar, Options> cast() const | |
| 1280 | { | ||
| 1281 | ✗ | return LogCholeskyParametersTpl<NewScalar, Options>(parameters.template cast<NewScalar>()); | |
| 1282 | } | ||
| 1283 | |||
| 1284 | 1 | void disp_impl(std::ostream & os) const | |
| 1285 | { | ||
| 1286 | 1 | os << " alpha: " << parameters[0] << "\n" | |
| 1287 | 1 | << " d1: " << parameters[1] << "\n" | |
| 1288 | 1 | << " d2: " << parameters[2] << "\n" | |
| 1289 | 1 | << " d3: " << parameters[3] << "\n" | |
| 1290 | 1 | << " s12: " << parameters[4] << "\n" | |
| 1291 | 1 | << " s23: " << parameters[5] << "\n" | |
| 1292 | 1 | << " s13: " << parameters[6] << "\n" | |
| 1293 | 1 | << " t1: " << parameters[7] << "\n" | |
| 1294 | 1 | << " t2: " << parameters[8] << "\n" | |
| 1295 | 1 | << " t3: " << parameters[9] << ""; | |
| 1296 | 1 | } | |
| 1297 | |||
| 1298 | 1 | friend std::ostream & operator<<(std::ostream & os, const LogCholeskyParametersTpl & lcp) | |
| 1299 | { | ||
| 1300 | 1 | lcp.disp_impl(os); | |
| 1301 | 1 | return os; | |
| 1302 | } | ||
| 1303 | }; | ||
| 1304 | |||
| 1305 | } // namespace pinocchio | ||
| 1306 | |||
| 1307 | #endif // ifndef __pinocchio_spatial_inertia_hpp__ | ||
| 1308 |