| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/joint/joint-mimic.hpp |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 48 | 213 | 22.5% |
| Branches: | 13 | 134 | 9.7% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2019-2021 INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_multibody_joint_mimic_hpp__ | ||
| 6 | #define __pinocchio_multibody_joint_mimic_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/macros.hpp" | ||
| 9 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
| 10 | |||
| 11 | namespace pinocchio | ||
| 12 | { | ||
| 13 | |||
| 14 | template<class Constraint> | ||
| 15 | struct ScaledJointMotionSubspace; | ||
| 16 | |||
| 17 | template<class Constraint> | ||
| 18 | struct traits<ScaledJointMotionSubspace<Constraint>> | ||
| 19 | { | ||
| 20 | typedef typename traits<Constraint>::Scalar Scalar; | ||
| 21 | enum | ||
| 22 | { | ||
| 23 | Options = traits<Constraint>::Options | ||
| 24 | }; | ||
| 25 | enum | ||
| 26 | { | ||
| 27 | LINEAR = traits<Constraint>::LINEAR, | ||
| 28 | ANGULAR = traits<Constraint>::ANGULAR | ||
| 29 | }; | ||
| 30 | |||
| 31 | typedef typename traits<Constraint>::JointMotion JointMotion; | ||
| 32 | typedef typename traits<Constraint>::JointForce JointForce; | ||
| 33 | typedef typename traits<Constraint>::DenseBase DenseBase; | ||
| 34 | typedef typename traits<Constraint>::ReducedSquaredMatrix ReducedSquaredMatrix; | ||
| 35 | |||
| 36 | typedef typename traits<Constraint>::MatrixReturnType MatrixReturnType; | ||
| 37 | typedef typename traits<Constraint>::ConstMatrixReturnType ConstMatrixReturnType; | ||
| 38 | typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; | ||
| 39 | }; // traits ScaledJointMotionSubspace | ||
| 40 | |||
| 41 | template<class Constraint> | ||
| 42 | struct SE3GroupAction<ScaledJointMotionSubspace<Constraint>> | ||
| 43 | { | ||
| 44 | typedef typename SE3GroupAction<Constraint>::ReturnType ReturnType; | ||
| 45 | }; | ||
| 46 | |||
| 47 | template<class Constraint, typename MotionDerived> | ||
| 48 | struct MotionAlgebraAction<ScaledJointMotionSubspace<Constraint>, MotionDerived> | ||
| 49 | { | ||
| 50 | typedef typename MotionAlgebraAction<Constraint, MotionDerived>::ReturnType ReturnType; | ||
| 51 | }; | ||
| 52 | |||
| 53 | template<class Constraint, typename ForceDerived> | ||
| 54 | struct ConstraintForceOp<ScaledJointMotionSubspace<Constraint>, ForceDerived> | ||
| 55 | { | ||
| 56 | typedef typename Constraint::Scalar Scalar; | ||
| 57 | typedef typename ConstraintForceOp<Constraint, ForceDerived>::ReturnType OriginalReturnType; | ||
| 58 | |||
| 59 | typedef typename ScalarMatrixProduct<Scalar, OriginalReturnType>::type IdealReturnType; | ||
| 60 | typedef Eigen::Matrix< | ||
| 61 | Scalar, | ||
| 62 | IdealReturnType::RowsAtCompileTime, | ||
| 63 | IdealReturnType::ColsAtCompileTime, | ||
| 64 | Constraint::Options> | ||
| 65 | ReturnType; | ||
| 66 | }; | ||
| 67 | |||
| 68 | template<class Constraint, typename ForceSet> | ||
| 69 | struct ConstraintForceSetOp<ScaledJointMotionSubspace<Constraint>, ForceSet> | ||
| 70 | { | ||
| 71 | typedef typename Constraint::Scalar Scalar; | ||
| 72 | typedef typename ConstraintForceSetOp<Constraint, ForceSet>::ReturnType OriginalReturnType; | ||
| 73 | typedef typename ScalarMatrixProduct<Scalar, OriginalReturnType>::type IdealReturnType; | ||
| 74 | typedef Eigen::Matrix< | ||
| 75 | Scalar, | ||
| 76 | Constraint::NV, | ||
| 77 | ForceSet::ColsAtCompileTime, | ||
| 78 | Constraint::Options | Eigen::RowMajor> | ||
| 79 | ReturnType; | ||
| 80 | }; | ||
| 81 | |||
| 82 | template<class Constraint> | ||
| 83 | struct ScaledJointMotionSubspace : JointMotionSubspaceBase<ScaledJointMotionSubspace<Constraint>> | ||
| 84 | { | ||
| 85 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 86 | |||
| 87 | PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledJointMotionSubspace) | ||
| 88 | enum | ||
| 89 | { | ||
| 90 | NV = Constraint::NV | ||
| 91 | }; | ||
| 92 | typedef JointMotionSubspaceBase<ScaledJointMotionSubspace> Base; | ||
| 93 | using Base::nv; | ||
| 94 | |||
| 95 | typedef typename SE3GroupAction<Constraint>::ReturnType SE3ActionReturnType; | ||
| 96 | |||
| 97 | 168 | ScaledJointMotionSubspace() | |
| 98 | 168 | { | |
| 99 | 168 | } | |
| 100 | |||
| 101 | 120 | explicit ScaledJointMotionSubspace(const Scalar & scaling_factor) | |
| 102 | 120 | : m_scaling_factor(scaling_factor) | |
| 103 | { | ||
| 104 | 120 | } | |
| 105 | |||
| 106 | 180 | ScaledJointMotionSubspace(const Constraint & constraint, const Scalar & scaling_factor) | |
| 107 | ✗ | : m_constraint(constraint) | |
| 108 | 180 | , m_scaling_factor(scaling_factor) | |
| 109 | { | ||
| 110 | 180 | } | |
| 111 | |||
| 112 | ✗ | ScaledJointMotionSubspace(const ScaledJointMotionSubspace & other) | |
| 113 | : m_constraint(other.m_constraint) | ||
| 114 | ✗ | , m_scaling_factor(other.m_scaling_factor) | |
| 115 | { | ||
| 116 | } | ||
| 117 | |||
| 118 | 168 | ScaledJointMotionSubspace & operator=(const ScaledJointMotionSubspace & other) | |
| 119 | { | ||
| 120 | m_constraint = other.m_constraint; | ||
| 121 | 168 | m_scaling_factor = other.m_scaling_factor; | |
| 122 | 168 | return *this; | |
| 123 | } | ||
| 124 | |||
| 125 | template<typename VectorLike> | ||
| 126 | ✗ | JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & v) const | |
| 127 | { | ||
| 128 | ✗ | assert(v.size() == nv()); | |
| 129 | ✗ | JointMotion jm = m_constraint * v; | |
| 130 | ✗ | return jm * m_scaling_factor; | |
| 131 | } | ||
| 132 | |||
| 133 | template<typename S1, int O1> | ||
| 134 | ✗ | SE3ActionReturnType se3Action(const SE3Tpl<S1, O1> & m) const | |
| 135 | { | ||
| 136 | ✗ | SE3ActionReturnType res = m_constraint.se3Action(m); | |
| 137 | ✗ | return m_scaling_factor * res; | |
| 138 | } | ||
| 139 | |||
| 140 | template<typename S1, int O1> | ||
| 141 | ✗ | SE3ActionReturnType se3ActionInverse(const SE3Tpl<S1, O1> & m) const | |
| 142 | { | ||
| 143 | ✗ | SE3ActionReturnType res = m_constraint.se3ActionInverse(m); | |
| 144 | ✗ | return m_scaling_factor * res; | |
| 145 | } | ||
| 146 | |||
| 147 | ✗ | int nv_impl() const | |
| 148 | { | ||
| 149 | ✗ | return m_constraint.nv(); | |
| 150 | } | ||
| 151 | |||
| 152 | struct TransposeConst : JointMotionSubspaceTransposeBase<ScaledJointMotionSubspace> | ||
| 153 | { | ||
| 154 | const ScaledJointMotionSubspace & ref; | ||
| 155 | ✗ | TransposeConst(const ScaledJointMotionSubspace & ref) | |
| 156 | ✗ | : ref(ref) | |
| 157 | { | ||
| 158 | } | ||
| 159 | |||
| 160 | template<typename Derived> | ||
| 161 | typename ConstraintForceOp<ScaledJointMotionSubspace, Derived>::ReturnType | ||
| 162 | ✗ | operator*(const ForceDense<Derived> & f) const | |
| 163 | { | ||
| 164 | // TODO: I don't know why, but we should a dense a return type, otherwise it fails at the | ||
| 165 | // evaluation level; | ||
| 166 | typedef | ||
| 167 | typename ConstraintForceOp<ScaledJointMotionSubspace, Derived>::ReturnType ReturnType; | ||
| 168 | ✗ | return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f)); | |
| 169 | } | ||
| 170 | |||
| 171 | /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) | ||
| 172 | template<typename Derived> | ||
| 173 | typename ConstraintForceSetOp<ScaledJointMotionSubspace, Derived>::ReturnType | ||
| 174 | ✗ | operator*(const Eigen::MatrixBase<Derived> & F) const | |
| 175 | { | ||
| 176 | typedef | ||
| 177 | typename ConstraintForceSetOp<ScaledJointMotionSubspace, Derived>::ReturnType ReturnType; | ||
| 178 | ✗ | return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F)); | |
| 179 | } | ||
| 180 | |||
| 181 | }; // struct TransposeConst | ||
| 182 | |||
| 183 | ✗ | TransposeConst transpose() const | |
| 184 | { | ||
| 185 | ✗ | return TransposeConst(*this); | |
| 186 | } | ||
| 187 | |||
| 188 | ✗ | DenseBase matrix_impl() const | |
| 189 | { | ||
| 190 | ✗ | DenseBase S = m_scaling_factor * m_constraint.matrix(); | |
| 191 | ✗ | return S; | |
| 192 | } | ||
| 193 | |||
| 194 | template<typename MotionDerived> | ||
| 195 | typename MotionAlgebraAction<ScaledJointMotionSubspace, MotionDerived>::ReturnType | ||
| 196 | ✗ | motionAction(const MotionDense<MotionDerived> & m) const | |
| 197 | { | ||
| 198 | typedef typename MotionAlgebraAction<ScaledJointMotionSubspace, MotionDerived>::ReturnType | ||
| 199 | ReturnType; | ||
| 200 | ✗ | ReturnType res = m_scaling_factor * m_constraint.motionAction(m); | |
| 201 | ✗ | return res; | |
| 202 | } | ||
| 203 | |||
| 204 | ✗ | inline const Scalar & scaling() const | |
| 205 | { | ||
| 206 | ✗ | return m_scaling_factor; | |
| 207 | } | ||
| 208 | ✗ | inline Scalar & scaling() | |
| 209 | { | ||
| 210 | ✗ | return m_scaling_factor; | |
| 211 | } | ||
| 212 | |||
| 213 | ✗ | inline const Constraint & constraint() const | |
| 214 | { | ||
| 215 | ✗ | return m_constraint; | |
| 216 | } | ||
| 217 | ✗ | inline Constraint & constraint() | |
| 218 | { | ||
| 219 | ✗ | return m_constraint; | |
| 220 | } | ||
| 221 | |||
| 222 | ✗ | bool isEqual(const ScaledJointMotionSubspace & other) const | |
| 223 | { | ||
| 224 | ✗ | return internal::comparison_eq(m_constraint, other.m_constraint) | |
| 225 | ✗ | && internal::comparison_eq(m_scaling_factor, other.m_scaling_factor); | |
| 226 | } | ||
| 227 | |||
| 228 | protected: | ||
| 229 | Constraint m_constraint; | ||
| 230 | Scalar m_scaling_factor; | ||
| 231 | }; // struct ScaledJointMotionSubspace | ||
| 232 | |||
| 233 | namespace details | ||
| 234 | { | ||
| 235 | template<typename ParentConstraint> | ||
| 236 | struct StDiagonalMatrixSOperation<ScaledJointMotionSubspace<ParentConstraint>> | ||
| 237 | { | ||
| 238 | typedef ScaledJointMotionSubspace<ParentConstraint> Constraint; | ||
| 239 | typedef typename traits<Constraint>::StDiagonalMatrixSOperationReturnType ReturnType; | ||
| 240 | |||
| 241 | ✗ | static ReturnType run(const JointMotionSubspaceBase<Constraint> & constraint) | |
| 242 | { | ||
| 243 | ✗ | const Constraint & constraint_ = constraint.derived(); | |
| 244 | ✗ | return (constraint_.constraint().transpose() * constraint_.constraint()) | |
| 245 | ✗ | * (constraint_.scaling() * constraint_.scaling()); | |
| 246 | } | ||
| 247 | }; | ||
| 248 | } // namespace details | ||
| 249 | |||
| 250 | template<typename S1, int O1, typename _Constraint> | ||
| 251 | struct MultiplicationOp<InertiaTpl<S1, O1>, ScaledJointMotionSubspace<_Constraint>> | ||
| 252 | { | ||
| 253 | typedef InertiaTpl<S1, O1> Inertia; | ||
| 254 | typedef ScaledJointMotionSubspace<_Constraint> Constraint; | ||
| 255 | typedef typename Constraint::Scalar Scalar; | ||
| 256 | |||
| 257 | typedef typename MultiplicationOp<Inertia, _Constraint>::ReturnType OriginalReturnType; | ||
| 258 | // typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type ReturnType; | ||
| 259 | typedef OriginalReturnType ReturnType; | ||
| 260 | }; | ||
| 261 | |||
| 262 | /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ | ||
| 263 | namespace impl | ||
| 264 | { | ||
| 265 | template<typename S1, int O1, typename _Constraint> | ||
| 266 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, ScaledJointMotionSubspace<_Constraint>> | ||
| 267 | { | ||
| 268 | typedef InertiaTpl<S1, O1> Inertia; | ||
| 269 | typedef ScaledJointMotionSubspace<_Constraint> Constraint; | ||
| 270 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
| 271 | |||
| 272 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & scaled_constraint) | |
| 273 | { | ||
| 274 | ✗ | return scaled_constraint.scaling() * (Y * scaled_constraint.constraint()); | |
| 275 | } | ||
| 276 | }; | ||
| 277 | } // namespace impl | ||
| 278 | |||
| 279 | template<typename M6Like, typename _Constraint> | ||
| 280 | struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledJointMotionSubspace<_Constraint>> | ||
| 281 | { | ||
| 282 | typedef typename MultiplicationOp<Inertia, _Constraint>::ReturnType OriginalReturnType; | ||
| 283 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType; | ||
| 284 | }; | ||
| 285 | |||
| 286 | /* [ABA] operator* (Inertia Y,Constraint S) */ | ||
| 287 | namespace impl | ||
| 288 | { | ||
| 289 | template<typename M6Like, typename _Constraint> | ||
| 290 | struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledJointMotionSubspace<_Constraint>> | ||
| 291 | { | ||
| 292 | typedef ScaledJointMotionSubspace<_Constraint> Constraint; | ||
| 293 | typedef | ||
| 294 | typename MultiplicationOp<Eigen::MatrixBase<M6Like>, Constraint>::ReturnType ReturnType; | ||
| 295 | |||
| 296 | static inline ReturnType | ||
| 297 | ✗ | run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & scaled_constraint) | |
| 298 | { | ||
| 299 | ✗ | return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint()); | |
| 300 | } | ||
| 301 | }; | ||
| 302 | } // namespace impl | ||
| 303 | |||
| 304 | template<class Joint> | ||
| 305 | struct JointMimic; | ||
| 306 | template<class JointModel> | ||
| 307 | struct JointModelMimic; | ||
| 308 | template<class JointData> | ||
| 309 | struct JointDataMimic; | ||
| 310 | |||
| 311 | template<class Joint> | ||
| 312 | struct traits<JointMimic<Joint>> | ||
| 313 | { | ||
| 314 | enum | ||
| 315 | { | ||
| 316 | NQ = traits<Joint>::NV, | ||
| 317 | NV = traits<Joint>::NQ | ||
| 318 | }; | ||
| 319 | typedef typename traits<Joint>::Scalar Scalar; | ||
| 320 | enum | ||
| 321 | { | ||
| 322 | Options = traits<Joint>::Options | ||
| 323 | }; | ||
| 324 | |||
| 325 | typedef typename traits<Joint>::JointDataDerived JointDataBase; | ||
| 326 | typedef typename traits<Joint>::JointModelDerived JointModelBase; | ||
| 327 | |||
| 328 | typedef JointDataMimic<JointDataBase> JointDataDerived; | ||
| 329 | typedef JointModelMimic<JointModelBase> JointModelDerived; | ||
| 330 | |||
| 331 | typedef ScaledJointMotionSubspace<typename traits<Joint>::Constraint_t> Constraint_t; | ||
| 332 | typedef typename traits<Joint>::Transformation_t Transformation_t; | ||
| 333 | typedef typename traits<Joint>::Motion_t Motion_t; | ||
| 334 | typedef typename traits<Joint>::Bias_t Bias_t; | ||
| 335 | |||
| 336 | // [ABA] | ||
| 337 | typedef typename traits<Joint>::U_t U_t; | ||
| 338 | typedef typename traits<Joint>::D_t D_t; | ||
| 339 | typedef typename traits<Joint>::UD_t UD_t; | ||
| 340 | |||
| 341 | typedef typename traits<Joint>::ConfigVector_t ConfigVector_t; | ||
| 342 | typedef typename traits<Joint>::TangentVector_t TangentVector_t; | ||
| 343 | |||
| 344 | PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE | ||
| 345 | }; | ||
| 346 | |||
| 347 | template<class Joint> | ||
| 348 | struct traits<JointDataMimic<Joint>> | ||
| 349 | { | ||
| 350 | typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; | ||
| 351 | typedef typename traits<JointDerived>::Scalar Scalar; | ||
| 352 | }; | ||
| 353 | |||
| 354 | template<class Joint> | ||
| 355 | struct traits<JointModelMimic<Joint>> | ||
| 356 | { | ||
| 357 | typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; | ||
| 358 | typedef typename traits<JointDerived>::Scalar Scalar; | ||
| 359 | }; | ||
| 360 | |||
| 361 | template<class JointData> | ||
| 362 | struct JointDataMimic : public JointDataBase<JointDataMimic<JointData>> | ||
| 363 | { | ||
| 364 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 365 | |||
| 366 | typedef typename traits<JointDataMimic>::JointDerived JointDerived; | ||
| 367 | typedef JointDataBase<JointDataMimic<JointData>> Base; | ||
| 368 | |||
| 369 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
| 370 | |||
| 371 | 120 | JointDataMimic() | |
| 372 | 120 | : m_scaling((Scalar)0) | |
| 373 |
1/2✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
|
120 | , joint_q(ConfigVector_t::Zero()) |
| 374 |
1/2✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
|
120 | , joint_v(TangentVector_t::Zero()) |
| 375 | 240 | , S((Scalar)0) | |
| 376 | { | ||
| 377 | 120 | } | |
| 378 | |||
| 379 | 168 | JointDataMimic(const JointDataMimic & other) | |
| 380 | 168 | { | |
| 381 | 168 | *this = other; | |
| 382 | 168 | } | |
| 383 | |||
| 384 | 12 | JointDataMimic(const JointDataBase<JointData> & jdata, const Scalar & scaling) | |
| 385 | 12 | : m_jdata_ref(jdata.derived()) | |
| 386 | 12 | , m_scaling(scaling) | |
| 387 | 24 | , S(m_jdata_ref.S, scaling) | |
| 388 | { | ||
| 389 | 12 | } | |
| 390 | |||
| 391 | 168 | JointDataMimic & operator=(const JointDataMimic & other) | |
| 392 | { | ||
| 393 | 168 | m_jdata_ref = other.m_jdata_ref; | |
| 394 | 168 | m_scaling = other.m_scaling; | |
| 395 | 168 | joint_q = other.joint_q; | |
| 396 | 168 | joint_v = other.joint_v; | |
| 397 | 168 | S = Constraint_t(m_jdata_ref.S, other.m_scaling); | |
| 398 | 168 | return *this; | |
| 399 | } | ||
| 400 | |||
| 401 | using Base::isEqual; | ||
| 402 | ✗ | bool isEqual(const JointDataMimic & other) const | |
| 403 | { | ||
| 404 | ✗ | return Base::isEqual(other) && internal::comparison_eq(m_jdata_ref, other.m_jdata_ref) | |
| 405 | ✗ | && internal::comparison_eq(m_scaling, other.m_scaling) | |
| 406 | ✗ | && internal::comparison_eq(joint_q, other.joint_q) | |
| 407 | ✗ | && internal::comparison_eq(joint_v, other.joint_v); | |
| 408 | } | ||
| 409 | |||
| 410 | 240 | static std::string classname() | |
| 411 | { | ||
| 412 |
5/10✓ Branch 2 taken 120 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 120 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 120 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 120 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 120 times.
✗ Branch 16 not taken.
|
240 | return std::string("JointDataMimic<") + JointData::classname() + std::string(">"); |
| 413 | } | ||
| 414 | |||
| 415 | ✗ | std::string shortname() const | |
| 416 | { | ||
| 417 | ✗ | return std::string("JointDataMimic<") + m_jdata_ref.shortname() + std::string(">"); | |
| 418 | } | ||
| 419 | |||
| 420 | // Accessors | ||
| 421 | ✗ | ConfigVectorTypeConstRef joint_q_accessor() const | |
| 422 | { | ||
| 423 | ✗ | return joint_q; | |
| 424 | } | ||
| 425 | ✗ | ConfigVectorTypeRef joint_q_accessor() | |
| 426 | { | ||
| 427 | ✗ | return joint_q; | |
| 428 | } | ||
| 429 | |||
| 430 | ✗ | TangentVectorTypeConstRef joint_v_accessor() const | |
| 431 | { | ||
| 432 | ✗ | return joint_v; | |
| 433 | } | ||
| 434 | ✗ | TangentVectorTypeRef joint_v_accessor() | |
| 435 | { | ||
| 436 | ✗ | return joint_v; | |
| 437 | } | ||
| 438 | |||
| 439 | ✗ | ConstraintTypeConstRef S_accessor() const | |
| 440 | { | ||
| 441 | ✗ | return S; | |
| 442 | } | ||
| 443 | ✗ | ConstraintTypeRef S_accessor() | |
| 444 | { | ||
| 445 | ✗ | return S; | |
| 446 | } | ||
| 447 | |||
| 448 | ✗ | TansformTypeConstRef M_accessor() const | |
| 449 | { | ||
| 450 | ✗ | return m_jdata_ref.M; | |
| 451 | } | ||
| 452 | ✗ | TansformTypeRef M_accessor() | |
| 453 | { | ||
| 454 | ✗ | return m_jdata_ref.M; | |
| 455 | } | ||
| 456 | |||
| 457 | ✗ | MotionTypeConstRef v_accessor() const | |
| 458 | { | ||
| 459 | ✗ | return m_jdata_ref.v; | |
| 460 | } | ||
| 461 | ✗ | MotionTypeRef v_accessor() | |
| 462 | { | ||
| 463 | ✗ | return m_jdata_ref.v; | |
| 464 | } | ||
| 465 | |||
| 466 | ✗ | BiasTypeConstRef c_accessor() const | |
| 467 | { | ||
| 468 | ✗ | return m_jdata_ref.c; | |
| 469 | } | ||
| 470 | ✗ | BiasTypeRef c_accessor() | |
| 471 | { | ||
| 472 | ✗ | return m_jdata_ref.c; | |
| 473 | } | ||
| 474 | |||
| 475 | ✗ | UTypeConstRef U_accessor() const | |
| 476 | { | ||
| 477 | ✗ | return m_jdata_ref.U; | |
| 478 | } | ||
| 479 | ✗ | UTypeRef U_accessor() | |
| 480 | { | ||
| 481 | ✗ | return m_jdata_ref.U; | |
| 482 | } | ||
| 483 | |||
| 484 | ✗ | DTypeConstRef Dinv_accessor() const | |
| 485 | { | ||
| 486 | ✗ | return m_jdata_ref.Dinv; | |
| 487 | } | ||
| 488 | ✗ | DTypeRef Dinv_accessor() | |
| 489 | { | ||
| 490 | ✗ | return m_jdata_ref.Dinv; | |
| 491 | } | ||
| 492 | |||
| 493 | ✗ | UDTypeConstRef UDinv_accessor() const | |
| 494 | { | ||
| 495 | ✗ | return m_jdata_ref.UDinv; | |
| 496 | } | ||
| 497 | ✗ | UDTypeRef UDinv_accessor() | |
| 498 | { | ||
| 499 | ✗ | return m_jdata_ref.UDinv; | |
| 500 | } | ||
| 501 | |||
| 502 | ✗ | DTypeConstRef StU_accessor() const | |
| 503 | { | ||
| 504 | ✗ | return m_jdata_ref.StU; | |
| 505 | } | ||
| 506 | ✗ | DTypeRef StU_accessor() | |
| 507 | { | ||
| 508 | ✗ | return m_jdata_ref.StU; | |
| 509 | } | ||
| 510 | |||
| 511 | template<class JointModel> | ||
| 512 | friend struct JointModelMimic; | ||
| 513 | |||
| 514 | const JointData & jdata() const | ||
| 515 | { | ||
| 516 | return m_jdata_ref; | ||
| 517 | } | ||
| 518 | ✗ | JointData & jdata() | |
| 519 | { | ||
| 520 | ✗ | return m_jdata_ref; | |
| 521 | } | ||
| 522 | |||
| 523 | const Scalar & scaling() const | ||
| 524 | { | ||
| 525 | return m_scaling; | ||
| 526 | } | ||
| 527 | ✗ | Scalar & scaling() | |
| 528 | { | ||
| 529 | ✗ | return m_scaling; | |
| 530 | } | ||
| 531 | |||
| 532 | ConfigVector_t & jointConfiguration() | ||
| 533 | { | ||
| 534 | return joint_q; | ||
| 535 | } | ||
| 536 | const ConfigVector_t & jointConfiguration() const | ||
| 537 | { | ||
| 538 | return joint_q; | ||
| 539 | } | ||
| 540 | |||
| 541 | TangentVector_t & jointVelocity() | ||
| 542 | { | ||
| 543 | return joint_v; | ||
| 544 | } | ||
| 545 | const TangentVector_t & jointVelocity() const | ||
| 546 | { | ||
| 547 | return joint_v; | ||
| 548 | } | ||
| 549 | |||
| 550 | protected: | ||
| 551 | JointData m_jdata_ref; | ||
| 552 | Scalar m_scaling; | ||
| 553 | |||
| 554 | /// \brief Transform configuration vector | ||
| 555 | ConfigVector_t joint_q; | ||
| 556 | /// \brief Transform velocity vector. | ||
| 557 | TangentVector_t joint_v; | ||
| 558 | |||
| 559 | public: | ||
| 560 | // data | ||
| 561 | Constraint_t S; | ||
| 562 | |||
| 563 | }; // struct JointDataMimic | ||
| 564 | |||
| 565 | template<typename NewScalar, typename JointModel> | ||
| 566 | struct CastType<NewScalar, JointModelMimic<JointModel>> | ||
| 567 | { | ||
| 568 | typedef typename CastType<NewScalar, JointModel>::type JointModelNewType; | ||
| 569 | typedef JointModelMimic<JointModelNewType> type; | ||
| 570 | }; | ||
| 571 | |||
| 572 | template<class JointModel> | ||
| 573 | struct JointModelMimic : public JointModelBase<JointModelMimic<JointModel>> | ||
| 574 | { | ||
| 575 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 576 | |||
| 577 | typedef typename traits<JointModelMimic>::JointDerived JointDerived; | ||
| 578 | |||
| 579 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
| 580 | |||
| 581 | typedef JointModelBase<JointModelMimic> Base; | ||
| 582 | using Base::id; | ||
| 583 | using Base::idx_q; | ||
| 584 | using Base::idx_v; | ||
| 585 | using Base::nq; | ||
| 586 | using Base::nv; | ||
| 587 | using Base::setIndexes; | ||
| 588 | |||
| 589 | 120 | JointModelMimic() | |
| 590 | 120 | { | |
| 591 | 120 | } | |
| 592 | |||
| 593 | ✗ | JointModelMimic( | |
| 594 | const JointModelBase<JointModel> & jmodel, const Scalar & scaling, const Scalar & offset) | ||
| 595 | ✗ | : m_jmodel_ref(jmodel.derived()) | |
| 596 | ✗ | , m_scaling(scaling) | |
| 597 | ✗ | , m_offset(offset) | |
| 598 | { | ||
| 599 | } | ||
| 600 | |||
| 601 | Base & base() | ||
| 602 | { | ||
| 603 | return *static_cast<Base *>(this); | ||
| 604 | } | ||
| 605 | const Base & base() const | ||
| 606 | { | ||
| 607 | return *static_cast<const Base *>(this); | ||
| 608 | } | ||
| 609 | |||
| 610 | ✗ | inline int nq_impl() const | |
| 611 | { | ||
| 612 | ✗ | return 0; | |
| 613 | } | ||
| 614 | ✗ | inline int nv_impl() const | |
| 615 | { | ||
| 616 | ✗ | return 0; | |
| 617 | } | ||
| 618 | |||
| 619 | 18 | inline int idx_q_impl() const | |
| 620 | { | ||
| 621 | 18 | return m_jmodel_ref.idx_q(); | |
| 622 | } | ||
| 623 | ✗ | inline int idx_v_impl() const | |
| 624 | { | ||
| 625 | 33 | return m_jmodel_ref.idx_v(); | |
| 626 | } | ||
| 627 | |||
| 628 | ✗ | void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/) | |
| 629 | { | ||
| 630 | ✗ | Base::i_id = id; // Only the id of the joint in the model is different. | |
| 631 | ✗ | Base::i_q = m_jmodel_ref.idx_q(); | |
| 632 | ✗ | Base::i_v = m_jmodel_ref.idx_v(); | |
| 633 | } | ||
| 634 | |||
| 635 | 12 | JointDataDerived createData() const | |
| 636 | { | ||
| 637 |
1/2✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
12 | return JointDataDerived(m_jmodel_ref.createData(), scaling()); |
| 638 | } | ||
| 639 | |||
| 640 | ✗ | const std::vector<bool> hasConfigurationLimit() const | |
| 641 | { | ||
| 642 | ✗ | return m_jmodel_ref.hasConfigurationLimit(); | |
| 643 | } | ||
| 644 | |||
| 645 | ✗ | const std::vector<bool> hasConfigurationLimitInTangent() const | |
| 646 | { | ||
| 647 | ✗ | return m_jmodel_ref.hasConfigurationLimitInTangent(); | |
| 648 | } | ||
| 649 | |||
| 650 | template<typename ConfigVector> | ||
| 651 | EIGEN_DONT_INLINE void | ||
| 652 | ✗ | calc(JointDataDerived & jdata, const typename Eigen::MatrixBase<ConfigVector> & qs) const | |
| 653 | { | ||
| 654 | typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform; | ||
| 655 | |||
| 656 | ✗ | AffineTransform::run(qs.head(m_jmodel_ref.nq()), m_scaling, m_offset, jdata.joint_q); | |
| 657 | ✗ | m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.joint_q); | |
| 658 | } | ||
| 659 | |||
| 660 | template<typename TangentVector> | ||
| 661 | ✗ | EIGEN_DONT_INLINE void calc( | |
| 662 | JointDataDerived & jdata, | ||
| 663 | const Blank blank, | ||
| 664 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
| 665 | { | ||
| 666 | ✗ | jdata.joint_v = m_scaling * vs.head(m_jmodel_ref.nv()); | |
| 667 | ✗ | m_jmodel_ref.calc(jdata.m_jdata_ref, blank, jdata.joint_v); | |
| 668 | } | ||
| 669 | |||
| 670 | template<typename ConfigVector, typename TangentVector> | ||
| 671 | ✗ | EIGEN_DONT_INLINE void calc( | |
| 672 | JointDataDerived & jdata, | ||
| 673 | const typename Eigen::MatrixBase<ConfigVector> & qs, | ||
| 674 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
| 675 | { | ||
| 676 | typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform; | ||
| 677 | |||
| 678 | ✗ | AffineTransform::run(qs.head(m_jmodel_ref.nq()), m_scaling, m_offset, jdata.joint_q); | |
| 679 | ✗ | jdata.joint_v = m_scaling * vs.head(m_jmodel_ref.nv()); | |
| 680 | ✗ | m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.joint_q, jdata.joint_v); | |
| 681 | } | ||
| 682 | |||
| 683 | template<typename VectorLike, typename Matrix6Like> | ||
| 684 | ✗ | void calc_aba( | |
| 685 | JointDataDerived & data, | ||
| 686 | const Eigen::MatrixBase<VectorLike> & armature, | ||
| 687 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
| 688 | const bool update_I) const | ||
| 689 | { | ||
| 690 | // TODO: fixme | ||
| 691 | ✗ | m_jmodel_ref.calc_aba( | |
| 692 | ✗ | data.m_jdata_ref, armature, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I); | |
| 693 | } | ||
| 694 | |||
| 695 | 240 | static std::string classname() | |
| 696 | { | ||
| 697 |
5/10✓ Branch 2 taken 120 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 120 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 120 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 120 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 120 times.
✗ Branch 16 not taken.
|
240 | return std::string("JointModelMimic<") + JointModel::classname() + std::string(">"); |
| 698 | ; | ||
| 699 | } | ||
| 700 | |||
| 701 | ✗ | std::string shortname() const | |
| 702 | { | ||
| 703 | ✗ | return std::string("JointModelMimic<") + m_jmodel_ref.shortname() + std::string(">"); | |
| 704 | } | ||
| 705 | |||
| 706 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 707 | template<typename NewScalar> | ||
| 708 | ✗ | typename CastType<NewScalar, JointModelMimic>::type cast() const | |
| 709 | { | ||
| 710 | typedef typename CastType<NewScalar, JointModelMimic>::type ReturnType; | ||
| 711 | |||
| 712 | ✗ | ReturnType res( | |
| 713 | ✗ | m_jmodel_ref.template cast<NewScalar>(), pinocchio::cast<NewScalar>(m_scaling), | |
| 714 | ✗ | pinocchio::cast<NewScalar>(m_offset)); | |
| 715 | ✗ | res.setIndexes(id(), idx_q(), idx_v()); | |
| 716 | ✗ | return res; | |
| 717 | } | ||
| 718 | |||
| 719 | ✗ | const JointModel & jmodel() const | |
| 720 | { | ||
| 721 | ✗ | return m_jmodel_ref; | |
| 722 | } | ||
| 723 | ✗ | JointModel & jmodel() | |
| 724 | { | ||
| 725 | ✗ | return m_jmodel_ref; | |
| 726 | } | ||
| 727 | |||
| 728 | 12 | const Scalar & scaling() const | |
| 729 | { | ||
| 730 | 12 | return m_scaling; | |
| 731 | } | ||
| 732 | ✗ | Scalar & scaling() | |
| 733 | { | ||
| 734 | ✗ | return m_scaling; | |
| 735 | } | ||
| 736 | |||
| 737 | const Scalar & offset() const | ||
| 738 | { | ||
| 739 | return m_offset; | ||
| 740 | } | ||
| 741 | ✗ | Scalar & offset() | |
| 742 | { | ||
| 743 | ✗ | return m_offset; | |
| 744 | } | ||
| 745 | |||
| 746 | protected: | ||
| 747 | // data | ||
| 748 | JointModel m_jmodel_ref; | ||
| 749 | Scalar m_scaling, m_offset; | ||
| 750 | |||
| 751 | public: | ||
| 752 | /* Acces to dedicated segment in robot config space. */ | ||
| 753 | // Const access | ||
| 754 | template<typename D> | ||
| 755 | typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType | ||
| 756 | ✗ | jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const | |
| 757 | { | ||
| 758 | ✗ | return SizeDepType<NQ>::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); | |
| 759 | } | ||
| 760 | |||
| 761 | // Non-const access | ||
| 762 | template<typename D> | ||
| 763 | typename SizeDepType<NQ>::template SegmentReturn<D>::Type | ||
| 764 | ✗ | jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const | |
| 765 | { | ||
| 766 | ✗ | return SizeDepType<NQ>::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); | |
| 767 | } | ||
| 768 | |||
| 769 | /* Acces to dedicated segment in robot config velocity space. */ | ||
| 770 | // Const access | ||
| 771 | template<typename D> | ||
| 772 | typename SizeDepType<NV>::template SegmentReturn<D>::ConstType | ||
| 773 | ✗ | jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const | |
| 774 | { | ||
| 775 | ✗ | return SizeDepType<NV>::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); | |
| 776 | } | ||
| 777 | |||
| 778 | // Non-const access | ||
| 779 | template<typename D> | ||
| 780 | typename SizeDepType<NV>::template SegmentReturn<D>::Type | ||
| 781 | ✗ | jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const | |
| 782 | { | ||
| 783 | ✗ | return SizeDepType<NV>::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); | |
| 784 | } | ||
| 785 | |||
| 786 | /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ | ||
| 787 | // Const access | ||
| 788 | template<typename D> | ||
| 789 | typename SizeDepType<NV>::template ColsReturn<D>::ConstType | ||
| 790 | ✗ | jointCols_impl(const Eigen::MatrixBase<D> & A) const | |
| 791 | { | ||
| 792 | ✗ | return SizeDepType<NV>::middleCols(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); | |
| 793 | } | ||
| 794 | |||
| 795 | // Non-const access | ||
| 796 | template<typename D> | ||
| 797 | typename SizeDepType<NV>::template ColsReturn<D>::Type | ||
| 798 | ✗ | jointCols_impl(Eigen::MatrixBase<D> & A) const | |
| 799 | { | ||
| 800 | ✗ | return SizeDepType<NV>::middleCols(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); | |
| 801 | } | ||
| 802 | |||
| 803 | /* Acces to dedicated rows in a matrix.*/ | ||
| 804 | // Const access | ||
| 805 | template<typename D> | ||
| 806 | typename SizeDepType<NV>::template RowsReturn<D>::ConstType | ||
| 807 | ✗ | jointRows_impl(const Eigen::MatrixBase<D> & A) const | |
| 808 | { | ||
| 809 | ✗ | return SizeDepType<NV>::middleRows(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); | |
| 810 | } | ||
| 811 | |||
| 812 | // Non-const access | ||
| 813 | template<typename D> | ||
| 814 | typename SizeDepType<NV>::template RowsReturn<D>::Type | ||
| 815 | ✗ | jointRows_impl(Eigen::MatrixBase<D> & A) const | |
| 816 | { | ||
| 817 | ✗ | return SizeDepType<NV>::middleRows(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); | |
| 818 | } | ||
| 819 | |||
| 820 | /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the | ||
| 821 | /// matrix Mat | ||
| 822 | // Const access | ||
| 823 | template<typename D> | ||
| 824 | typename SizeDepType<NV>::template BlockReturn<D>::ConstType | ||
| 825 | jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const | ||
| 826 | { | ||
| 827 | return SizeDepType<NV>::block( | ||
| 828 | Mat.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv(), | ||
| 829 | m_jmodel_ref.nv()); | ||
| 830 | } | ||
| 831 | |||
| 832 | // Non-const access | ||
| 833 | template<typename D> | ||
| 834 | typename SizeDepType<NV>::template BlockReturn<D>::Type | ||
| 835 | ✗ | jointBlock_impl(Eigen::MatrixBase<D> & Mat) const | |
| 836 | { | ||
| 837 | ✗ | return SizeDepType<NV>::block( | |
| 838 | ✗ | Mat.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv(), | |
| 839 | ✗ | m_jmodel_ref.nv()); | |
| 840 | } | ||
| 841 | |||
| 842 | }; // struct JointModelMimic | ||
| 843 | |||
| 844 | } // namespace pinocchio | ||
| 845 | |||
| 846 | #endif // ifndef __pinocchio_multibody_joint_mimic_hpp__ | ||
| 847 |