| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/joint/joint-free-flyer.hpp |
| Date: | 2025-04-30 16:14:33 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 81 | 81 | 100.0% |
| Branches: | 45 | 83 | 54.2% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2020 CNRS INRIA | ||
| 3 | // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
| 4 | // | ||
| 5 | |||
| 6 | #ifndef __pinocchio_multibody_joint_free_flyer_hpp__ | ||
| 7 | #define __pinocchio_multibody_joint_free_flyer_hpp__ | ||
| 8 | |||
| 9 | #include "pinocchio/macros.hpp" | ||
| 10 | #include "pinocchio/spatial/inertia.hpp" | ||
| 11 | #include "pinocchio/spatial/explog.hpp" | ||
| 12 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
| 13 | #include "pinocchio/multibody/joint-motion-subspace.hpp" | ||
| 14 | #include "pinocchio/math/fwd.hpp" | ||
| 15 | #include "pinocchio/math/quaternion.hpp" | ||
| 16 | |||
| 17 | namespace pinocchio | ||
| 18 | { | ||
| 19 | |||
| 20 | template<typename Scalar, int Options> | ||
| 21 | struct JointMotionSubspaceIdentityTpl; | ||
| 22 | |||
| 23 | template<typename _Scalar, int _Options> | ||
| 24 | struct traits<JointMotionSubspaceIdentityTpl<_Scalar, _Options>> | ||
| 25 | { | ||
| 26 | typedef _Scalar Scalar; | ||
| 27 | enum | ||
| 28 | { | ||
| 29 | Options = _Options | ||
| 30 | }; | ||
| 31 | typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6; | ||
| 32 | enum | ||
| 33 | { | ||
| 34 | LINEAR = 0, | ||
| 35 | ANGULAR = 3 | ||
| 36 | }; | ||
| 37 | typedef MotionTpl<Scalar, Options> JointMotion; | ||
| 38 | typedef Eigen::Matrix<Scalar, 6, 1, Options> JointForce; | ||
| 39 | typedef Eigen::Matrix<Scalar, 6, 6, Options> DenseBase; | ||
| 40 | typedef Eigen::Matrix<Scalar, 6, 6, Options> ReducedSquaredMatrix; | ||
| 41 | |||
| 42 | typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType; | ||
| 43 | typedef typename Matrix6::IdentityReturnType MatrixReturnType; | ||
| 44 | typedef typename Matrix6::IdentityReturnType StDiagonalMatrixSOperationReturnType; | ||
| 45 | }; // traits ConstraintRevolute | ||
| 46 | |||
| 47 | template<typename _Scalar, int _Options> | ||
| 48 | struct JointMotionSubspaceIdentityTpl | ||
| 49 | : JointMotionSubspaceBase<JointMotionSubspaceIdentityTpl<_Scalar, _Options>> | ||
| 50 | { | ||
| 51 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 52 | PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceIdentityTpl) | ||
| 53 | |||
| 54 | enum | ||
| 55 | { | ||
| 56 | NV = 6 | ||
| 57 | }; | ||
| 58 | |||
| 59 | template<typename Vector6Like> | ||
| 60 | JointMotion __mult__(const Eigen::MatrixBase<Vector6Like> & vj) const | ||
| 61 | { | ||
| 62 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6); | ||
| 63 | return JointMotion(vj); | ||
| 64 | } | ||
| 65 | |||
| 66 | template<typename S1, int O1> | ||
| 67 | 3921 | typename SE3Tpl<S1, O1>::ActionMatrixType se3Action(const SE3Tpl<S1, O1> & m) const | |
| 68 | { | ||
| 69 | 3921 | return m.toActionMatrix(); | |
| 70 | } | ||
| 71 | |||
| 72 | template<typename S1, int O1> | ||
| 73 | 373 | typename SE3Tpl<S1, O1>::ActionMatrixType se3ActionInverse(const SE3Tpl<S1, O1> & m) const | |
| 74 | { | ||
| 75 | 373 | return m.toActionMatrixInverse(); | |
| 76 | } | ||
| 77 | |||
| 78 | 33 | int nv_impl() const | |
| 79 | { | ||
| 80 | 33 | return NV; | |
| 81 | } | ||
| 82 | |||
| 83 | struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceIdentityTpl> | ||
| 84 | { | ||
| 85 | template<typename Derived> | ||
| 86 | typename ForceDense<Derived>::ToVectorConstReturnType | ||
| 87 | 1314 | operator*(const ForceDense<Derived> & phi) | |
| 88 | { | ||
| 89 | 1314 | return phi.toVector(); | |
| 90 | } | ||
| 91 | |||
| 92 | /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ | ||
| 93 | template<typename MatrixDerived> | ||
| 94 | typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived) | ||
| 95 | 114 | operator*(const Eigen::MatrixBase<MatrixDerived> & F) | |
| 96 | { | ||
| 97 | 114 | return F.derived(); | |
| 98 | } | ||
| 99 | }; | ||
| 100 | |||
| 101 | 1429 | TransposeConst transpose() const | |
| 102 | { | ||
| 103 | 1429 | return TransposeConst(); | |
| 104 | } | ||
| 105 | 38 | MatrixReturnType matrix_impl() const | |
| 106 | { | ||
| 107 | 38 | return DenseBase::Identity(); | |
| 108 | } | ||
| 109 | |||
| 110 | template<typename MotionDerived> | ||
| 111 | 5 | typename MotionDerived::ActionMatrixType motionAction(const MotionBase<MotionDerived> & v) const | |
| 112 | { | ||
| 113 | 5 | return v.toActionMatrix(); | |
| 114 | } | ||
| 115 | |||
| 116 | 28 | bool isEqual(const JointMotionSubspaceIdentityTpl &) const | |
| 117 | { | ||
| 118 | 28 | return true; | |
| 119 | } | ||
| 120 | |||
| 121 | }; // struct JointMotionSubspaceIdentityTpl | ||
| 122 | |||
| 123 | template<typename Scalar, int Options, typename Vector6Like> | ||
| 124 | 2177 | MotionRef<const Vector6Like> operator*( | |
| 125 | const JointMotionSubspaceIdentityTpl<Scalar, Options> &, | ||
| 126 | const Eigen::MatrixBase<Vector6Like> & v) | ||
| 127 | { | ||
| 128 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6); | ||
| 129 | // typedef typename JointMotionSubspaceIdentityTpl<Scalar,Options>::Motion Motion; | ||
| 130 | typedef MotionRef<const Vector6Like> Motion; | ||
| 131 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
2177 | return Motion(v.derived()); |
| 132 | } | ||
| 133 | |||
| 134 | /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ | ||
| 135 | template<typename S1, int O1, typename S2, int O2> | ||
| 136 | inline typename InertiaTpl<S1, O1>::Matrix6 | ||
| 137 | 44 | operator*(const InertiaTpl<S1, O1> & Y, const JointMotionSubspaceIdentityTpl<S2, O2> &) | |
| 138 | { | ||
| 139 | 44 | return Y.matrix(); | |
| 140 | } | ||
| 141 | |||
| 142 | /* [ABA] Y*S operator*/ | ||
| 143 | template<typename Matrix6Like, typename S2, int O2> | ||
| 144 | 2 | inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*( | |
| 145 | const Eigen::MatrixBase<Matrix6Like> & Y, const JointMotionSubspaceIdentityTpl<S2, O2> &) | ||
| 146 | { | ||
| 147 | 2 | return Y.derived(); | |
| 148 | } | ||
| 149 | |||
| 150 | template<typename S1, int O1> | ||
| 151 | struct SE3GroupAction<JointMotionSubspaceIdentityTpl<S1, O1>> | ||
| 152 | { | ||
| 153 | typedef typename SE3Tpl<S1, O1>::ActionMatrixType ReturnType; | ||
| 154 | }; | ||
| 155 | |||
| 156 | template<typename S1, int O1, typename MotionDerived> | ||
| 157 | struct MotionAlgebraAction<JointMotionSubspaceIdentityTpl<S1, O1>, MotionDerived> | ||
| 158 | { | ||
| 159 | typedef typename SE3Tpl<S1, O1>::ActionMatrixType ReturnType; | ||
| 160 | }; | ||
| 161 | |||
| 162 | template<typename Scalar, int Options> | ||
| 163 | struct JointFreeFlyerTpl; | ||
| 164 | |||
| 165 | template<typename _Scalar, int _Options> | ||
| 166 | struct traits<JointFreeFlyerTpl<_Scalar, _Options>> | ||
| 167 | { | ||
| 168 | enum | ||
| 169 | { | ||
| 170 | NQ = 7, | ||
| 171 | NV = 6, | ||
| 172 | NVExtended = 6 | ||
| 173 | }; | ||
| 174 | typedef _Scalar Scalar; | ||
| 175 | enum | ||
| 176 | { | ||
| 177 | Options = _Options | ||
| 178 | }; | ||
| 179 | typedef JointDataFreeFlyerTpl<Scalar, Options> JointDataDerived; | ||
| 180 | typedef JointModelFreeFlyerTpl<Scalar, Options> JointModelDerived; | ||
| 181 | typedef JointMotionSubspaceIdentityTpl<Scalar, Options> Constraint_t; | ||
| 182 | typedef SE3Tpl<Scalar, Options> Transformation_t; | ||
| 183 | typedef MotionTpl<Scalar, Options> Motion_t; | ||
| 184 | typedef MotionZeroTpl<Scalar, Options> Bias_t; | ||
| 185 | |||
| 186 | // [ABA] | ||
| 187 | typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t; | ||
| 188 | typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t; | ||
| 189 | typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t; | ||
| 190 | |||
| 191 | typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t; | ||
| 192 | typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t; | ||
| 193 | |||
| 194 | typedef boost::mpl::false_ is_mimicable_t; | ||
| 195 | |||
| 196 | PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE | ||
| 197 | }; | ||
| 198 | |||
| 199 | template<typename _Scalar, int _Options> | ||
| 200 | struct traits<JointDataFreeFlyerTpl<_Scalar, _Options>> | ||
| 201 | { | ||
| 202 | typedef JointFreeFlyerTpl<_Scalar, _Options> JointDerived; | ||
| 203 | typedef _Scalar Scalar; | ||
| 204 | }; | ||
| 205 | |||
| 206 | template<typename _Scalar, int _Options> | ||
| 207 | struct traits<JointModelFreeFlyerTpl<_Scalar, _Options>> | ||
| 208 | { | ||
| 209 | typedef JointFreeFlyerTpl<_Scalar, _Options> JointDerived; | ||
| 210 | typedef _Scalar Scalar; | ||
| 211 | }; | ||
| 212 | |||
| 213 | template<typename _Scalar, int _Options> | ||
| 214 | struct JointDataFreeFlyerTpl : public JointDataBase<JointDataFreeFlyerTpl<_Scalar, _Options>> | ||
| 215 | { | ||
| 216 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 217 | typedef JointFreeFlyerTpl<_Scalar, _Options> JointDerived; | ||
| 218 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
| 219 | 97434 | PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR | |
| 220 | |||
| 221 | ConfigVector_t joint_q; | ||
| 222 | TangentVector_t joint_v; | ||
| 223 | |||
| 224 | Constraint_t S; | ||
| 225 | Transformation_t M; | ||
| 226 | Motion_t v; | ||
| 227 | Bias_t c; | ||
| 228 | |||
| 229 | // [ABA] specific data | ||
| 230 | U_t U; | ||
| 231 | D_t Dinv; | ||
| 232 | UD_t UDinv; | ||
| 233 | D_t StU; | ||
| 234 | |||
| 235 | 5340 | JointDataFreeFlyerTpl() | |
| 236 |
1/2✓ Branch 2 taken 5338 times.
✗ Branch 3 not taken.
|
5340 | : joint_q(ConfigVector_t::Zero()) |
| 237 |
3/5✓ Branch 1 taken 20 times.
✓ Branch 2 taken 5318 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
|
5340 | , joint_v(TangentVector_t::Zero()) |
| 238 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
5340 | , M(Transformation_t::Identity()) |
| 239 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
5340 | , v(Motion_t::Zero()) |
| 240 |
3/5✓ Branch 1 taken 20 times.
✓ Branch 2 taken 5318 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
|
5340 | , U(U_t::Zero()) |
| 241 |
3/5✓ Branch 1 taken 20 times.
✓ Branch 2 taken 5318 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
|
5340 | , Dinv(D_t::Zero()) |
| 242 |
3/5✓ Branch 1 taken 20 times.
✓ Branch 2 taken 5318 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
|
5340 | , UDinv(UD_t::Identity()) |
| 243 |
3/5✓ Branch 2 taken 20 times.
✓ Branch 3 taken 5318 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
|
10680 | , StU(D_t::Zero()) |
| 244 | { | ||
| 245 |
3/6✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
|
5340 | joint_q[6] = Scalar(1); |
| 246 | 5340 | } | |
| 247 | |||
| 248 | 150 | static std::string classname() | |
| 249 | { | ||
| 250 |
1/2✓ Branch 2 taken 150 times.
✗ Branch 3 not taken.
|
150 | return std::string("JointDataFreeFlyer"); |
| 251 | } | ||
| 252 | 3 | std::string shortname() const | |
| 253 | { | ||
| 254 | 3 | return classname(); | |
| 255 | } | ||
| 256 | |||
| 257 | }; // struct JointDataFreeFlyerTpl | ||
| 258 | |||
| 259 | /// @brief Free-flyer joint in \f$SE(3)\f$. | ||
| 260 | /// | ||
| 261 | /// A free-flyer joint adds seven coordinates to the configuration space. | ||
| 262 | /// Given a configuration vector `q`: | ||
| 263 | /// | ||
| 264 | /// - `q[idx_q:idx_q + 3]` are the translation coordinates, in meters, | ||
| 265 | /// representing the position of the child frame in the parent frame. | ||
| 266 | /// - `q[idx_q + 3:idx_q + 7]` is a unit quaternion representing the rotation | ||
| 267 | /// from the child frame to the parent frame, with quaternion coordinates | ||
| 268 | /// ordered as (x, y, z, w). | ||
| 269 | /// | ||
| 270 | /// Likewise, a free-flyer joint adds six coordinates to the tangent space. | ||
| 271 | /// Let's consider a tangent vector `v`, say, a velocity vector. Following | ||
| 272 | /// Featherstone's convention, all our tangent vectors are body rather than | ||
| 273 | /// spatial vectors: | ||
| 274 | /// | ||
| 275 | /// - `v[idx_v:idx_v + 3]` is the linear velocity, in meters / second, | ||
| 276 | /// corresponding to the linear velocity of the child frame with respect | ||
| 277 | /// to the parent frame, expressed in the child frame (body linear velocity | ||
| 278 | /// of the child frame). | ||
| 279 | /// - `v[idx_v + 3:idx_v + 6]` is the angular velocity, in radians / second, | ||
| 280 | /// corresponding to the angular velocity from the child frame to the | ||
| 281 | /// parent frame, expressed in the child frame (body angular velocity of the | ||
| 282 | /// child frame). | ||
| 283 | PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl); | ||
| 284 | template<typename _Scalar, int _Options> | ||
| 285 | struct JointModelFreeFlyerTpl : public JointModelBase<JointModelFreeFlyerTpl<_Scalar, _Options>> | ||
| 286 | { | ||
| 287 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 288 | typedef JointFreeFlyerTpl<_Scalar, _Options> JointDerived; | ||
| 289 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
| 290 | |||
| 291 | typedef JointModelBase<JointModelFreeFlyerTpl> Base; | ||
| 292 | using Base::id; | ||
| 293 | using Base::idx_q; | ||
| 294 | using Base::idx_v; | ||
| 295 | using Base::idx_vExtended; | ||
| 296 | using Base::setIndexes; | ||
| 297 | |||
| 298 | 5253 | JointDataDerived createData() const | |
| 299 | { | ||
| 300 | 5253 | return JointDataDerived(); | |
| 301 | } | ||
| 302 | |||
| 303 | 2 | const std::vector<bool> hasConfigurationLimit() const | |
| 304 | { | ||
| 305 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | return {true, true, true, false, false, false, false}; |
| 306 | } | ||
| 307 | |||
| 308 | 2 | const std::vector<bool> hasConfigurationLimitInTangent() const | |
| 309 | { | ||
| 310 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | return {true, true, true, false, false, false}; |
| 311 | } | ||
| 312 | |||
| 313 | template<typename ConfigVectorLike> | ||
| 314 | inline void forwardKinematics( | ||
| 315 | Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const | ||
| 316 | { | ||
| 317 | EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t, ConfigVectorLike); | ||
| 318 | typedef typename Eigen::Quaternion< | ||
| 319 | typename ConfigVectorLike::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> | ||
| 320 | Quaternion; | ||
| 321 | typedef Eigen::Map<const Quaternion> ConstQuaternionMap; | ||
| 322 | |||
| 323 | ConstQuaternionMap quat(q_joint.template tail<4>().data()); | ||
| 324 | // assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename | ||
| 325 | // V::Scalar>::epsilon())); TODO: check validity of the rhs precision | ||
| 326 | assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm() - 1)) <= 1e-4); | ||
| 327 | |||
| 328 | M.rotation(quat.matrix()); | ||
| 329 | M.translation(q_joint.template head<3>()); | ||
| 330 | } | ||
| 331 | |||
| 332 | template<typename Vector3Derived, typename QuaternionDerived> | ||
| 333 | 31927 | PINOCCHIO_DONT_INLINE void calc( | |
| 334 | JointDataDerived & data, | ||
| 335 | const typename Eigen::MatrixBase<Vector3Derived> & trans, | ||
| 336 | const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const | ||
| 337 | { | ||
| 338 |
1/2✓ Branch 2 taken 31911 times.
✗ Branch 3 not taken.
|
31927 | data.M.translation(trans); |
| 339 |
1/2✓ Branch 2 taken 31911 times.
✗ Branch 3 not taken.
|
31927 | data.M.rotation(quat.matrix()); |
| 340 | 31927 | } | |
| 341 | |||
| 342 | template<typename ConfigVector> | ||
| 343 | PINOCCHIO_DONT_INLINE void | ||
| 344 | 57588 | calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const | |
| 345 | { | ||
| 346 | typedef typename Eigen::Quaternion<Scalar, Options> Quaternion; | ||
| 347 | typedef Eigen::Map<const Quaternion> ConstQuaternionMap; | ||
| 348 | |||
| 349 |
3/6✓ Branch 1 taken 31911 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31911 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31911 times.
✗ Branch 8 not taken.
|
57588 | data.joint_q = qs.template segment<NQ>(idx_q()); |
| 350 |
2/4✓ Branch 1 taken 31911 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 31911 times.
✗ Branch 6 not taken.
|
57588 | ConstQuaternionMap quat(data.joint_q.template tail<4>().data()); |
| 351 | |||
| 352 |
2/4✓ Branch 1 taken 31911 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31911 times.
✗ Branch 5 not taken.
|
57588 | calc(data, data.joint_q.template head<3>(), quat); |
| 353 | 57588 | } | |
| 354 | |||
| 355 | template<typename TangentVector> | ||
| 356 | PINOCCHIO_DONT_INLINE void | ||
| 357 | 1 | calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs) | |
| 358 | const | ||
| 359 | { | ||
| 360 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | data.joint_v = vs.template segment<NV>(idx_v()); |
| 361 | 1 | data.v = data.joint_v; | |
| 362 | 1 | } | |
| 363 | |||
| 364 | template<typename ConfigVector, typename TangentVector> | ||
| 365 | 11210 | PINOCCHIO_DONT_INLINE void calc( | |
| 366 | JointDataDerived & data, | ||
| 367 | const typename Eigen::MatrixBase<ConfigVector> & qs, | ||
| 368 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
| 369 | { | ||
| 370 | 11210 | calc(data, qs.derived()); | |
| 371 | |||
| 372 |
1/2✓ Branch 3 taken 6317 times.
✗ Branch 4 not taken.
|
11210 | data.joint_v = vs.template segment<NV>(idx_v()); |
| 373 | 11210 | data.v = data.joint_v; | |
| 374 | 11210 | } | |
| 375 | |||
| 376 | template<typename VectorLike, typename Matrix6Like> | ||
| 377 | 393 | void calc_aba( | |
| 378 | JointDataDerived & data, | ||
| 379 | const Eigen::MatrixBase<VectorLike> & armature, | ||
| 380 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
| 381 | const bool update_I) const | ||
| 382 | { | ||
| 383 | 393 | data.U = I; | |
| 384 | 393 | data.StU = I; | |
| 385 |
1/2✓ Branch 2 taken 384 times.
✗ Branch 3 not taken.
|
393 | data.StU.diagonal() += armature; |
| 386 | |||
| 387 | 393 | internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv); | |
| 388 |
2/4✓ Branch 2 taken 384 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 384 times.
✗ Branch 6 not taken.
|
393 | data.UDinv.noalias() = I * data.Dinv; |
| 389 | |||
| 390 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 382 times.
|
393 | if (update_I) |
| 391 |
3/6✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
|
3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose(); |
| 392 | 393 | } | |
| 393 | |||
| 394 | 17518 | static std::string classname() | |
| 395 | { | ||
| 396 |
1/2✓ Branch 2 taken 17518 times.
✗ Branch 3 not taken.
|
17518 | return std::string("JointModelFreeFlyer"); |
| 397 | } | ||
| 398 | 17370 | std::string shortname() const | |
| 399 | { | ||
| 400 | 17370 | return classname(); | |
| 401 | } | ||
| 402 | |||
| 403 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 404 | template<typename NewScalar> | ||
| 405 | 31 | JointModelFreeFlyerTpl<NewScalar, Options> cast() const | |
| 406 | { | ||
| 407 | typedef JointModelFreeFlyerTpl<NewScalar, Options> ReturnType; | ||
| 408 | 31 | ReturnType res; | |
| 409 | 31 | res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); | |
| 410 | 31 | return res; | |
| 411 | } | ||
| 412 | |||
| 413 | }; // struct JointModelFreeFlyerTpl | ||
| 414 | |||
| 415 | } // namespace pinocchio | ||
| 416 | |||
| 417 | #include <boost/type_traits.hpp> | ||
| 418 | |||
| 419 | namespace boost | ||
| 420 | { | ||
| 421 | template<typename Scalar, int Options> | ||
| 422 | struct has_nothrow_constructor<::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>> | ||
| 423 | : public integral_constant<bool, true> | ||
| 424 | { | ||
| 425 | }; | ||
| 426 | |||
| 427 | template<typename Scalar, int Options> | ||
| 428 | struct has_nothrow_copy<::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>> | ||
| 429 | : public integral_constant<bool, true> | ||
| 430 | { | ||
| 431 | }; | ||
| 432 | |||
| 433 | template<typename Scalar, int Options> | ||
| 434 | struct has_nothrow_constructor<::pinocchio::JointDataFreeFlyerTpl<Scalar, Options>> | ||
| 435 | : public integral_constant<bool, true> | ||
| 436 | { | ||
| 437 | }; | ||
| 438 | |||
| 439 | template<typename Scalar, int Options> | ||
| 440 | struct has_nothrow_copy<::pinocchio::JointDataFreeFlyerTpl<Scalar, Options>> | ||
| 441 | : public integral_constant<bool, true> | ||
| 442 | { | ||
| 443 | }; | ||
| 444 | } // namespace boost | ||
| 445 | |||
| 446 | #endif // ifndef __pinocchio_multibody_joint_free_flyer_hpp__ | ||
| 447 |