| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/joint-motion-subspace-generic.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 47 | 47 | 100.0% |
| Branches: | 17 | 36 | 47.2% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2020 CNRS INRIA | ||
| 3 | // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
| 4 | // | ||
| 5 | |||
| 6 | #ifndef __pinocchio_multibody_constraint_generic_hpp__ | ||
| 7 | #define __pinocchio_multibody_constraint_generic_hpp__ | ||
| 8 | |||
| 9 | namespace pinocchio | ||
| 10 | { | ||
| 11 | |||
| 12 | template<int _Dim, typename _Scalar, int _Options> | ||
| 13 | struct traits<JointMotionSubspaceTpl<_Dim, _Scalar, _Options>> | ||
| 14 | { | ||
| 15 | typedef _Scalar Scalar; | ||
| 16 | enum | ||
| 17 | { | ||
| 18 | LINEAR = 0, | ||
| 19 | ANGULAR = 3, | ||
| 20 | Options = _Options, | ||
| 21 | Dim = _Dim | ||
| 22 | }; | ||
| 23 | |||
| 24 | typedef MotionTpl<Scalar, Options> JointMotion; | ||
| 25 | typedef Eigen::Matrix<Scalar, Dim, 1, Options> JointForce; | ||
| 26 | typedef Eigen::Matrix<Scalar, 6, Dim, Options> DenseBase; | ||
| 27 | typedef Eigen::Matrix<Scalar, Dim, Dim, Options> ReducedSquaredMatrix; | ||
| 28 | |||
| 29 | typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(DenseBase) ConstMatrixReturnType; | ||
| 30 | typedef typename PINOCCHIO_EIGEN_REF_TYPE(DenseBase) MatrixReturnType; | ||
| 31 | |||
| 32 | typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; | ||
| 33 | }; // traits JointMotionSubspaceTpl | ||
| 34 | |||
| 35 | template<int Dim, typename Scalar, int Options> | ||
| 36 | struct SE3GroupAction<JointMotionSubspaceTpl<Dim, Scalar, Options>> | ||
| 37 | { | ||
| 38 | typedef Eigen::Matrix<Scalar, 6, Dim> ReturnType; | ||
| 39 | }; | ||
| 40 | |||
| 41 | template<int Dim, typename Scalar, int Options, typename MotionDerived> | ||
| 42 | struct MotionAlgebraAction<JointMotionSubspaceTpl<Dim, Scalar, Options>, MotionDerived> | ||
| 43 | { | ||
| 44 | typedef Eigen::Matrix<Scalar, 6, Dim> ReturnType; | ||
| 45 | }; | ||
| 46 | |||
| 47 | template<int _Dim, typename _Scalar, int _Options> | ||
| 48 | struct JointMotionSubspaceTpl | ||
| 49 | : public JointMotionSubspaceBase<JointMotionSubspaceTpl<_Dim, _Scalar, _Options>> | ||
| 50 | { | ||
| 51 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 52 | |||
| 53 | typedef JointMotionSubspaceBase<JointMotionSubspaceTpl> Base; | ||
| 54 | |||
| 55 | friend class JointMotionSubspaceBase<JointMotionSubspaceTpl>; | ||
| 56 | PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceTpl) | ||
| 57 | |||
| 58 | enum | ||
| 59 | { | ||
| 60 | NV = _Dim | ||
| 61 | }; | ||
| 62 | |||
| 63 | using Base::nv; | ||
| 64 | |||
| 65 | template<typename D> | ||
| 66 | 1480 | explicit JointMotionSubspaceTpl(const Eigen::MatrixBase<D> & _S) | |
| 67 | 1480 | : S(_S) | |
| 68 | { | ||
| 69 | // There is currently a bug in Eigen/Core/util/StaticAssert.h in the use of the full namespace | ||
| 70 | // path | ||
| 71 | // TODO | ||
| 72 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DenseBase, D); | ||
| 73 | 1480 | } | |
| 74 | |||
| 75 | JointMotionSubspaceTpl() | ||
| 76 | : S() | ||
| 77 | { | ||
| 78 | EIGEN_STATIC_ASSERT( | ||
| 79 | _Dim != Eigen::Dynamic, YOU_CALLED_A_DYNAMIC_SIZE_METHOD_ON_A_FIXED_SIZE_MATRIX_OR_VECTOR) | ||
| 80 | } | ||
| 81 | |||
| 82 | // It is only valid for dynamics size | ||
| 83 | 129 | explicit JointMotionSubspaceTpl(const int dim) | |
| 84 |
1/2✓ Branch 1 taken 127 times.
✗ Branch 2 not taken.
|
129 | : S(6, dim) |
| 85 | { | ||
| 86 | EIGEN_STATIC_ASSERT( | ||
| 87 | _Dim == Eigen::Dynamic, YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR) | ||
| 88 | 129 | } | |
| 89 | |||
| 90 | 53 | static JointMotionSubspaceTpl Zero(const int dim) | |
| 91 | { | ||
| 92 | 53 | return JointMotionSubspaceTpl(dim); | |
| 93 | } | ||
| 94 | |||
| 95 | template<typename VectorLike> | ||
| 96 | 4 | JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & vj) const | |
| 97 | { | ||
| 98 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
8 | return JointMotion(S * vj); |
| 99 | } | ||
| 100 | |||
| 101 | struct Transpose : JointMotionSubspaceTransposeBase<JointMotionSubspaceTpl> | ||
| 102 | { | ||
| 103 | const JointMotionSubspaceTpl & ref; | ||
| 104 | 7 | Transpose(const JointMotionSubspaceTpl & ref) | |
| 105 | 7 | : ref(ref) | |
| 106 | { | ||
| 107 | 7 | } | |
| 108 | |||
| 109 | template<typename Derived> | ||
| 110 | 5 | JointForce operator*(const ForceDense<Derived> & f) const | |
| 111 | { | ||
| 112 |
2/4✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
|
10 | return (ref.S.transpose() * f.toVector()).eval(); |
| 113 | } | ||
| 114 | |||
| 115 | template<typename D> | ||
| 116 | 1 | typename Eigen::Matrix<Scalar, NV, Eigen::Dynamic> operator*(const Eigen::MatrixBase<D> & F) | |
| 117 | { | ||
| 118 |
2/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
2 | return (ref.S.transpose() * F).eval(); |
| 119 | } | ||
| 120 | }; | ||
| 121 | |||
| 122 | 7 | Transpose transpose() const | |
| 123 | { | ||
| 124 | 7 | return Transpose(*this); | |
| 125 | } | ||
| 126 | |||
| 127 | 403 | MatrixReturnType matrix_impl() | |
| 128 | { | ||
| 129 | 403 | return S; | |
| 130 | } | ||
| 131 | 5 | ConstMatrixReturnType matrix_impl() const | |
| 132 | { | ||
| 133 | 5 | return S; | |
| 134 | } | ||
| 135 | |||
| 136 | 337 | int nv_impl() const | |
| 137 | { | ||
| 138 | 337 | return (int)S.cols(); | |
| 139 | } | ||
| 140 | |||
| 141 | template<typename S2, int O2> | ||
| 142 | friend typename JointMotionSubspaceTpl<_Dim, _Scalar, _Options>::DenseBase | ||
| 143 | 1 | operator*(const InertiaTpl<S2, O2> & Y, const JointMotionSubspaceTpl & S) | |
| 144 | { | ||
| 145 | typedef typename JointMotionSubspaceTpl::DenseBase ReturnType; | ||
| 146 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | ReturnType res(6, S.nv()); |
| 147 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | motionSet::inertiaAction(Y, S.S, res); |
| 148 | 1 | return res; | |
| 149 | } | ||
| 150 | |||
| 151 | template<typename S2, int O2> | ||
| 152 | friend Eigen::Matrix<_Scalar, 6, _Dim> | ||
| 153 | 1 | operator*(const Eigen::Matrix<S2, 6, 6, O2> & Ymatrix, const JointMotionSubspaceTpl & S) | |
| 154 | { | ||
| 155 | typedef Eigen::Matrix<_Scalar, 6, _Dim> ReturnType; | ||
| 156 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
2 | return ReturnType(Ymatrix * S.matrix()); |
| 157 | } | ||
| 158 | |||
| 159 | 11 | DenseBase se3Action(const SE3Tpl<Scalar, Options> & m) const | |
| 160 | { | ||
| 161 |
1/2✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
|
11 | DenseBase res(6, nv()); |
| 162 |
1/2✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
|
11 | motionSet::se3Action(m, S, res); |
| 163 | 11 | return res; | |
| 164 | } | ||
| 165 | |||
| 166 | 3 | DenseBase se3ActionInverse(const SE3Tpl<Scalar, Options> & m) const | |
| 167 | { | ||
| 168 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | DenseBase res(6, nv()); |
| 169 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | motionSet::se3ActionInverse(m, S, res); |
| 170 | 3 | return res; | |
| 171 | } | ||
| 172 | |||
| 173 | template<typename MotionDerived> | ||
| 174 | 1 | DenseBase motionAction(const MotionDense<MotionDerived> & v) const | |
| 175 | { | ||
| 176 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | DenseBase res(6, nv()); |
| 177 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | motionSet::motionAction(v, S, res); |
| 178 | 1 | return res; | |
| 179 | } | ||
| 180 | |||
| 181 | void disp_impl(std::ostream & os) const | ||
| 182 | { | ||
| 183 | os << "S =\n" << S << std::endl; | ||
| 184 | } | ||
| 185 | |||
| 186 | 339 | bool isEqual(const JointMotionSubspaceTpl & other) const | |
| 187 | { | ||
| 188 | 339 | return S == other.S; | |
| 189 | } | ||
| 190 | |||
| 191 | protected: | ||
| 192 | DenseBase S; | ||
| 193 | }; // class JointMotionSubspaceTpl | ||
| 194 | |||
| 195 | namespace details | ||
| 196 | { | ||
| 197 | template<int Dim, typename Scalar, int Options> | ||
| 198 | struct StDiagonalMatrixSOperation<JointMotionSubspaceTpl<Dim, Scalar, Options>> | ||
| 199 | { | ||
| 200 | typedef JointMotionSubspaceTpl<Dim, Scalar, Options> Constraint; | ||
| 201 | typedef typename traits<Constraint>::StDiagonalMatrixSOperationReturnType ReturnType; | ||
| 202 | |||
| 203 | 1 | static ReturnType run(const JointMotionSubspaceBase<Constraint> & constraint) | |
| 204 | { | ||
| 205 |
2/4✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | return constraint.matrix().transpose() * constraint.matrix(); |
| 206 | } | ||
| 207 | }; | ||
| 208 | } // namespace details | ||
| 209 | |||
| 210 | } // namespace pinocchio | ||
| 211 | |||
| 212 | #endif // ifndef __pinocchio_multibody_constraint_generic_hpp__ | ||
| 213 |