GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint-motion-subspace-generic.hpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 51 51 100.0%
Branches: 19 41 46.3%
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, int _MaxDim>
13 struct traits<JointMotionSubspaceTpl<_Dim, _Scalar, _Options, _MaxDim>>
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, _MaxDim, 1> JointForce;
26 typedef Eigen::Matrix<Scalar, 6, Dim, Options, 6, _MaxDim> DenseBase;
27 typedef Eigen::Matrix<Scalar, Dim, Dim, Options, _MaxDim, _MaxDim> 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, int _MaxDim>
36 struct SE3GroupAction<JointMotionSubspaceTpl<Dim, Scalar, Options, _MaxDim>>
37 {
38 typedef Eigen::Matrix<Scalar, 6, Dim, Options, 6, _MaxDim> ReturnType;
39 };
40
41 template<int Dim, typename Scalar, int Options, int MaxDim, typename MotionDerived>
42 struct MotionAlgebraAction<JointMotionSubspaceTpl<Dim, Scalar, Options, MaxDim>, MotionDerived>
43 {
44 typedef Eigen::Matrix<Scalar, 6, Dim, Options, 6, MaxDim> ReturnType;
45 };
46
47 template<int Dim, typename Scalar, int Options, int MaxDim, typename ForceDerived>
48 struct ConstraintForceOp<JointMotionSubspaceTpl<Dim, Scalar, Options, MaxDim>, ForceDerived>
49 {
50 typedef
51 typename traits<JointMotionSubspaceTpl<Dim, Scalar, Options, MaxDim>>::DenseBase DenseBase;
52 typedef Eigen::Matrix<Scalar, Dim, Dim, Options, MaxDim, MaxDim> ReturnType;
53 };
54
55 template<int Dim, typename Scalar, int Options, int MaxDim, typename ForceSet>
56 struct ConstraintForceSetOp<JointMotionSubspaceTpl<Dim, Scalar, Options, MaxDim>, ForceSet>
57 {
58 typedef
59 typename traits<JointMotionSubspaceTpl<Dim, Scalar, Options, MaxDim>>::DenseBase DenseBase;
60 typedef
61 typename MatrixMatrixProduct<Eigen::Transpose<const DenseBase>, ForceSet>::type ReturnType;
62 };
63
64 template<int _Dim, typename _Scalar, int _Options, int _MaxDim>
65 struct JointMotionSubspaceTpl
66 : public JointMotionSubspaceBase<JointMotionSubspaceTpl<_Dim, _Scalar, _Options, _MaxDim>>
67 {
68 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69
70 typedef JointMotionSubspaceBase<JointMotionSubspaceTpl> Base;
71
72 friend class JointMotionSubspaceBase<JointMotionSubspaceTpl>;
73 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceTpl)
74
75 enum
76 {
77 NV = _Dim
78 };
79
80 constexpr static int MaxNV = NV < 0 ? _MaxDim : NV;
81
82 using Base::nv;
83
84 template<typename D>
85 4898 explicit JointMotionSubspaceTpl(const Eigen::MatrixBase<D> & _S)
86 4898 : S(_S)
87 {
88 // There is currently a bug in Eigen/Core/util/StaticAssert.h in the use of the full namespace
89 // path
90 // TODO
91 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DenseBase, D);
92 }
93
94 JointMotionSubspaceTpl()
95 : S()
96 {
97 EIGEN_STATIC_ASSERT(
98 _Dim != Eigen::Dynamic, YOU_CALLED_A_DYNAMIC_SIZE_METHOD_ON_A_FIXED_SIZE_MATRIX_OR_VECTOR)
99 }
100
101 // It is only valid for dynamics size
102 10706 explicit JointMotionSubspaceTpl(const int dim)
103
1/2
✓ Branch 1 taken 5353 times.
✗ Branch 2 not taken.
 10706 : S(6, dim)
