Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/joint-motion-subspace-generic.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 11 | 45 | 24.4% |
Branches: | 2 | 36 | 5.6% |
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 | 54 | explicit JointMotionSubspaceTpl(const Eigen::MatrixBase<D> & _S) | |
67 | 54 | : 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 | } | ||
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 | 25 | explicit JointMotionSubspaceTpl(const int dim) | |
84 |
1/2✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
|
25 | : 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 | 25 | } | |
89 | |||
90 | 6 | static JointMotionSubspaceTpl Zero(const int dim) | |
91 | { | ||
92 | 6 | return JointMotionSubspaceTpl(dim); | |
93 | } | ||
94 | |||
95 | template<typename VectorLike> | ||
96 | 3 | JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & vj) const | |
97 | { | ||
98 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | return JointMotion(S * vj); |
99 | } | ||
100 | |||
101 | struct Transpose : JointMotionSubspaceTransposeBase<JointMotionSubspaceTpl> | ||
102 | { | ||
103 | const JointMotionSubspaceTpl & ref; | ||
104 | ✗ | Transpose(const JointMotionSubspaceTpl & ref) | |
105 | ✗ | : ref(ref) | |
106 | { | ||
107 | } | ||
108 | |||
109 | template<typename Derived> | ||
110 | ✗ | JointForce operator*(const ForceDense<Derived> & f) const | |
111 | { | ||
112 | ✗ | return (ref.S.transpose() * f.toVector()).eval(); | |
113 | } | ||
114 | |||
115 | template<typename D> | ||
116 | ✗ | typename Eigen::Matrix<Scalar, NV, Eigen::Dynamic> operator*(const Eigen::MatrixBase<D> & F) | |
117 | { | ||
118 | ✗ | return (ref.S.transpose() * F).eval(); | |
119 | } | ||
120 | }; | ||
121 | |||
122 | ✗ | Transpose transpose() const | |
123 | { | ||
124 | ✗ | return Transpose(*this); | |
125 | } | ||
126 | |||
127 | 30 | MatrixReturnType matrix_impl() | |
128 | { | ||
129 | 30 | return S; | |
130 | } | ||
131 | ✗ | ConstMatrixReturnType matrix_impl() const | |
132 | { | ||
133 | ✗ | return S; | |
134 | } | ||
135 | |||
136 | ✗ | int nv_impl() const | |
137 | { | ||
138 | ✗ | return (int)S.cols(); | |
139 | } | ||
140 | |||
141 | template<typename S2, int O2> | ||
142 | friend typename JointMotionSubspaceTpl<_Dim, _Scalar, _Options>::DenseBase | ||
143 | ✗ | operator*(const InertiaTpl<S2, O2> & Y, const JointMotionSubspaceTpl & S) | |
144 | { | ||
145 | typedef typename JointMotionSubspaceTpl::DenseBase ReturnType; | ||
146 | ✗ | ReturnType res(6, S.nv()); | |
147 | ✗ | motionSet::inertiaAction(Y, S.S, res); | |
148 | ✗ | return res; | |
149 | } | ||
150 | |||
151 | template<typename S2, int O2> | ||
152 | friend Eigen::Matrix<_Scalar, 6, _Dim> | ||
153 | ✗ | operator*(const Eigen::Matrix<S2, 6, 6, O2> & Ymatrix, const JointMotionSubspaceTpl & S) | |
154 | { | ||
155 | typedef Eigen::Matrix<_Scalar, 6, _Dim> ReturnType; | ||
156 | ✗ | return ReturnType(Ymatrix * S.matrix()); | |
157 | } | ||
158 | |||
159 | ✗ | DenseBase se3Action(const SE3Tpl<Scalar, Options> & m) const | |
160 | { | ||
161 | ✗ | DenseBase res(6, nv()); | |
162 | ✗ | motionSet::se3Action(m, S, res); | |
163 | ✗ | return res; | |
164 | } | ||
165 | |||
166 | ✗ | DenseBase se3ActionInverse(const SE3Tpl<Scalar, Options> & m) const | |
167 | { | ||
168 | ✗ | DenseBase res(6, nv()); | |
169 | ✗ | motionSet::se3ActionInverse(m, S, res); | |
170 | ✗ | return res; | |
171 | } | ||
172 | |||
173 | template<typename MotionDerived> | ||
174 | ✗ | DenseBase motionAction(const MotionDense<MotionDerived> & v) const | |
175 | { | ||
176 | ✗ | DenseBase res(6, nv()); | |
177 | ✗ | motionSet::motionAction(v, S, res); | |
178 | ✗ | return res; | |
179 | } | ||
180 | |||
181 | void disp_impl(std::ostream & os) const | ||
182 | { | ||
183 | os << "S =\n" << S << std::endl; | ||
184 | } | ||
185 | |||
186 | ✗ | bool isEqual(const JointMotionSubspaceTpl & other) const | |
187 | { | ||
188 | ✗ | 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 | ✗ | static ReturnType run(const JointMotionSubspaceBase<Constraint> & constraint) | |
204 | { | ||
205 | ✗ | 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 |