| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/joint/joint-mimic.hpp |
| Date: | 2025-04-30 16:14:33 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 221 | 233 | 94.8% |
| Branches: | 98 | 212 | 46.2% |
| 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/multibody/joint/fwd.hpp" | ||
| 9 | #include "pinocchio/multibody/joint/joint-collection.hpp" | ||
| 10 | #include "pinocchio/macros.hpp" | ||
| 11 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
| 12 | #include "pinocchio/multibody/joint/joint-basic-visitors.hpp" | ||
| 13 | |||
| 14 | namespace pinocchio | ||
| 15 | { | ||
| 16 | template<typename _Scalar, int _Options, int MaxDim> | ||
| 17 | struct ScaledJointMotionSubspaceTpl; | ||
| 18 | |||
| 19 | template<typename _Scalar, int _Options, int _MaxDim> | ||
| 20 | struct traits<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>> | ||
| 21 | { | ||
| 22 | enum | ||
| 23 | { | ||
| 24 | MaxDim = _MaxDim | ||
| 25 | }; | ||
| 26 | typedef JointMotionSubspaceTpl<Eigen::Dynamic, _Scalar, _Options, _MaxDim> | ||
| 27 | RefJointMotionSubspace; | ||
| 28 | typedef typename traits<RefJointMotionSubspace>::Scalar Scalar; | ||
| 29 | enum | ||
| 30 | { | ||
| 31 | Options = traits<RefJointMotionSubspace>::Options | ||
| 32 | }; | ||
| 33 | enum | ||
| 34 | { | ||
| 35 | LINEAR = traits<RefJointMotionSubspace>::LINEAR, | ||
| 36 | ANGULAR = traits<RefJointMotionSubspace>::ANGULAR | ||
| 37 | }; | ||
| 38 | typedef typename traits<RefJointMotionSubspace>::JointMotion JointMotion; | ||
| 39 | typedef typename traits<RefJointMotionSubspace>::JointForce JointForce; | ||
| 40 | typedef typename traits<RefJointMotionSubspace>::DenseBase DenseBase; | ||
| 41 | typedef typename traits<RefJointMotionSubspace>::MatrixReturnType MatrixReturnType; | ||
| 42 | typedef typename traits<RefJointMotionSubspace>::ConstMatrixReturnType ConstMatrixReturnType; | ||
| 43 | }; // traits ScaledJointMotionSubspaceTpl | ||
| 44 | |||
| 45 | template<typename _Scalar, int _Options, int _MaxDim> | ||
| 46 | struct SE3GroupAction<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>> | ||
| 47 | { | ||
| 48 | typedef typename SE3GroupAction<typename traits< | ||
| 49 | ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>::RefJointMotionSubspace>::ReturnType | ||
| 50 | ReturnType; | ||
| 51 | }; | ||
| 52 | |||
| 53 | template<typename _Scalar, int _Options, int _MaxDim, typename MotionDerived> | ||
| 54 | struct MotionAlgebraAction< | ||
| 55 | ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>, | ||
| 56 | MotionDerived> | ||
| 57 | { | ||
| 58 | typedef typename MotionAlgebraAction< | ||
| 59 | typename traits< | ||
| 60 | ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>::RefJointMotionSubspace, | ||
| 61 | MotionDerived>::ReturnType ReturnType; | ||
| 62 | }; | ||
| 63 | |||
| 64 | template<typename _Scalar, int _Options, int _MaxDim, typename ForceDerived> | ||
| 65 | struct ConstraintForceOp<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>, ForceDerived> | ||
| 66 | { | ||
| 67 | typedef | ||
| 68 | typename ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>::RefJointMotionSubspace | ||
| 69 | RefJointMotionSubspace; | ||
| 70 | typedef | ||
| 71 | typename ConstraintForceOp<RefJointMotionSubspace, ForceDerived>::ReturnType RefReturnType; | ||
| 72 | typedef typename ScalarMatrixProduct<_Scalar, RefReturnType>::type ReturnType; | ||
| 73 | }; | ||
| 74 | |||
| 75 | template<typename _Scalar, int _Options, int _MaxDim, typename ForceSet> | ||
| 76 | struct ConstraintForceSetOp<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>, ForceSet> | ||
| 77 | { | ||
| 78 | typedef | ||
| 79 | typename ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>::RefJointMotionSubspace | ||
| 80 | RefJointMotionSubspace; | ||
| 81 | typedef | ||
| 82 | typename ConstraintForceSetOp<RefJointMotionSubspace, ForceSet>::ReturnType RefReturnType; | ||
| 83 | typedef typename ScalarMatrixProduct<_Scalar, RefReturnType>::type ReturnType; | ||
| 84 | }; | ||
| 85 | |||
| 86 | template<typename _Scalar, int _Options, int _MaxDim> | ||
| 87 | struct ScaledJointMotionSubspaceTpl | ||
| 88 | : JointMotionSubspaceBase<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>> | ||
| 89 | { | ||
| 90 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 91 | |||
| 92 | PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledJointMotionSubspaceTpl) | ||
| 93 | enum | ||
| 94 | { | ||
| 95 | NV = Eigen::Dynamic, | ||
| 96 | MaxDim = _MaxDim | ||
| 97 | }; | ||
| 98 | typedef JointMotionSubspaceBase<ScaledJointMotionSubspaceTpl> Base; | ||
| 99 | using Base::nv; | ||
| 100 | |||
| 101 | typedef typename traits<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>:: | ||
| 102 | RefJointMotionSubspace RefJointMotionSubspace; | ||
| 103 | typedef typename SE3GroupAction<RefJointMotionSubspace>::ReturnType SE3ActionReturnType; | ||
| 104 | |||
| 105 | 5061 | ScaledJointMotionSubspaceTpl() | |
| 106 | 5061 | : m_constraint(0) | |
| 107 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
5061 | , m_scaling_factor(Scalar(1)) |
| 108 | { | ||
| 109 | 5061 | } | |
| 110 | |||
| 111 | 80 | explicit ScaledJointMotionSubspaceTpl(const Scalar & scaling_factor) | |
| 112 | 80 | : m_constraint(0) | |
| 113 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
80 | , m_scaling_factor(scaling_factor) |
| 114 | { | ||
| 115 | 80 | } | |
| 116 | |||
| 117 | template<typename ConstraintTpl> | ||
| 118 | 1707 | ScaledJointMotionSubspaceTpl(const ConstraintTpl & constraint, const Scalar & scaling_factor) | |
| 119 |
1/2✓ Branch 2 taken 1688 times.
✗ Branch 3 not taken.
|
1707 | : m_constraint(constraint) |
| 120 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
1707 | , m_scaling_factor(scaling_factor) |
| 121 | { | ||
| 122 | 1707 | } | |
| 123 | |||
| 124 | 5061 | ScaledJointMotionSubspaceTpl(const ScaledJointMotionSubspaceTpl & other) | |
| 125 | 5061 | : m_constraint(other.m_constraint) | |
| 126 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
5061 | , m_scaling_factor(other.m_scaling_factor) |
| 127 | { | ||
| 128 | 5061 | } | |
| 129 | |||
| 130 | 5061 | ScaledJointMotionSubspaceTpl & operator=(const ScaledJointMotionSubspaceTpl & other) | |
| 131 | { | ||
| 132 | 5061 | m_constraint = other.m_constraint; | |
| 133 | 5061 | m_scaling_factor = other.m_scaling_factor; | |
| 134 | 5061 | return *this; | |
| 135 | } | ||
| 136 | |||
| 137 | template<typename VectorLike> | ||
| 138 | 266 | JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & v) const | |
| 139 | { | ||
| 140 | |||
| 141 |
2/4✓ Branch 2 taken 266 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 266 times.
|
266 | assert(v.size() == nv()); |
| 142 |
1/2✓ Branch 1 taken 266 times.
✗ Branch 2 not taken.
|
266 | JointMotion jm = m_constraint * v; |
| 143 |
1/4✓ Branch 1 taken 266 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
532 | return m_scaling_factor * jm; |
| 144 | } | ||
| 145 | |||
| 146 | template<typename S1, int O1> | ||
| 147 | 767 | SE3ActionReturnType se3Action(const SE3Tpl<S1, O1> & m) const | |
| 148 | { | ||
| 149 |
2/4✓ Branch 2 taken 767 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 767 times.
✗ Branch 6 not taken.
|
767 | return m_scaling_factor * m_constraint.se3Action(m); |
| 150 | } | ||
| 151 | |||
| 152 | template<typename S1, int O1> | ||
| 153 | 76 | SE3ActionReturnType se3ActionInverse(const SE3Tpl<S1, O1> & m) const | |
| 154 | { | ||
| 155 |
2/4✓ Branch 2 taken 76 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 76 times.
✗ Branch 6 not taken.
|
76 | return m_scaling_factor * m_constraint.se3ActionInverse(m); |
| 156 | } | ||
| 157 | |||
| 158 | 274 | int nv_impl() const | |
| 159 | { | ||
| 160 | 274 | return m_constraint.nv(); | |
| 161 | } | ||
| 162 | |||
| 163 | struct TransposeConst | ||
| 164 | { | ||
| 165 | const ScaledJointMotionSubspaceTpl & ref; | ||
| 166 | 320 | explicit TransposeConst(const ScaledJointMotionSubspaceTpl & ref) | |
| 167 | 320 | : ref(ref) | |
| 168 | { | ||
| 169 | 320 | } | |
| 170 | |||
| 171 | template<typename Derived> | ||
| 172 | // typename ConstraintForceOp<ScaledJointMotionSubspaceTpl, Derived>::ReturnType | ||
| 173 | 280 | JointForce operator*(const ForceDense<Derived> & f) const | |
| 174 | { | ||
| 175 |
3/6✓ Branch 2 taken 280 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 280 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 280 times.
✗ Branch 9 not taken.
|
280 | return ref.m_scaling_factor * (ref.m_constraint.transpose() * f); |
| 176 | } | ||
| 177 | |||
| 178 | /// [CRBA] MatrixBase operator* (RefConstraint::Transpose S, ForceSet::Block) | ||
| 179 | template<typename Derived> | ||
| 180 | typename ConstraintForceSetOp<ScaledJointMotionSubspaceTpl, Derived>::ReturnType | ||
| 181 | 40 | operator*(const Eigen::MatrixBase<Derived> & F) const | |
| 182 | { | ||
| 183 |
2/4✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
|
80 | return ref.m_scaling_factor * (ref.m_constraint.transpose() * F); |
| 184 | } | ||
| 185 | |||
| 186 | }; // struct TransposeConst | ||
| 187 | |||
| 188 | 320 | TransposeConst transpose() const | |
| 189 | { | ||
| 190 | 320 | return TransposeConst(*this); | |
| 191 | } | ||
| 192 | |||
| 193 | 2 | const DenseBase & matrix_impl() const | |
| 194 | { | ||
| 195 |
1/2✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
2 | S = m_scaling_factor * m_constraint.matrix_impl(); |
| 196 | 2 | return S; | |
| 197 | } | ||
| 198 | |||
| 199 | 60 | DenseBase & matrix_impl() | |
| 200 | { | ||
| 201 |
1/2✓ Branch 3 taken 30 times.
✗ Branch 4 not taken.
|
60 | S = m_scaling_factor * m_constraint.matrix_impl(); |
| 202 | 60 | return S; | |
| 203 | } | ||
| 204 | |||
| 205 | template<typename MotionDerived> | ||
| 206 | typename MotionAlgebraAction<ScaledJointMotionSubspaceTpl, MotionDerived>::ReturnType | ||
| 207 | 8 | motionAction(const MotionDense<MotionDerived> & m) const | |
| 208 | { | ||
| 209 |
2/4✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
|
8 | return m_scaling_factor * m_constraint.motionAction(m); |
| 210 | } | ||
| 211 | |||
| 212 | 25 | inline const Scalar & scaling() const | |
| 213 | { | ||
| 214 | 25 | return m_scaling_factor; | |
| 215 | } | ||
| 216 | 16 | inline Scalar & scaling() | |
| 217 | { | ||
| 218 | 16 | return m_scaling_factor; | |
| 219 | } | ||
| 220 | |||
| 221 | 25 | inline const RefJointMotionSubspace & constraint() const | |
| 222 | { | ||
| 223 | 25 | return m_constraint.derived(); | |
| 224 | } | ||
| 225 | 16 | inline RefJointMotionSubspace & constraint() | |
| 226 | { | ||
| 227 | 16 | return m_constraint.derived(); | |
| 228 | } | ||
| 229 | |||
| 230 | 8 | bool isEqual(const ScaledJointMotionSubspaceTpl & other) const | |
| 231 | { | ||
| 232 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
|
8 | return m_constraint == other.m_constraint && m_scaling_factor == other.m_scaling_factor; |
| 233 | } | ||
| 234 | |||
| 235 | protected: | ||
| 236 | RefJointMotionSubspace m_constraint; | ||
| 237 | Scalar m_scaling_factor; | ||
| 238 | mutable DenseBase S; | ||
| 239 | }; // struct ScaledJointMotionSubspaceTpl | ||
| 240 | |||
| 241 | template<typename S1, int O1, typename S2, int O2, int MD2> | ||
| 242 | struct MultiplicationOp<InertiaTpl<S1, O1>, ScaledJointMotionSubspaceTpl<S2, O2, MD2>> | ||
| 243 | { | ||
| 244 | typedef InertiaTpl<S1, O1> Inertia; | ||
| 245 | typedef ScaledJointMotionSubspaceTpl<S2, O2, MD2> Constraint; | ||
| 246 | typedef typename Constraint::Scalar Scalar; | ||
| 247 | |||
| 248 | typedef Eigen::Matrix<S2, 6, Eigen::Dynamic, O2, 6, MD2> ReturnType; | ||
| 249 | }; | ||
| 250 | |||
| 251 | /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ | ||
| 252 | namespace impl | ||
| 253 | { | ||
| 254 | template<typename S1, int O1, typename S2, int O2, int MD2> | ||
| 255 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, ScaledJointMotionSubspaceTpl<S2, O2, MD2>> | ||
| 256 | { | ||
| 257 | typedef InertiaTpl<S1, O1> Inertia; | ||
| 258 | typedef ScaledJointMotionSubspaceTpl<S2, O2, MD2> Constraint; | ||
| 259 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
| 260 | |||
| 261 | 25 | static inline ReturnType run(const Inertia & Y, const Constraint & scaled_constraint) | |
| 262 | { | ||
| 263 |
2/4✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25 times.
✗ Branch 8 not taken.
|
25 | return scaled_constraint.scaling() * (Y * scaled_constraint.constraint()); |
| 264 | } | ||
| 265 | }; | ||
| 266 | } // namespace impl | ||
| 267 | |||
| 268 | template<typename M6Like, typename S2, int O2, int MD2> | ||
| 269 | struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledJointMotionSubspaceTpl<S2, O2, MD2>> | ||
| 270 | { | ||
| 271 | typedef ScaledJointMotionSubspaceTpl<S2, O2, MD2> MotionSubspace; | ||
| 272 | typedef Eigen::Matrix<S2, 6, Eigen::Dynamic, O2, 6, MD2> ReturnType; | ||
| 273 | }; | ||
| 274 | |||
| 275 | /* [ABA] operator* (Inertia Y,Constraint S) */ | ||
| 276 | namespace impl | ||
| 277 | { | ||
| 278 | template<typename M6Like, typename S2, int O2, int MD2> | ||
| 279 | struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledJointMotionSubspaceTpl<S2, O2, MD2>> | ||
| 280 | { | ||
| 281 | typedef ScaledJointMotionSubspaceTpl<S2, O2, MD2> Constraint; | ||
| 282 | typedef | ||
| 283 | typename MultiplicationOp<Eigen::MatrixBase<M6Like>, Constraint>::ReturnType ReturnType; | ||
| 284 | |||
| 285 | static inline ReturnType | ||
| 286 | run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & scaled_constraint) | ||
| 287 | { | ||
| 288 | return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint()); | ||
| 289 | } | ||
| 290 | }; | ||
| 291 | } // namespace impl | ||
| 292 | |||
| 293 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
| 294 | struct JointMimicTpl; | ||
| 295 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
| 296 | struct JointModelMimicTpl; | ||
| 297 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
| 298 | struct JointDataMimicTpl; | ||
| 299 | |||
| 300 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
| 301 | struct traits<JointMimicTpl<_Scalar, _Options, JointCollectionTpl>> | ||
| 302 | { | ||
| 303 | typedef _Scalar Scalar; | ||
| 304 | |||
| 305 | enum | ||
| 306 | { | ||
| 307 | Options = _Options, | ||
| 308 | NQ = Eigen::Dynamic, | ||
| 309 | NV = Eigen::Dynamic, | ||
| 310 | NVExtended = Eigen::Dynamic, | ||
| 311 | MaxNVMimicked = 6 | ||
| 312 | }; | ||
| 313 | |||
| 314 | typedef JointCollectionTpl<Scalar, Options> JointCollection; | ||
| 315 | typedef JointDataMimicTpl<Scalar, Options, JointCollectionTpl> JointDataDerived; | ||
| 316 | typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointModelDerived; | ||
| 317 | |||
| 318 | typedef ScaledJointMotionSubspaceTpl<Scalar, Options, MaxNVMimicked> Constraint_t; | ||
| 319 | typedef SE3Tpl<Scalar, Options> Transformation_t; | ||
| 320 | typedef MotionTpl<Scalar, Options> Motion_t; | ||
| 321 | typedef MotionTpl<Scalar, Options> Bias_t; | ||
| 322 | |||
| 323 | // [ABA] | ||
| 324 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> U_t; | ||
| 325 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> D_t; | ||
| 326 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> UD_t; | ||
| 327 | |||
| 328 | typedef const Constraint_t & ConstraintTypeConstRef; | ||
| 329 | typedef Constraint_t & ConstraintTypeRef; | ||
| 330 | typedef Transformation_t TansformTypeConstRef; | ||
| 331 | typedef Transformation_t TansformTypeRef; | ||
| 332 | typedef Motion_t MotionTypeConstRef; | ||
| 333 | typedef Motion_t MotionTypeRef; | ||
| 334 | typedef Bias_t BiasTypeConstRef; | ||
| 335 | typedef Bias_t BiasTypeRef; | ||
| 336 | typedef U_t UTypeConstRef; | ||
| 337 | typedef U_t UTypeRef; | ||
| 338 | typedef D_t DTypeConstRef; | ||
| 339 | typedef D_t DTypeRef; | ||
| 340 | typedef UD_t UDTypeConstRef; | ||
| 341 | typedef UD_t UDTypeRef; | ||
| 342 | |||
| 343 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> ConfigVector_t; | ||
| 344 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> TangentVector_t; | ||
| 345 | |||
| 346 | typedef const ConfigVector_t & ConfigVectorTypeConstRef; | ||
| 347 | typedef ConfigVector_t & ConfigVectorTypeRef; | ||
| 348 | typedef const TangentVector_t TangentVectorTypeConstRef; | ||
| 349 | typedef TangentVector_t & TangentVectorTypeRef; | ||
| 350 | |||
| 351 | typedef boost::mpl::false_ is_mimicable_t; | ||
| 352 | }; | ||
| 353 | |||
| 354 | template<typename _Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
| 355 | struct traits<JointDataMimicTpl<_Scalar, Options, JointCollectionTpl>> | ||
| 356 | { | ||
| 357 | typedef JointMimicTpl<_Scalar, Options, JointCollectionTpl> JointDerived; | ||
| 358 | typedef _Scalar Scalar; | ||
| 359 | }; | ||
| 360 | |||
| 361 | template<typename _Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
| 362 | struct traits<JointModelMimicTpl<_Scalar, Options, JointCollectionTpl>> | ||
| 363 | { | ||
| 364 | typedef JointMimicTpl<_Scalar, Options, JointCollectionTpl> JointDerived; | ||
| 365 | typedef _Scalar Scalar; | ||
| 366 | }; | ||
| 367 | |||
| 368 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
| 369 | struct JointDataMimicTpl | ||
| 370 | : public JointDataBase<JointDataMimicTpl<_Scalar, _Options, JointCollectionTpl>> | ||
| 371 | { | ||
| 372 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 373 | |||
| 374 | typedef JointDataBase<JointDataMimicTpl> Base; | ||
| 375 | typedef JointMimicTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
| 376 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
| 377 | |||
| 378 | typedef JointDataTpl<_Scalar, _Options, JointCollectionTpl> RefJointData; | ||
| 379 | typedef typename RefJointData::JointDataVariant RefJointDataVariant; | ||
| 380 | |||
| 381 | 80 | JointDataMimicTpl() | |
| 382 |
6/12✓ Branch 3 taken 80 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 80 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 80 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 80 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 80 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 4 times.
✗ Branch 19 not taken.
|
80 | : S((Scalar)0) |
| 383 | { | ||
| 384 |
1/2✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
|
80 | joint_q.resize(0, 1); |
| 385 |
1/2✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
|
80 | joint_q_transformed.resize(0, 1); |
| 386 |
1/2✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
|
80 | joint_v.resize(0, 1); |
| 387 |
1/2✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
|
80 | joint_v_transformed.resize(0, 1); |
| 388 | 80 | } | |
| 389 | |||
| 390 | 1680 | JointDataMimicTpl( | |
| 391 | const RefJointData & jdata, const Scalar & scaling, const int & nq, const int & nv) | ||
| 392 | 1680 | : m_jdata_mimicking(checkMimic(jdata.derived())) | |
| 393 |
6/12✓ Branch 2 taken 1680 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1680 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1680 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1680 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1680 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1680 times.
✗ Branch 18 not taken.
|
3360 | , S(m_jdata_mimicking.S(), scaling) |
| 394 | { | ||
| 395 |
1/2✓ Branch 1 taken 1680 times.
✗ Branch 2 not taken.
|
1680 | joint_q.resize(nq, 1); |
| 396 |
1/2✓ Branch 1 taken 1680 times.
✗ Branch 2 not taken.
|
1680 | joint_q_transformed.resize(nq, 1); |
| 397 |
1/2✓ Branch 1 taken 1680 times.
✗ Branch 2 not taken.
|
1680 | joint_v.resize(nv, 1); |
| 398 |
1/2✓ Branch 1 taken 1680 times.
✗ Branch 2 not taken.
|
1680 | joint_v_transformed.resize(nv, 1); |
| 399 | 1680 | } | |
| 400 | |||
| 401 | 5061 | JointDataMimicTpl(const JointDataMimicTpl & other) | |
| 402 |
5/10✓ Branch 3 taken 5061 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5061 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5061 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5061 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 5061 times.
✗ Branch 16 not taken.
|
5061 | { |
| 403 |
1/2✓ Branch 1 taken 5061 times.
✗ Branch 2 not taken.
|
5061 | *this = other; |
| 404 | 5061 | } | |
| 405 | |||
| 406 | 5061 | JointDataMimicTpl & operator=(const JointDataMimicTpl & other) | |
| 407 | { | ||
| 408 | 5061 | m_jdata_mimicking = other.m_jdata_mimicking; | |
| 409 | 5061 | joint_q = other.joint_q; | |
| 410 | 5061 | joint_q_transformed = other.joint_q_transformed; | |
| 411 | 5061 | joint_v = other.joint_v; | |
| 412 | 5061 | joint_v_transformed = other.joint_v_transformed; | |
| 413 |
1/2✓ Branch 2 taken 5061 times.
✗ Branch 3 not taken.
|
5061 | S = Constraint_t(other.S); |
| 414 | 5061 | return *this; | |
| 415 | } | ||
| 416 | |||
| 417 | using Base::isEqual; | ||
| 418 | 8 | bool isEqual(const JointDataMimicTpl & other) const | |
| 419 | { | ||
| 420 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
16 | return Base::isEqual(other) && m_jdata_mimicking == other.m_jdata_mimicking |
| 421 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
8 | && joint_q == other.joint_q && joint_q_transformed == other.joint_q_transformed |
| 422 |
3/6✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
|
16 | && joint_v == other.joint_v && joint_v_transformed == other.joint_v_transformed; |
| 423 | } | ||
| 424 | |||
| 425 | 149 | static std::string classname() | |
| 426 | { | ||
| 427 |
1/2✓ Branch 2 taken 149 times.
✗ Branch 3 not taken.
|
149 | return std::string("JointDataMimic"); |
| 428 | } | ||
| 429 | |||
| 430 | 2 | std::string shortname() const | |
| 431 | { | ||
| 432 | 2 | return classname(); | |
| 433 | } | ||
| 434 | |||
| 435 | // // Accessors | ||
| 436 | 18 | ConstraintTypeConstRef S_accessor() const | |
| 437 | { | ||
| 438 | 18 | return S; | |
| 439 | } | ||
| 440 | 1430 | ConstraintTypeRef S_accessor() | |
| 441 | { | ||
| 442 | 1430 | return S; | |
| 443 | } | ||
| 444 | |||
| 445 | 1826 | Transformation_t M_accessor() const | |
| 446 | { | ||
| 447 | 1826 | return m_jdata_mimicking.M(); | |
| 448 | } | ||
| 449 | |||
| 450 | 1213 | Motion_t v_accessor() const | |
| 451 | { | ||
| 452 | 1213 | return m_jdata_mimicking.v(); | |
| 453 | } | ||
| 454 | |||
| 455 | 284 | Bias_t c_accessor() const | |
| 456 | { | ||
| 457 | 284 | return m_jdata_mimicking.c(); | |
| 458 | } | ||
| 459 | |||
| 460 | 18 | U_t U_accessor() const | |
| 461 | { | ||
| 462 | 18 | return m_jdata_mimicking.U(); | |
| 463 | } | ||
| 464 | |||
| 465 | 18 | D_t Dinv_accessor() const | |
| 466 | { | ||
| 467 | 18 | return m_jdata_mimicking.Dinv(); | |
| 468 | } | ||
| 469 | |||
| 470 | 18 | UD_t UDinv_accessor() const | |
| 471 | { | ||
| 472 | 18 | return m_jdata_mimicking.UDinv(); | |
| 473 | } | ||
| 474 | |||
| 475 | ✗ | D_t StU_accessor() const | |
| 476 | { | ||
| 477 | ✗ | return m_jdata_mimicking.StU(); | |
| 478 | } | ||
| 479 | |||
| 480 | friend struct JointModelMimicTpl<_Scalar, _Options, JointCollectionTpl>; | ||
| 481 | |||
| 482 | const RefJointData & jdata() const | ||
| 483 | { | ||
| 484 | return m_jdata_mimicking; | ||
| 485 | } | ||
| 486 | 16 | RefJointData & jdata() | |
| 487 | { | ||
| 488 | 16 | return m_jdata_mimicking; | |
| 489 | } | ||
| 490 | |||
| 491 | 16 | ConfigVectorTypeRef joint_q_accessor() | |
| 492 | { | ||
| 493 | 16 | return joint_q; | |
| 494 | } | ||
| 495 | 18 | ConfigVectorTypeConstRef joint_q_accessor() const | |
| 496 | { | ||
| 497 | 18 | return joint_q; | |
| 498 | } | ||
| 499 | |||
| 500 | 16 | ConfigVector_t & q_transformed() | |
| 501 | { | ||
| 502 | 16 | return joint_q_transformed; | |
| 503 | } | ||
| 504 | const ConfigVector_t & q_transformed() const | ||
| 505 | { | ||
| 506 | return joint_q_transformed; | ||
| 507 | } | ||
| 508 | 16 | TangentVectorTypeRef joint_v_accessor() | |
| 509 | { | ||
| 510 | 16 | return joint_v; | |
| 511 | } | ||
| 512 | 18 | TangentVectorTypeConstRef joint_v_accessor() const | |
| 513 | { | ||
| 514 | 18 | return joint_v; | |
| 515 | } | ||
| 516 | |||
| 517 | 16 | TangentVector_t & v_transformed() | |
| 518 | { | ||
| 519 | 16 | return joint_v_transformed; | |
| 520 | } | ||
| 521 | const TangentVector_t & v_transformed() const | ||
| 522 | { | ||
| 523 | return joint_v_transformed; | ||
| 524 | } | ||
| 525 | |||
| 526 | 1 | void disp(std::ostream & os) const | |
| 527 | { | ||
| 528 | 1 | Base::disp(os); | |
| 529 |
2/4✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
1 | os << " Mimicking joint data: " << m_jdata_mimicking.shortname() << std::endl; |
| 530 | 1 | } | |
| 531 | |||
| 532 | RefJointData m_jdata_mimicking; | ||
| 533 | |||
| 534 | /// \brief original configuration vector | ||
| 535 | ConfigVector_t joint_q; | ||
| 536 | /// \brief Transformed configuration vector | ||
| 537 | ConfigVector_t joint_q_transformed; | ||
| 538 | /// \brief original velocity vector | ||
| 539 | TangentVector_t joint_v; | ||
| 540 | /// \brief Transform velocity vector. | ||
| 541 | TangentVector_t joint_v_transformed; | ||
| 542 | // data | ||
| 543 | Constraint_t S; | ||
| 544 | }; // struct JointDataMimicTpl | ||
| 545 | |||
| 546 | template< | ||
| 547 | typename NewScalar, | ||
| 548 | typename Scalar, | ||
| 549 | int Options, | ||
| 550 | template<typename S, int O> class JointCollectionTpl> | ||
| 551 | struct CastType<NewScalar, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>> | ||
| 552 | { | ||
| 553 | typedef JointModelMimicTpl<NewScalar, Options, JointCollectionTpl> type; | ||
| 554 | }; | ||
| 555 | |||
| 556 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
| 557 | struct JointModelMimicTpl | ||
| 558 | : public JointModelBase<JointModelMimicTpl<_Scalar, _Options, JointCollectionTpl>> | ||
| 559 | { | ||
| 560 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 561 | |||
| 562 | typedef JointModelBase<JointModelMimicTpl> Base; | ||
| 563 | typedef JointMimicTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
| 564 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
| 565 | enum | ||
| 566 | { | ||
| 567 | MaxNVMimicked = traits<JointDerived>::MaxNVMimicked | ||
| 568 | }; | ||
| 569 | |||
| 570 | typedef JointCollectionTpl<Scalar, Options> JointCollection; | ||
| 571 | typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModel; | ||
| 572 | |||
| 573 | typedef SE3Tpl<Scalar, Options> SE3; | ||
| 574 | typedef MotionTpl<Scalar, Options> Motion; | ||
| 575 | typedef InertiaTpl<Scalar, Options> Inertia; | ||
| 576 | |||
| 577 | using Base::id; | ||
| 578 | using Base::idx_q; | ||
| 579 | using Base::idx_v; | ||
| 580 | using Base::idx_vExtended; | ||
| 581 | using Base::nq; | ||
| 582 | using Base::nv; | ||
| 583 | using Base::nvExtended; | ||
| 584 | using Base::setIndexes; | ||
| 585 | |||
| 586 | 95 | JointModelMimicTpl() | |
| 587 |
2/4✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
|
95 | { |
| 588 | 95 | } | |
| 589 | |||
| 590 | template<typename JointModel> | ||
| 591 | 53 | JointModelMimicTpl( | |
| 592 | const JointModelBase<JointModel> & jmodel, const Scalar & scaling, const Scalar & offset) | ||
| 593 | 53 | : JointModelMimicTpl(jmodel, jmodel, scaling, offset) | |
| 594 | { | ||
| 595 | 53 | } | |
| 596 | |||
| 597 | template<typename JointModelMimicking, typename JointModelMimicked> | ||
| 598 | 175 | JointModelMimicTpl( | |
| 599 | const JointModelBase<JointModelMimicking> & jmodel_mimicking, | ||
| 600 | const JointModelBase<JointModelMimicked> & jmodel_mimicked, | ||
| 601 | const Scalar & scaling, | ||
| 602 | const Scalar & offset) | ||
| 603 |
1/2✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
|
175 | : m_jmodel_mimicking(checkMimic((JointModel)jmodel_mimicking.derived())) |
| 604 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
175 | , m_scaling(scaling) |
| 605 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
175 | , m_offset(offset) |
| 606 |
1/2✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
|
175 | , m_nqExtended(jmodel_mimicking.nq()) |
| 607 |
1/2✓ Branch 2 taken 130 times.
✗ Branch 3 not taken.
|
350 | , m_nvExtended(jmodel_mimicking.nvExtended()) |
| 608 | { | ||
| 609 |
3/6✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 130 times.
|
175 | assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); |
| 610 |
3/6✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 130 times.
|
175 | assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); |
| 611 |
3/6✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 130 times.
|
175 | assert(jmodel_mimicking.nvExtended() == jmodel_mimicked.nvExtended()); |
| 612 | |||
| 613 |
5/10✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 130 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 130 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 130 times.
✗ Branch 14 not taken.
|
175 | setMimicIndexes( |
| 614 | jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), | ||
| 615 | jmodel_mimicked.idx_vExtended()); | ||
| 616 | 175 | } | |
| 617 | |||
| 618 | Base & base() | ||
| 619 | { | ||
| 620 | return *static_cast<Base *>(this); | ||
| 621 | } | ||
| 622 | const Base & base() const | ||
| 623 | { | ||
| 624 | return *static_cast<const Base *>(this); | ||
| 625 | } | ||
| 626 | |||
| 627 | 9878 | inline int nq_impl() const | |
| 628 | { | ||
| 629 | 9878 | return 0; | |
| 630 | } | ||
| 631 | 31466 | inline int nv_impl() const | |
| 632 | { | ||
| 633 | 31495 | return 0; | |
| 634 | } | ||
| 635 | 26343 | inline int nvExtended_impl() const | |
| 636 | { | ||
| 637 | 26357 | return m_nvExtended; | |
| 638 | } | ||
| 639 | |||
| 640 | /** | ||
| 641 | * @note q and v are ignored in the _impl for mimic joint because most algorithms will pass | ||
| 642 | * indexes of their current position in the tree, while in this case idx_q and idx_v should | ||
| 643 | * remain pointing to the mimicked joint. (See setMimicIndexes) | ||
| 644 | */ | ||
| 645 | 160 | void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/, int vExtended) | |
| 646 | { | ||
| 647 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 149 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
160 | PINOCCHIO_THROW( |
| 648 | (id > m_jmodel_mimicking.id()), std::invalid_argument, | ||
| 649 | "Mimic joint index is lower than its directing joint. Should never happen"); | ||
| 650 | 160 | Base::i_id = id; | |
| 651 | // Base::i_q = q; | ||
| 652 | // Base::i_v = v; | ||
| 653 | 160 | Base::i_vExtended = vExtended; | |
| 654 | 160 | } | |
| 655 | |||
| 656 | /** | ||
| 657 | * @brief Specific way for mimic joints to set the mimicked q,v indexes. | ||
| 658 | * Used for manipulating tree (e.g. appendModel) | ||
| 659 | * | ||
| 660 | * @param id Set the mimicking joint id | ||
| 661 | * @param q Set the mimic joint idx_q (should point to the mimicked joint) | ||
| 662 | * @param v Set the mimic joint idx_v (should point to the mimicked joint) | ||
| 663 | * @param vExtended Set the mimicking idx_vExtended | ||
| 664 | */ | ||
| 665 | 187 | void setMimicIndexes(JointIndex id, int q, int v, int vExtended) | |
| 666 | { | ||
| 667 | // Set idx_q, idx_v to zero because only the corresponding subsegment of q,v are passed to the | ||
| 668 | // m_jmodel_mimicking, thus, its indexes starts at 0 | ||
| 669 | 187 | m_jmodel_mimicking.setIndexes(id, 0, 0, vExtended); | |
| 670 | |||
| 671 | // idx_q, idx_v are kept separately to extract the subsegment | ||
| 672 | 187 | Base::i_q = q; | |
| 673 | 187 | Base::i_v = v; | |
| 674 | 187 | } | |
| 675 | |||
| 676 | 1680 | JointDataDerived createData() const | |
| 677 | { | ||
| 678 | return JointDataDerived( | ||
| 679 |
1/2✓ Branch 3 taken 1680 times.
✗ Branch 4 not taken.
|
1680 | m_jmodel_mimicking.createData(), scaling(), m_nqExtended, m_nvExtended); |
| 680 | } | ||
| 681 | |||
| 682 | ✗ | const std::vector<bool> hasConfigurationLimit() const | |
| 683 | { | ||
| 684 | ✗ | return m_jmodel_mimicking.hasConfigurationLimit(); | |
| 685 | } | ||
| 686 | |||
| 687 | ✗ | const std::vector<bool> hasConfigurationLimitInTangent() const | |
| 688 | { | ||
| 689 | ✗ | return m_jmodel_mimicking.hasConfigurationLimitInTangent(); | |
| 690 | } | ||
| 691 | |||
| 692 | template<typename ConfigVector> | ||
| 693 | PINOCCHIO_DONT_INLINE void | ||
| 694 | 1754 | calc(JointDataDerived & jdata, const typename Eigen::MatrixBase<ConfigVector> & qs) const | |
| 695 | { | ||
| 696 |
1/2✓ Branch 2 taken 877 times.
✗ Branch 3 not taken.
|
1754 | jdata.joint_q = qs.segment(Base::i_q, m_nqExtended); |
| 697 | 1754 | configVectorAffineTransform( | |
| 698 | 1754 | m_jmodel_mimicking, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed); | |
| 699 | 1754 | m_jmodel_mimicking.calc(jdata.m_jdata_mimicking, jdata.joint_q_transformed); | |
| 700 | 1754 | } | |
| 701 | |||
| 702 | template<typename ConfigVector, typename TangentVector> | ||
| 703 | 1859 | PINOCCHIO_DONT_INLINE void calc( | |
| 704 | JointDataDerived & jdata, | ||
| 705 | const typename Eigen::MatrixBase<ConfigVector> & qs, | ||
| 706 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
| 707 | { | ||
| 708 |
1/2✓ Branch 2 taken 930 times.
✗ Branch 3 not taken.
|
1859 | jdata.joint_q = qs.segment(Base::i_q, m_nqExtended); |
| 709 |
1/2✓ Branch 2 taken 930 times.
✗ Branch 3 not taken.
|
1859 | jdata.joint_v = vs.segment(Base::i_v, m_nvExtended); |
| 710 | 1859 | configVectorAffineTransform( | |
| 711 | 1859 | m_jmodel_mimicking, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed); | |
| 712 |
1/2✓ Branch 2 taken 930 times.
✗ Branch 3 not taken.
|
1859 | jdata.joint_v_transformed = m_scaling * jdata.joint_v; |
| 713 | |||
| 714 | 1859 | m_jmodel_mimicking.calc( | |
| 715 | 1859 | jdata.m_jdata_mimicking, jdata.joint_q_transformed, jdata.joint_v_transformed); | |
| 716 | 1859 | } | |
| 717 | |||
| 718 | template<typename VectorLike, typename Matrix6Like> | ||
| 719 | ✗ | void calc_aba( | |
| 720 | JointDataDerived &, | ||
| 721 | const Eigen::MatrixBase<VectorLike> &, | ||
| 722 | const Eigen::MatrixBase<Matrix6Like> &, | ||
| 723 | const bool) const | ||
| 724 | { | ||
| 725 | ✗ | assert( | |
| 726 | false | ||
| 727 | && "Joint Mimic is not supported for aba yet. Remove it from your model if you want to use " | ||
| 728 | "this function"); | ||
| 729 | } | ||
| 730 | |||
| 731 | 168 | static std::string classname() | |
| 732 | { | ||
| 733 |
1/2✓ Branch 2 taken 168 times.
✗ Branch 3 not taken.
|
168 | return std::string("JointModelMimic"); |
| 734 | } | ||
| 735 | |||
| 736 | 21 | std::string shortname() const | |
| 737 | { | ||
| 738 | 21 | return classname(); | |
| 739 | } | ||
| 740 | |||
| 741 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 742 | template<typename NewScalar> | ||
| 743 | 12 | typename CastType<NewScalar, JointModelMimicTpl>::type cast() const | |
| 744 | { | ||
| 745 | typedef typename CastType<NewScalar, JointModelMimicTpl>::type ReturnType; | ||
| 746 |
1/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
12 | ReturnType res( |
| 747 | m_jmodel_mimicking.template cast<NewScalar>(), | ||
| 748 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | ScalarCast<NewScalar, Scalar>::cast(m_scaling), |
| 749 |
0/2✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
12 | ScalarCast<NewScalar, Scalar>::cast(m_offset)); |
| 750 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | res.setIndexes(id(), Base::i_q, Base::i_v, Base::i_vExtended); |
| 751 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | res.setMimicIndexes(m_jmodel_mimicking.id(), Base::i_q, Base::i_v, Base::i_vExtended); |
| 752 | 12 | return res; | |
| 753 | } | ||
| 754 | |||
| 755 | 34 | const JointModel & jmodel() const | |
| 756 | { | ||
| 757 | 34 | return m_jmodel_mimicking; | |
| 758 | } | ||
| 759 | 84 | JointModel & jmodel() | |
| 760 | { | ||
| 761 | 84 | return m_jmodel_mimicking; | |
| 762 | } | ||
| 763 | |||
| 764 | 1680 | const Scalar & scaling() const | |
| 765 | { | ||
| 766 | 1680 | return m_scaling; | |
| 767 | } | ||
| 768 | 14 | Scalar & scaling() | |
| 769 | { | ||
| 770 | 14 | return m_scaling; | |
| 771 | } | ||
| 772 | |||
| 773 | const Scalar & offset() const | ||
| 774 | { | ||
| 775 | return m_offset; | ||
| 776 | } | ||
| 777 | 14 | Scalar & offset() | |
| 778 | { | ||
| 779 | 14 | return m_offset; | |
| 780 | } | ||
| 781 | |||
| 782 | protected: | ||
| 783 | // data | ||
| 784 | JointModel m_jmodel_mimicking; | ||
| 785 | Scalar m_scaling, m_offset; | ||
| 786 | int m_nqExtended, m_nvExtended; | ||
| 787 | |||
| 788 | public: | ||
| 789 | /* Acces to dedicated segment in robot config space. */ | ||
| 790 | // Const access | ||
| 791 | template<typename D> | ||
| 792 | typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType | ||
| 793 | 42 | JointMappedConfigSelector_impl(const Eigen::MatrixBase<D> & a) const | |
| 794 | { | ||
| 795 | 42 | return SizeDepType<NQ>::segment(a.derived(), Base::i_q, m_nqExtended); | |
| 796 | } | ||
| 797 | |||
| 798 | // Non-const access | ||
| 799 | template<typename D> | ||
| 800 | typename SizeDepType<NQ>::template SegmentReturn<D>::Type | ||
| 801 | 2 | JointMappedConfigSelector_impl(Eigen::MatrixBase<D> & a) const | |
| 802 | { | ||
| 803 | 2 | return SizeDepType<NQ>::segment(a.derived(), Base::i_q, m_nqExtended); | |
| 804 | } | ||
| 805 | /* Acces to dedicated segment in robot tangent space. */ | ||
| 806 | // Const access | ||
| 807 | template<typename D> | ||
| 808 | typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType | ||
| 809 | 258 | JointMappedVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const | |
| 810 | { | ||
| 811 | 258 | return SizeDepType<NQ>::segment(a.derived(), Base::i_v, m_nvExtended); | |
| 812 | } | ||
| 813 | |||
| 814 | // Non-const access | ||
| 815 | template<typename D> | ||
| 816 | typename SizeDepType<NQ>::template SegmentReturn<D>::Type | ||
| 817 | 272 | JointMappedVelocitySelector_impl(Eigen::MatrixBase<D> & a) const | |
| 818 | { | ||
| 819 | 272 | return SizeDepType<NQ>::segment(a.derived(), Base::i_v, m_nvExtended); | |
| 820 | } | ||
| 821 | |||
| 822 | /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ | ||
| 823 | // Const access | ||
| 824 | template<typename D> | ||
| 825 | typename SizeDepType<NV>::template ColsReturn<D>::ConstType | ||
| 826 | ✗ | jointCols_impl(const Eigen::MatrixBase<D> & A) const | |
| 827 | { | ||
| 828 | ✗ | return SizeDepType<NV>::middleCols(A.derived(), Base::i_v, m_nvExtended); | |
| 829 | } | ||
| 830 | |||
| 831 | // Non-const access | ||
| 832 | template<typename D> | ||
| 833 | typename SizeDepType<NV>::template ColsReturn<D>::Type | ||
| 834 | 367 | jointCols_impl(Eigen::MatrixBase<D> & A) const | |
| 835 | { | ||
| 836 | 367 | return SizeDepType<NV>::middleCols(A.derived(), Base::i_v, m_nvExtended); | |
| 837 | } | ||
| 838 | |||
| 839 | /* Acces to dedicated rows in a matrix.*/ | ||
| 840 | // Const access | ||
| 841 | template<typename D> | ||
| 842 | typename SizeDepType<NV>::template RowsReturn<D>::ConstType | ||
| 843 | ✗ | jointRows_impl(const Eigen::MatrixBase<D> & A) const | |
| 844 | { | ||
| 845 | ✗ | return SizeDepType<NV>::middleRows(A.derived(), Base::i_v, m_nvExtended); | |
| 846 | } | ||
| 847 | |||
| 848 | // Non-const access | ||
| 849 | template<typename D> | ||
| 850 | typename SizeDepType<NV>::template RowsReturn<D>::Type | ||
| 851 | 30 | jointRows_impl(Eigen::MatrixBase<D> & A) const | |
| 852 | { | ||
| 853 | 30 | return SizeDepType<NV>::middleRows(A.derived(), Base::i_v, m_nvExtended); | |
| 854 | } | ||
| 855 | |||
| 856 | // /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the | ||
| 857 | // matrix Mat | ||
| 858 | // // Const access | ||
| 859 | template<typename D> | ||
| 860 | typename SizeDepType<NV>::template BlockReturn<D>::ConstType | ||
| 861 | jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const | ||
| 862 | { | ||
| 863 | return SizeDepType<NV>::block( | ||
| 864 | Mat.derived(), Base::i_v, Base::i_v, m_nvExtended, m_nvExtended); | ||
| 865 | } | ||
| 866 | |||
| 867 | // Non-const access | ||
| 868 | template<typename D> | ||
| 869 | typename SizeDepType<NV>::template BlockReturn<D>::Type | ||
| 870 | 17 | jointBlock_impl(Eigen::MatrixBase<D> & Mat) const | |
| 871 | { | ||
| 872 | 17 | return SizeDepType<NV>::block( | |
| 873 | 34 | Mat.derived(), Base::i_v, Base::i_v, m_nvExtended, m_nvExtended); | |
| 874 | } | ||
| 875 | |||
| 876 | 1 | void disp(std::ostream & os) const | |
| 877 | { | ||
| 878 | 1 | Base::disp(os); | |
| 879 |
2/4✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
1 | os << " Mimicking joint type: " << m_jmodel_mimicking.shortname() << std::endl; |
| 880 | 1 | os << " Mimicked joint id: " << m_jmodel_mimicking.id() << std::endl; | |
| 881 | 1 | os << " Mimic scaling: " << m_scaling << std::endl; | |
| 882 | 1 | os << " Mimic offset: " << m_offset << std::endl; | |
| 883 | 1 | } | |
| 884 | |||
| 885 | }; // struct JointModelMimicTpl | ||
| 886 | |||
| 887 | } // namespace pinocchio | ||
| 888 | |||
| 889 | #include <boost/type_traits.hpp> | ||
| 890 | |||
| 891 | namespace boost | ||
| 892 | { | ||
| 893 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
| 894 | struct has_nothrow_constructor< | ||
| 895 | ::pinocchio::JointModelMimicTpl<Scalar, Options, JointCollectionTpl>> | ||
| 896 | : public integral_constant<bool, true> | ||
| 897 | { | ||
| 898 | }; | ||
| 899 | |||
| 900 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
| 901 | struct has_nothrow_copy<::pinocchio::JointModelMimicTpl<Scalar, Options, JointCollectionTpl>> | ||
| 902 | : public integral_constant<bool, true> | ||
| 903 | { | ||
| 904 | }; | ||
| 905 | |||
| 906 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
| 907 | struct has_nothrow_constructor< | ||
| 908 | ::pinocchio::JointDataMimicTpl<Scalar, Options, JointCollectionTpl>> | ||
| 909 | : public integral_constant<bool, true> | ||
| 910 | { | ||
| 911 | }; | ||
| 912 | |||
| 913 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
| 914 | struct has_nothrow_copy<::pinocchio::JointDataMimicTpl<Scalar, Options, JointCollectionTpl>> | ||
| 915 | : public integral_constant<bool, true> | ||
| 916 | { | ||
| 917 | }; | ||
| 918 | } // namespace boost | ||
| 919 | |||
| 920 | #endif // ifndef __pinocchio_multibody_joint_mimic_hpp__ | ||
| 921 |