104 {
105 EIGEN_STATIC_ASSERT(
106 _Dim == Eigen::Dynamic, YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR);
107
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5141 times.
 10282 assert(_MaxDim < 0 || dim <= _MaxDim);
108 10706 }
109
110 // It is only valid for dynamics size
111 template<int D, int MD>
112 1707 explicit JointMotionSubspaceTpl(const JointMotionSubspaceTpl<D, _Scalar, _Options, MD> subspace)
113 1707 : S(subspace.matrix())
114 {
115 EIGEN_STATIC_ASSERT(
116 _Dim == Eigen::Dynamic, YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR);
117
1/5
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 1688 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 1707 assert(_MaxDim < 0 || subspace.matrix().cols() <= _MaxDim);
118 1707 }
119
120 135 static JointMotionSubspaceTpl Zero(const int dim)
121 {
122 135 return JointMotionSubspaceTpl(dim);
123 }
124
125 template<typename VectorLike>
126 609 JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & vj) const
127 {
128
1/2
✓ Branch 2 taken 305 times.
✗ Branch 3 not taken.
 1218 return JointMotion(S * vj);
129 }
130
131 struct Transpose : JointMotionSubspaceTransposeBase<JointMotionSubspaceTpl>
132 {
133 const JointMotionSubspaceTpl & ref;
134 737 Transpose(const JointMotionSubspaceTpl & ref)
135 737 : ref(ref)
136 {
137 737 }
138
139 template<typename ForceDerived>
140 typename ConstraintForceOp<JointMotionSubspaceTpl, ForceDerived>::ReturnType
141 639 operator*(const ForceDense<ForceDerived> & f) const
142 {
143
3/6
✓ Branch 3 taken 320 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 320 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 320 times.
✗ Branch 10 not taken.
 639 return (ref.S.transpose() * f.toVector()).eval();
144 }
145
146 template<typename ForceSet>
147 typename ConstraintForceSetOp<JointMotionSubspaceTpl, ForceSet>::ReturnType
148 97 operator*(const Eigen::MatrixBase<ForceSet> & F)
149 {
150
1/2
✓ Branch 3 taken 49 times.
✗ Branch 4 not taken.
 97 return ref.S.transpose() * F.derived();
151 }
152 };
153
154 737 Transpose transpose() const
155 {
156 737 return Transpose(*this);
157 }
158
159 1378 MatrixReturnType matrix_impl()
160 {
161 1378 return S;
162 }
163 3383 ConstMatrixReturnType matrix_impl() const
164 {
165 6760 return S;
166 }
167
168 2898 int nv_impl() const
169 {
170 2898 return (int)S.cols();
171 }
172
173 template<typename S2, int O2>
174 friend typename JointMotionSubspaceTpl<_Dim, _Scalar, _Options, _MaxDim>::DenseBase
175 67 operator*(const InertiaTpl<S2, O2> & Y, const JointMotionSubspaceTpl & S)
176 {
177 typedef typename JointMotionSubspaceTpl::DenseBase ReturnType;
178
1/2
✓ Branch 2 taken 34 times.
✗ Branch 3 not taken.
 67 ReturnType res(6, S.nv());
179
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 67 motionSet::inertiaAction(Y, S.S, res);
180 67 return res;
181 }
182
183 template<typename S2, int O2>
184 friend Eigen::Matrix<_Scalar, 6, _Dim, _Options, 6, _MaxDim>
185 1 operator*(const Eigen::Matrix<S2, 6, 6, O2> & Ymatrix, const JointMotionSubspaceTpl & S)
186 {
187 typedef Eigen::Matrix<_Scalar, 6, _Dim, _Options, 6, _MaxDim> ReturnType;
188
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 return ReturnType(Ymatrix * S.matrix());
189 }
190
191 1677 DenseBase se3Action(const SE3Tpl<Scalar, Options> & m) const
192 {
193
1/2
✓ Branch 2 taken 840 times.
✗ Branch 3 not taken.
 1677 DenseBase res(6, nv());
194
1/2
✓ Branch 1 taken 73 times.
✗ Branch 2 not taken.
 1677 motionSet::se3Action(m, S, res);
195 1677 return res;
196 }
197
198 212 DenseBase se3ActionInverse(const SE3Tpl<Scalar, Options> & m) const
199 {
200
1/2
✓ Branch 2 taken 106 times.
✗ Branch 3 not taken.
 212 DenseBase res(6, nv());
201
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 212 motionSet::se3ActionInverse(m, S, res);
202 212 return res;
203 }
204
205 template<typename MotionDerived>
206 33 DenseBase motionAction(const MotionDense<MotionDerived> & v) const
207 {
208
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 33 DenseBase res(6, nv());
209
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 33 motionSet::motionAction(v, S, res);
210 33 return res;
211 }
212
213 void disp_impl(std::ostream & os) const
214 {
215 os << "S =\n" << S << std::endl;
216 }
217
218 355 bool isEqual(const JointMotionSubspaceTpl & other) const
219 {
220 355 return S == other.S;
221 }
222
223 protected:
224 DenseBase S;
225 }; // class JointMotionSubspaceTpl
226
227 namespace details
228 {
229 template<int Dim, typename Scalar, int Options, int MaxDim>
230 struct StDiagonalMatrixSOperation<JointMotionSubspaceTpl<Dim, Scalar, Options, MaxDim>>
231 {
232 typedef JointMotionSubspaceTpl<Dim, Scalar, Options, MaxDim> Constraint;
233 typedef typename traits<Constraint>::StDiagonalMatrixSOperationReturnType ReturnType;
234
235 1 static ReturnType run(const JointMotionSubspaceBase<Constraint> & constraint)
236 {
237
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();
238 }
239 };
240 } // namespace details
241
242 } // namespace pinocchio
243
244 #endif // ifndef __pinocchio_multibody_constraint_generic_hpp__
245