| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/joint/joint-universal.hpp |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 59 | 168 | 35.1% |
| Branches: | 99 | 466 | 21.2% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2023 INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_multibody_joint_universal_hpp__ | ||
| 6 | #define __pinocchio_multibody_joint_universal_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/fwd.hpp" | ||
| 9 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
| 10 | #include "pinocchio/multibody/joint/joint-spherical.hpp" | ||
| 11 | #include "pinocchio/multibody/joint-motion-subspace.hpp" | ||
| 12 | #include "pinocchio/spatial/inertia.hpp" | ||
| 13 | #include "pinocchio/utils/check.hpp" | ||
| 14 | |||
| 15 | #include "pinocchio/math/matrix.hpp" | ||
| 16 | #include "pinocchio/math/rotation.hpp" | ||
| 17 | |||
| 18 | namespace pinocchio | ||
| 19 | { | ||
| 20 | template<typename Scalar, int Options> | ||
| 21 | struct JointMotionSubspaceUniversalTpl; | ||
| 22 | |||
| 23 | template<typename _Scalar, int _Options> | ||
| 24 | struct traits<JointMotionSubspaceUniversalTpl<_Scalar, _Options>> | ||
| 25 | { | ||
| 26 | typedef _Scalar Scalar; | ||
| 27 | enum | ||
| 28 | { | ||
| 29 | Options = _Options | ||
| 30 | }; | ||
| 31 | enum | ||
| 32 | { | ||
| 33 | LINEAR = 0, | ||
| 34 | ANGULAR = 3 | ||
| 35 | }; | ||
| 36 | |||
| 37 | typedef MotionSphericalTpl<Scalar, Options> JointMotion; | ||
| 38 | typedef Eigen::Matrix<Scalar, 2, 1, Options> JointForce; | ||
| 39 | typedef Eigen::Matrix<Scalar, 6, 2, Options> DenseBase; | ||
| 40 | typedef Eigen::Matrix<Scalar, 3, 3, Options> ReducedSquaredMatrix; | ||
| 41 | |||
| 42 | typedef DenseBase MatrixReturnType; | ||
| 43 | typedef const DenseBase ConstMatrixReturnType; | ||
| 44 | |||
| 45 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
| 46 | typedef Eigen::Matrix<Scalar, 3, 2, Options> Matrix32; | ||
| 47 | typedef Eigen::Matrix<Scalar, 2, 2, Options> Matrix2; | ||
| 48 | |||
| 49 | typedef Matrix2 StDiagonalMatrixSOperationReturnType; | ||
| 50 | }; // traits JointMotionSubspaceUniversalTpl | ||
| 51 | |||
| 52 | template<typename Scalar, int Options> | ||
| 53 | struct SE3GroupAction<JointMotionSubspaceUniversalTpl<Scalar, Options>> | ||
| 54 | { | ||
| 55 | typedef Eigen::Matrix<Scalar, 6, 2, Options> ReturnType; | ||
| 56 | }; | ||
| 57 | |||
| 58 | template<typename Scalar, int Options, typename MotionDerived> | ||
| 59 | struct MotionAlgebraAction<JointMotionSubspaceUniversalTpl<Scalar, Options>, MotionDerived> | ||
| 60 | { | ||
| 61 | typedef Eigen::Matrix<Scalar, 6, 2, Options> ReturnType; | ||
| 62 | }; | ||
| 63 | |||
| 64 | template<typename Scalar, int Options, typename ForceDerived> | ||
| 65 | struct ConstraintForceOp<JointMotionSubspaceUniversalTpl<Scalar, Options>, ForceDerived> | ||
| 66 | { | ||
| 67 | typedef typename traits<JointMotionSubspaceUniversalTpl<Scalar, Options>>::Vector3 Vector3; | ||
| 68 | typedef Eigen::Matrix< | ||
| 69 | typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE( | ||
| 70 | Vector3, typename ForceDense<ForceDerived>::ConstAngularType), | ||
| 71 | 2, | ||
| 72 | 1, | ||
| 73 | Options> | ||
| 74 | ReturnType; | ||
| 75 | }; | ||
| 76 | |||
| 77 | template<typename Scalar, int Options, typename ForceSet> | ||
| 78 | struct ConstraintForceSetOp<JointMotionSubspaceUniversalTpl<Scalar, Options>, ForceSet> | ||
| 79 | { | ||
| 80 | typedef typename traits<JointMotionSubspaceUniversalTpl<Scalar, Options>>::Matrix32 Matrix32; | ||
| 81 | typedef typename MatrixMatrixProduct< | ||
| 82 | Eigen::Transpose<const Matrix32>, | ||
| 83 | typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type>::type ReturnType; | ||
| 84 | }; | ||
| 85 | |||
| 86 | template<typename _Scalar, int _Options> | ||
| 87 | struct JointMotionSubspaceUniversalTpl | ||
| 88 | : JointMotionSubspaceBase<JointMotionSubspaceUniversalTpl<_Scalar, _Options>> | ||
| 89 | { | ||
| 90 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 91 | PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceUniversalTpl) | ||
| 92 | |||
| 93 | enum | ||
| 94 | { | ||
| 95 | NV = 2 | ||
| 96 | }; | ||
| 97 | |||
| 98 | typedef typename traits<JointMotionSubspaceUniversalTpl>::Vector3 Vector3; | ||
| 99 | typedef typename traits<JointMotionSubspaceUniversalTpl>::Matrix32 Matrix32; | ||
| 100 | |||
| 101 | ✗ | JointMotionSubspaceUniversalTpl() | |
| 102 | { | ||
| 103 | } | ||
| 104 | |||
| 105 | template<typename Matrix32Like> | ||
| 106 | 24 | JointMotionSubspaceUniversalTpl(const Eigen::MatrixBase<Matrix32Like> & subspace) | |
| 107 | 24 | : m_S(subspace) | |
| 108 | { | ||
| 109 | 24 | } | |
| 110 | |||
| 111 | template<typename Vector3Like> | ||
| 112 | 2 | JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & v) const | |
| 113 | { | ||
| 114 | // EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); | ||
| 115 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
4 | return JointMotion(m_S * v); |
| 116 | } | ||
| 117 | |||
| 118 | template<typename S1, int O1> | ||
| 119 | typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType | ||
| 120 | ✗ | se3Action(const SE3Tpl<S1, O1> & m) const | |
| 121 | { | ||
| 122 | typedef typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType ReturnType; | ||
| 123 | |||
| 124 | ✗ | ReturnType res; | |
| 125 | ✗ | res.template middleRows<3>(ANGULAR).noalias() = m.rotation() * m_S; | |
| 126 | ✗ | cross( | |
| 127 | ✗ | m.translation(), res.template middleRows<3>(Motion::ANGULAR), | |
| 128 | ✗ | res.template middleRows<3>(LINEAR)); | |
| 129 | ✗ | return res; | |
| 130 | } | ||
| 131 | |||
| 132 | template<typename S1, int O1> | ||
| 133 | typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType | ||
| 134 | ✗ | se3ActionInverse(const SE3Tpl<S1, O1> & m) const | |
| 135 | { | ||
| 136 | typedef typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType ReturnType; | ||
| 137 | |||
| 138 | ✗ | ReturnType res; | |
| 139 | ✗ | cross(m.translation(), m_S, res.template middleRows<3>(ANGULAR)); | |
| 140 | ✗ | res.template middleRows<3>(LINEAR).noalias() = | |
| 141 | ✗ | -m.rotation().transpose() * res.template middleRows<3>(ANGULAR); | |
| 142 | |||
| 143 | // ANGULAR | ||
| 144 | ✗ | res.template middleRows<3>(ANGULAR).noalias() = m.rotation().transpose() * m_S; | |
| 145 | ✗ | return res; | |
| 146 | } | ||
| 147 | |||
| 148 | ✗ | int nv_impl() const | |
| 149 | { | ||
| 150 | ✗ | return NV; | |
| 151 | } | ||
| 152 | |||
| 153 | struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceUniversalTpl> | ||
| 154 | { | ||
| 155 | const JointMotionSubspaceUniversalTpl & ref; | ||
| 156 | ✗ | TransposeConst(const JointMotionSubspaceUniversalTpl & ref) | |
| 157 | ✗ | : ref(ref) | |
| 158 | { | ||
| 159 | } | ||
| 160 | |||
| 161 | template<typename ForceDerived> | ||
| 162 | typename ConstraintForceOp<JointMotionSubspaceUniversalTpl, ForceDerived>::ReturnType | ||
| 163 | ✗ | operator*(const ForceDense<ForceDerived> & f) const | |
| 164 | { | ||
| 165 | ✗ | return ref.m_S.transpose() * f.angular(); | |
| 166 | } | ||
| 167 | |||
| 168 | /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ | ||
| 169 | template<typename ForceSet> | ||
| 170 | typename ConstraintForceSetOp<JointMotionSubspaceUniversalTpl, ForceSet>::ReturnType | ||
| 171 | ✗ | operator*(const Eigen::MatrixBase<ForceSet> & F) | |
| 172 | { | ||
| 173 | EIGEN_STATIC_ASSERT( | ||
| 174 | ForceSet::RowsAtCompileTime == 6, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE) | ||
| 175 | /* Return ax.T * F[3:end,:] */ | ||
| 176 | ✗ | return ref.m_S.transpose() * F.template middleRows<3>(ANGULAR); | |
| 177 | } | ||
| 178 | }; | ||
| 179 | |||
| 180 | ✗ | TransposeConst transpose() const | |
| 181 | { | ||
| 182 | ✗ | return TransposeConst(*this); | |
| 183 | } | ||
| 184 | |||
| 185 | /* CRBA joint operators | ||
| 186 | * - ForceSet::Block = ForceSet | ||
| 187 | * - ForceSet operator* (Inertia Y,Constraint S) | ||
| 188 | * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) | ||
| 189 | * - SE3::act(ForceSet::Block) | ||
| 190 | */ | ||
| 191 | ✗ | DenseBase matrix_impl() const | |
| 192 | { | ||
| 193 | ✗ | DenseBase S; | |
| 194 | ✗ | S.template middleRows<3>(LINEAR).setZero(); | |
| 195 | ✗ | S.template middleRows<3>(ANGULAR) = m_S; | |
| 196 | ✗ | return S; | |
| 197 | } | ||
| 198 | |||
| 199 | template<typename MotionDerived> | ||
| 200 | typename MotionAlgebraAction<JointMotionSubspaceUniversalTpl, MotionDerived>::ReturnType | ||
| 201 | ✗ | motionAction(const MotionDense<MotionDerived> & m) const | |
| 202 | { | ||
| 203 | ✗ | const typename MotionDerived::ConstLinearType v = m.linear(); | |
| 204 | ✗ | const typename MotionDerived::ConstAngularType w = m.angular(); | |
| 205 | |||
| 206 | ✗ | DenseBase res; | |
| 207 | ✗ | cross(v, m_S, res.template middleRows<3>(LINEAR)); | |
| 208 | ✗ | cross(w, m_S, res.template middleRows<3>(ANGULAR)); | |
| 209 | ✗ | return res; | |
| 210 | } | ||
| 211 | |||
| 212 | ✗ | const Matrix32 & angularSubspace() const | |
| 213 | { | ||
| 214 | ✗ | return m_S; | |
| 215 | } | ||
| 216 | ✗ | Matrix32 & angularSubspace() | |
| 217 | { | ||
| 218 | ✗ | return m_S; | |
| 219 | } | ||
| 220 | |||
| 221 | ✗ | bool isEqual(const JointMotionSubspaceUniversalTpl & other) const | |
| 222 | { | ||
| 223 | ✗ | return internal::comparison_eq(m_S, other.m_S); | |
| 224 | } | ||
| 225 | |||
| 226 | protected: | ||
| 227 | Matrix32 m_S; | ||
| 228 | |||
| 229 | }; // struct JointMotionSubspaceUniversalTpl | ||
| 230 | |||
| 231 | namespace details | ||
| 232 | { | ||
| 233 | template<typename Scalar, int Options> | ||
| 234 | struct StDiagonalMatrixSOperation<JointMotionSubspaceUniversalTpl<Scalar, Options>> | ||
| 235 | { | ||
| 236 | typedef JointMotionSubspaceUniversalTpl<Scalar, Options> Constraint; | ||
| 237 | typedef typename traits<Constraint>::StDiagonalMatrixSOperationReturnType ReturnType; | ||
| 238 | |||
| 239 | ✗ | static ReturnType run(const JointMotionSubspaceBase<Constraint> & constraint) | |
| 240 | { | ||
| 241 | ✗ | return constraint.matrix().transpose() * constraint.matrix(); | |
| 242 | } | ||
| 243 | }; | ||
| 244 | } // namespace details | ||
| 245 | |||
| 246 | template<typename S1, int O1, typename S2, int O2> | ||
| 247 | struct MultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceUniversalTpl<S2, O2>> | ||
| 248 | { | ||
| 249 | typedef Eigen::Matrix<S2, 6, 2, O2> ReturnType; | ||
| 250 | }; | ||
| 251 | |||
| 252 | /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ | ||
| 253 | namespace impl | ||
| 254 | { | ||
| 255 | template<typename S1, int O1, typename S2, int O2> | ||
| 256 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceUniversalTpl<S2, O2>> | ||
| 257 | { | ||
| 258 | typedef InertiaTpl<S1, O1> Inertia; | ||
| 259 | typedef JointMotionSubspaceUniversalTpl<S2, O2> Constraint; | ||
| 260 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
| 261 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & cru) | |
| 262 | { | ||
| 263 | typedef typename InertiaTpl<S1, O1>::Symmetric3 Symmetric3; | ||
| 264 | ✗ | Eigen::Matrix<S1, 6, 3, O1> M; | |
| 265 | ✗ | alphaSkew(-Y.mass(), Y.lever(), M.template middleRows<3>(Constraint::LINEAR)); | |
| 266 | ✗ | M.template middleRows<3>(Constraint::ANGULAR) = | |
| 267 | ✗ | (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix(); | |
| 268 | |||
| 269 | ✗ | return (M * cru.angularSubspace()).eval(); | |
| 270 | } | ||
| 271 | }; | ||
| 272 | } // namespace impl | ||
| 273 | |||
| 274 | template<typename M6Like, typename Scalar, int Options> | ||
| 275 | struct MultiplicationOp< | ||
| 276 | Eigen::MatrixBase<M6Like>, | ||
| 277 | JointMotionSubspaceUniversalTpl<Scalar, Options>> | ||
| 278 | { | ||
| 279 | typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols; | ||
| 280 | typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst; | ||
| 281 | |||
| 282 | typedef JointMotionSubspaceUniversalTpl<Scalar, Options> Constraint; | ||
| 283 | typedef typename Constraint::Matrix32 Matrix32; | ||
| 284 | typedef const typename MatrixMatrixProduct<M6LikeColsNonConst, Matrix32>::type ReturnType; | ||
| 285 | }; | ||
| 286 | |||
| 287 | /* [ABA] operator* (Inertia Y,Constraint S) */ | ||
| 288 | namespace impl | ||
| 289 | { | ||
| 290 | template<typename M6Like, typename Scalar, int Options> | ||
| 291 | struct LhsMultiplicationOp< | ||
| 292 | Eigen::MatrixBase<M6Like>, | ||
| 293 | JointMotionSubspaceUniversalTpl<Scalar, Options>> | ||
| 294 | { | ||
| 295 | typedef JointMotionSubspaceUniversalTpl<Scalar, Options> Constraint; | ||
| 296 | typedef | ||
| 297 | typename MultiplicationOp<Eigen::MatrixBase<M6Like>, Constraint>::ReturnType ReturnType; | ||
| 298 | |||
| 299 | ✗ | static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & cru) | |
| 300 | { | ||
| 301 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6); | ||
| 302 | ✗ | return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.angularSubspace(); | |
| 303 | } | ||
| 304 | }; | ||
| 305 | } // namespace impl | ||
| 306 | |||
| 307 | template<typename Scalar, int Options> | ||
| 308 | struct JointUniversalTpl; | ||
| 309 | |||
| 310 | template<typename _Scalar, int _Options> | ||
| 311 | struct traits<JointUniversalTpl<_Scalar, _Options>> | ||
| 312 | { | ||
| 313 | enum | ||
| 314 | { | ||
| 315 | NQ = 2, | ||
| 316 | NV = 2 | ||
| 317 | }; | ||
| 318 | typedef _Scalar Scalar; | ||
| 319 | enum | ||
| 320 | { | ||
| 321 | Options = _Options | ||
| 322 | }; | ||
| 323 | typedef JointDataUniversalTpl<Scalar, Options> JointDataDerived; | ||
| 324 | typedef JointModelUniversalTpl<Scalar, Options> JointModelDerived; | ||
| 325 | typedef JointMotionSubspaceUniversalTpl<Scalar, Options> Constraint_t; | ||
| 326 | typedef SE3Tpl<Scalar, Options> Transformation_t; | ||
| 327 | typedef MotionSphericalTpl<Scalar, Options> Motion_t; | ||
| 328 | typedef MotionSphericalTpl<Scalar, Options> Bias_t; | ||
| 329 | |||
| 330 | // [ABA] | ||
| 331 | typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t; | ||
| 332 | typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t; | ||
| 333 | typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t; | ||
| 334 | |||
| 335 | typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t; | ||
| 336 | typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t; | ||
| 337 | |||
| 338 | PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE | ||
| 339 | }; | ||
| 340 | |||
| 341 | template<typename _Scalar, int _Options> | ||
| 342 | struct traits<JointDataUniversalTpl<_Scalar, _Options>> | ||
| 343 | { | ||
| 344 | typedef JointUniversalTpl<_Scalar, _Options> JointDerived; | ||
| 345 | typedef _Scalar Scalar; | ||
| 346 | }; | ||
| 347 | |||
| 348 | template<typename _Scalar, int _Options> | ||
| 349 | struct traits<JointModelUniversalTpl<_Scalar, _Options>> | ||
| 350 | { | ||
| 351 | typedef JointUniversalTpl<_Scalar, _Options> JointDerived; | ||
| 352 | typedef _Scalar Scalar; | ||
| 353 | }; | ||
| 354 | |||
| 355 | template<typename _Scalar, int _Options> | ||
| 356 | struct JointDataUniversalTpl : public JointDataBase<JointDataUniversalTpl<_Scalar, _Options>> | ||
| 357 | { | ||
| 358 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 359 | typedef JointUniversalTpl<_Scalar, _Options> JointDerived; | ||
| 360 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
| 361 | 2 | PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR | |
| 362 | |||
| 363 | ConfigVector_t joint_q; | ||
| 364 | TangentVector_t joint_v; | ||
| 365 | |||
| 366 | Transformation_t M; | ||
| 367 | Constraint_t S; | ||
| 368 | Motion_t v; | ||
| 369 | Bias_t c; | ||
| 370 | |||
| 371 | // [ABA] specific data | ||
| 372 | U_t U; | ||
| 373 | D_t Dinv; | ||
| 374 | UD_t UDinv; | ||
| 375 | D_t StU; | ||
| 376 | |||
| 377 | 24 | JointDataUniversalTpl() | |
| 378 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | : joint_q(ConfigVector_t::Zero()) |
| 379 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , joint_v(TangentVector_t::Zero()) |
| 380 | 24 | , M(Transformation_t::Identity()) | |
| 381 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , S(Constraint_t::Matrix32::Zero()) |
| 382 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , v(Motion_t::Vector3::Zero()) |
| 383 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , c(Bias_t::Vector3::Zero()) |
| 384 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , U(U_t::Zero()) |
| 385 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , Dinv(D_t::Zero()) |
| 386 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | , UDinv(UD_t::Zero()) |
| 387 |
1/2✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
|
48 | , StU(D_t::Zero()) |
| 388 | { | ||
| 389 | 24 | } | |
| 390 | |||
| 391 | 40 | static std::string classname() | |
| 392 | { | ||
| 393 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | return std::string("JointDataUniversal"); |
| 394 | } | ||
| 395 | ✗ | std::string shortname() const | |
| 396 | { | ||
| 397 | ✗ | return classname(); | |
| 398 | } | ||
| 399 | |||
| 400 | }; // struct JointDataUniversalTpl | ||
| 401 | |||
| 402 | PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelUniversalTpl); | ||
| 403 | template<typename _Scalar, int _Options> | ||
| 404 | struct JointModelUniversalTpl : public JointModelBase<JointModelUniversalTpl<_Scalar, _Options>> | ||
| 405 | { | ||
| 406 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 407 | typedef JointUniversalTpl<_Scalar, _Options> JointDerived; | ||
| 408 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
| 409 | typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3; | ||
| 410 | typedef Eigen::Matrix<Scalar, 3, 3, _Options> Matrix3; | ||
| 411 | |||
| 412 | typedef JointModelBase<JointModelUniversalTpl> Base; | ||
| 413 | using Base::id; | ||
| 414 | using Base::idx_q; | ||
| 415 | using Base::idx_v; | ||
| 416 | using Base::setIndexes; | ||
| 417 | |||
| 418 | 20 | JointModelUniversalTpl() | |
| 419 | 20 | { | |
| 420 | 20 | } | |
| 421 | |||
| 422 | ✗ | JointModelUniversalTpl( | |
| 423 | const Scalar & x1, | ||
| 424 | const Scalar & y1, | ||
| 425 | const Scalar & z1, | ||
| 426 | const Scalar & x2, | ||
| 427 | const Scalar & y2, | ||
| 428 | const Scalar & z2) | ||
| 429 | ✗ | : axis1(x1, y1, z1) | |
| 430 | ✗ | , axis2(x2, y2, z2) | |
| 431 | { | ||
| 432 | ✗ | assert(isUnitary(axis1) && "First Rotation axis is not unitary"); | |
| 433 | ✗ | assert(isUnitary(axis2) && "Second Rotation axis is not unitary"); | |
| 434 | ✗ | assert(check_expression_if_real<Scalar>(axis1.dot(axis2) == 0) && "Axii are not orthogonal"); | |
| 435 | } | ||
| 436 | |||
| 437 | template<typename Vector3Like> | ||
| 438 | ✗ | JointModelUniversalTpl( | |
| 439 | const Eigen::MatrixBase<Vector3Like> & axis1_, const Eigen::MatrixBase<Vector3Like> & axis2_) | ||
| 440 | ✗ | : axis1(axis1_) | |
| 441 | ✗ | , axis2(axis2_) | |
| 442 | { | ||
| 443 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like); | ||
| 444 | ✗ | assert(isUnitary(axis1) && "First Rotation axis is not unitary"); | |
| 445 | ✗ | assert(isUnitary(axis2) && "Second Rotation axis is not unitary"); | |
| 446 | ✗ | assert( | |
| 447 | check_expression_if_real<Scalar>(axis1.dot(axis2) == Scalar(0)) | ||
| 448 | && "Axii are not orthogonal"); | ||
| 449 | } | ||
| 450 | |||
| 451 | 4 | JointDataDerived createData() const | |
| 452 | { | ||
| 453 | 4 | return JointDataDerived(); | |
| 454 | } | ||
| 455 | |||
| 456 | ✗ | const std::vector<bool> hasConfigurationLimit() const | |
| 457 | { | ||
| 458 | ✗ | return {true, true}; | |
| 459 | } | ||
| 460 | |||
| 461 | ✗ | const std::vector<bool> hasConfigurationLimitInTangent() const | |
| 462 | { | ||
| 463 | ✗ | return {true, true}; | |
| 464 | } | ||
| 465 | |||
| 466 | using Base::isEqual; | ||
| 467 | ✗ | bool isEqual(const JointModelUniversalTpl & other) const | |
| 468 | { | ||
| 469 | ✗ | return Base::isEqual(other) && internal::comparison_eq(axis1, other.axis1) | |
| 470 | ✗ | && internal::comparison_eq(axis2, other.axis2); | |
| 471 | } | ||
| 472 | |||
| 473 | template<typename ConfigVector> | ||
| 474 | ✗ | void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const | |
| 475 | { | ||
| 476 | ✗ | data.joint_q = qs.template segment<NQ>(idx_q()); | |
| 477 | Scalar c0, s0; | ||
| 478 | ✗ | SINCOS(data.joint_q(0), &s0, &c0); | |
| 479 | Scalar c1, s1; | ||
| 480 | ✗ | SINCOS(data.joint_q(1), &s1, &c1); | |
| 481 | |||
| 482 | ✗ | Matrix3 rot1, rot2; | |
| 483 | ✗ | toRotationMatrix(axis1, c0, s0, rot1); | |
| 484 | ✗ | toRotationMatrix(axis2, c1, s1, rot2); | |
| 485 | ✗ | data.M.rotation() = rot1 * rot2; | |
| 486 | |||
| 487 | ✗ | data.S.angularSubspace() << rot2.coeffRef(0, 0) * axis1.x() + rot2.coeffRef(1, 0) * axis1.y() | |
| 488 | ✗ | + rot2.coeffRef(2, 0) * axis1.z(), | |
| 489 | ✗ | axis2.x(), | |
| 490 | ✗ | rot2.coeffRef(0, 1) * axis1.x() + rot2.coeffRef(1, 1) * axis1.y() | |
| 491 | ✗ | + rot2.coeffRef(2, 1) * axis1.z(), | |
| 492 | ✗ | axis2.y(), | |
| 493 | ✗ | rot2.coeffRef(0, 2) * axis1.x() + rot2.coeffRef(1, 2) * axis1.y() | |
| 494 | ✗ | + rot2.coeffRef(2, 2) * axis1.z(), | |
| 495 | ✗ | axis2.z(); | |
| 496 | } | ||
| 497 | |||
| 498 | template<typename TangentVector> | ||
| 499 | void | ||
| 500 | ✗ | calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs) | |
| 501 | const | ||
| 502 | { | ||
| 503 | ✗ | data.joint_v = vs.template segment<NV>(idx_v()); | |
| 504 | ✗ | data.v().noalias() = data.S.angularSubspace() * data.joint_v; | |
| 505 | |||
| 506 | // TODO: need to be done | ||
| 507 | // #define q_dot data.joint_v | ||
| 508 | // Scalar tmp; | ||
| 509 | // tmp = (-s1+axis2.x()*axis2.x()*s1)*axis1.x() + | ||
| 510 | // (axis2.x()*axis2.y()*s1+axis2.z()*c1)*axis1.y() + | ||
| 511 | // (axis2.x()*axis2.z()*s1-axis2.y()*c1)*axis1.z(); data.c()(0) = tmp * | ||
| 512 | // q_dot(1)*q_dot(0); tmp = (axis2.x()*axis2.y()*s1-axis2.z()*c1)*axis1.x() + | ||
| 513 | // (-s1+axis2.y()*axis2.y()*s1)*axis1.y() + | ||
| 514 | // (axis2.y()*axis2.z()*s1+axis2.x()*c1)*axis1.z(); data.c()(1) = tmp * | ||
| 515 | // q_dot(1)*q_dot(0); tmp = (axis2.z()*axis2.x()*s1+axis2.y()*c1)*axis1.x() + | ||
| 516 | // (axis2.y()*axis2.z()*s1-axis2.x()*c1)*axis1.y() + | ||
| 517 | // (-s1+axis2.z()*axis2.z()*s1)*axis1.z(); data.c()(2) = tmp * q_dot(1)*q_dot(0); | ||
| 518 | // #undef q_dot | ||
| 519 | } | ||
| 520 | |||
| 521 | template<typename ConfigVector, typename TangentVector> | ||
| 522 | 6 | void calc( | |
| 523 | JointDataDerived & data, | ||
| 524 | const typename Eigen::MatrixBase<ConfigVector> & qs, | ||
| 525 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
| 526 | { | ||
| 527 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | data.joint_q = qs.template segment<NQ>(idx_q()); |
| 528 | |||
| 529 | Scalar c0, s0; | ||
| 530 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | SINCOS(data.joint_q(0), &s0, &c0); |
| 531 | Scalar c1, s1; | ||
| 532 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | SINCOS(data.joint_q(1), &s1, &c1); |
| 533 | |||
| 534 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | Matrix3 rot1, rot2; |
| 535 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | toRotationMatrix(axis1, c0, s0, rot1); |
| 536 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | toRotationMatrix(axis2, c1, s1, rot2); |
| 537 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | data.M.rotation() = rot1 * rot2; |
| 538 | |||
| 539 |
4/10✓ 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 taken 6 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
|
6 | data.S.angularSubspace() << rot2.coeffRef(0, 0) * axis1.x() + rot2.coeffRef(1, 0) * axis1.y() |
| 540 |
3/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | + rot2.coeffRef(2, 0) * axis1.z(), |
| 541 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | axis2.x(), |
| 542 |
4/10✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | rot2.coeffRef(0, 1) * axis1.x() + rot2.coeffRef(1, 1) * axis1.y() |
| 543 |
3/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | + rot2.coeffRef(2, 1) * axis1.z(), |
| 544 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | axis2.y(), |
| 545 |
4/10✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | rot2.coeffRef(0, 2) * axis1.x() + rot2.coeffRef(1, 2) * axis1.y() |
| 546 |
3/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
12 | + rot2.coeffRef(2, 2) * axis1.z(), |
| 547 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | axis2.z(); |
| 548 | |||
| 549 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | data.joint_v = vs.template segment<NV>(idx_v()); |
| 550 |
3/6✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
|
6 | data.v().noalias() = data.S.angularSubspace() * data.joint_v; |
| 551 | |||
| 552 | #define q_dot data.joint_v | ||
| 553 | Scalar tmp; | ||
| 554 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | tmp = (-s1 + axis2.x() * axis2.x() * s1) * axis1.x() |
| 555 |
4/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | + (axis2.x() * axis2.y() * s1 + axis2.z() * c1) * axis1.y() |
| 556 |
4/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | + (axis2.x() * axis2.z() * s1 - axis2.y() * c1) * axis1.z(); |
| 557 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
|
6 | data.c()(0) = tmp * q_dot(1) * q_dot(0); |
| 558 |
4/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | tmp = (axis2.x() * axis2.y() * s1 - axis2.z() * c1) * axis1.x() |
| 559 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | + (-s1 + axis2.y() * axis2.y() * s1) * axis1.y() |
| 560 |
4/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | + (axis2.y() * axis2.z() * s1 + axis2.x() * c1) * axis1.z(); |
| 561 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
|
6 | data.c()(1) = tmp * q_dot(1) * q_dot(0); |
| 562 |
4/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | tmp = (axis2.z() * axis2.x() * s1 + axis2.y() * c1) * axis1.x() |
| 563 |
4/8✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
6 | + (axis2.y() * axis2.z() * s1 - axis2.x() * c1) * axis1.y() |
| 564 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | + (-s1 + axis2.z() * axis2.z() * s1) * axis1.z(); |
| 565 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
|
6 | data.c()(2) = tmp * q_dot(1) * q_dot(0); |
| 566 | #undef q_dot | ||
| 567 | 6 | } | |
| 568 | |||
| 569 | template<typename VectorLike, typename Matrix6Like> | ||
| 570 | ✗ | void calc_aba( | |
| 571 | JointDataDerived & data, | ||
| 572 | const Eigen::MatrixBase<VectorLike> & armature, | ||
| 573 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
| 574 | const bool update_I) const | ||
| 575 | { | ||
| 576 | ✗ | data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.angularSubspace(); | |
| 577 | ✗ | data.StU.noalias() = | |
| 578 | ✗ | data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR); | |
| 579 | ✗ | data.StU.diagonal() += armature; | |
| 580 | ✗ | internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv); | |
| 581 | |||
| 582 | ✗ | data.UDinv.noalias() = data.U * data.Dinv; | |
| 583 | |||
| 584 | ✗ | if (update_I) | |
| 585 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose(); | |
| 586 | } | ||
| 587 | |||
| 588 | 40 | static std::string classname() | |
| 589 | { | ||
| 590 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | return std::string("JointModelUniversal"); |
| 591 | } | ||
| 592 | ✗ | std::string shortname() const | |
| 593 | { | ||
| 594 | ✗ | return classname(); | |
| 595 | } | ||
| 596 | |||
| 597 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 598 | template<typename NewScalar> | ||
| 599 | ✗ | JointModelUniversalTpl<NewScalar, Options> cast() const | |
| 600 | { | ||
| 601 | typedef JointModelUniversalTpl<NewScalar, Options> ReturnType; | ||
| 602 | ✗ | ReturnType res(axis1.template cast<NewScalar>(), axis2.template cast<NewScalar>()); | |
| 603 | ✗ | res.setIndexes(id(), idx_q(), idx_v()); | |
| 604 | ✗ | return res; | |
| 605 | } | ||
| 606 | |||
| 607 | /// \brief 3d main axii of the joint. | ||
| 608 | /// | ||
| 609 | Vector3 axis1; | ||
| 610 | Vector3 axis2; | ||
| 611 | }; // struct JointModelUniversalTpl | ||
| 612 | |||
| 613 | } // namespace pinocchio | ||
| 614 | |||
| 615 | #include <boost/type_traits.hpp> | ||
| 616 | |||
| 617 | namespace boost | ||
| 618 | { | ||
| 619 | template<typename Scalar, int Options> | ||
| 620 | struct has_nothrow_constructor<::pinocchio::JointModelUniversalTpl<Scalar, Options>> | ||
| 621 | : public integral_constant<bool, true> | ||
| 622 | { | ||
| 623 | }; | ||
| 624 | |||
| 625 | template<typename Scalar, int Options> | ||
| 626 | struct has_nothrow_copy<::pinocchio::JointModelUniversalTpl<Scalar, Options>> | ||
| 627 | : public integral_constant<bool, true> | ||
| 628 | { | ||
| 629 | }; | ||
| 630 | |||
| 631 | template<typename Scalar, int Options> | ||
| 632 | struct has_nothrow_constructor<::pinocchio::JointDataUniversalTpl<Scalar, Options>> | ||
| 633 | : public integral_constant<bool, true> | ||
| 634 | { | ||
| 635 | }; | ||
| 636 | |||
| 637 | template<typename Scalar, int Options> | ||
| 638 | struct has_nothrow_copy<::pinocchio::JointDataUniversalTpl<Scalar, Options>> | ||
| 639 | : public integral_constant<bool, true> | ||
| 640 | { | ||
| 641 | }; | ||
| 642 | } // namespace boost | ||
| 643 | |||
| 644 | #endif // ifndef __pinocchio_multibody_joint_universal_hpp__ | ||
| 645 |