GCC Code Coverage Report

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