GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/liegroup/vector-space.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 60 110 54.5%
Branches: 29 86 33.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__
6 #define __pinocchio_multibody_liegroup_vector_space_operation_hpp__
7
8 #include "pinocchio/multibody/liegroup/liegroup-base.hpp"
9
10 #include <stdexcept>
11 #include <boost/integer/static_min_max.hpp>
12
13 namespace pinocchio
14 {
15 template<int Dim, typename Scalar, int Options = context::Options>
16 struct VectorSpaceOperationTpl;
17
18 template<int Dim, typename _Scalar, int _Options>
19 struct traits<VectorSpaceOperationTpl<Dim, _Scalar, _Options>>
20 {
21 typedef _Scalar Scalar;
22 enum
23 {
24 Options = _Options,
25 NQ = Dim,
26 NV = Dim
27 };
28 };
29
30 template<int Dim, typename _Scalar, int _Options>
31 struct VectorSpaceOperationTpl
32 : public LieGroupBase<VectorSpaceOperationTpl<Dim, _Scalar, _Options>>
33 {
34 PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(VectorSpaceOperationTpl);
35
36 /// Constructor
37 /// \param size size of the vector space: should be the equal to template
38 /// argument for static sized vector-spaces.
39 240260 VectorSpaceOperationTpl(int size = boost::static_signed_max<0, Dim>::value)
40 240260 : size_(size)
41 {
42 ✗ assert(size_.value() >= 0);
43 }
44
45 /// Constructor
46 /// \param other other VectorSpaceOperationTpl from which to retrieve size
47 ✗ VectorSpaceOperationTpl(const VectorSpaceOperationTpl & other)
48 : Base()
49 ✗ , size_(other.size_.value())
50 {
51 ✗ assert(size_.value() >= 0);
52 }
53
54 ✗ VectorSpaceOperationTpl & operator=(const VectorSpaceOperationTpl & other)
55 {
56 ✗ size_.setValue(other.size_.value());
57 ✗ assert(size_.value() >= 0);
58 ✗ return *this;
59 }
60
61 314620 Index nq() const
62 {
63 314620 return size_.value();
64 }
65 40 Index nv() const
66 {
67 80 return size_.value();
68 }
69
70 1896 ConfigVector_t neutral() const
71 {
72
1/2
✓ Branch 3 taken 948 times.
✗ Branch 4 not taken.
 1896 return ConfigVector_t::Zero(size_.value());
73 }
74
75 ✗ std::string name() const
76 {
77 ✗ std::ostringstream oss;
78 ✗ oss << "R^" << nq();
79 ✗ return oss.str();
80 }
81
82 template<class ConfigL_t, class ConfigR_t, class Tangent_t>
83 2080 static void difference_impl(
84 const Eigen::MatrixBase<ConfigL_t> & q0,
85 const Eigen::MatrixBase<ConfigR_t> & q1,
86 const Eigen::MatrixBase<Tangent_t> & d)
87 {
88
1/2
✓ Branch 3 taken 1040 times.
✗ Branch 4 not taken.
 2080 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d) = q1 - q0;
89 }
90
91 template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
92 96 void dDifference_impl(
93 const Eigen::MatrixBase<ConfigL_t> &,
94 const Eigen::MatrixBase<ConfigR_t> &,
95 const Eigen::MatrixBase<JacobianOut_t> & J) const
96 {
97 if (arg == ARG0)
98
2/4
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 24 times.
✗ Branch 7 not taken.
 48 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J) =
99 48 -JacobianMatrix_t::Identity(size_.value(), size_.value());
100 else if (arg == ARG1)
101 48 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setIdentity();
102 }
103
104 template<
105 ArgumentPosition arg,
106 class ConfigL_t,
107 class ConfigR_t,
108 class JacobianIn_t,
109 class JacobianOut_t>
110 ✗ static void dDifference_product_impl(
111 const ConfigL_t &,
112 const ConfigR_t &,
113 const JacobianIn_t & Jin,
114 JacobianOut_t & Jout,
115 bool,
116 const AssignmentOperatorType op)
117 {
118 ✗ switch (op)
119 {
120 ✗ case SETTO:
121 if (arg == ARG0)
122 ✗ Jout = -Jin;
123 else
124 ✗ Jout = Jin;
125 ✗ return;
126 ✗ case ADDTO:
127 if (arg == ARG0)
128 ✗ Jout -= Jin;
129 else
130 ✗ Jout += Jin;
131 ✗ return;
132 ✗ case RMTO:
133 if (arg == ARG0)
134 ✗ Jout += Jin;
135 else
136 ✗ Jout -= Jin;
137 ✗ return;
138 }
139 }
140
141 template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
142 123896 static void integrate_impl(
143 const Eigen::MatrixBase<ConfigIn_t> & q,
144 const Eigen::MatrixBase<Velocity_t> & v,
145 const Eigen::MatrixBase<ConfigOut_t> & qout)
146 {
147
1/2
✓ Branch 3 taken 61948 times.
✗ Branch 4 not taken.
 123896 PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout) = q + v;
148 }
149
150 template<class Config_t, class Jacobian_t>
151 52 static void integrateCoeffWiseJacobian_impl(
152 const Eigen::MatrixBase<Config_t> &, const Eigen::MatrixBase<Jacobian_t> & J)
153 {
154 52 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).setIdentity();
155 }
156
157 template<class Config_t, class Tangent_t, class JacobianOut_t>
158 176 static void dIntegrate_dq_impl(
159 const Eigen::MatrixBase<Config_t> & /*q*/,
160 const Eigen::MatrixBase<Tangent_t> & /*v*/,
161 const Eigen::MatrixBase<JacobianOut_t> & J,
162 const AssignmentOperatorType op = SETTO)
163 {
164 176 Eigen::MatrixBase<JacobianOut_t> & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
165
3/4
✓ Branch 0 taken 72 times.
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 176 switch (op)
166 {
167 144 case SETTO:
168 144 Jout.setIdentity();
169 144 break;
170 16 case ADDTO:
171
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 16 Jout.diagonal().array() += Scalar(1);
172 16 break;
173 16 case RMTO:
174
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 16 Jout.diagonal().array() -= Scalar(1);
175 16 break;
176 ✗ default:
177 ✗ assert(false && "Wrong Op requesed value");
178 break;
179 }
180 }
181
182 template<class Config_t, class Tangent_t, class JacobianOut_t>
183 176 static void dIntegrate_dv_impl(
184 const Eigen::MatrixBase<Config_t> & /*q*/,
185 const Eigen::MatrixBase<Tangent_t> & /*v*/,
186 const Eigen::MatrixBase<JacobianOut_t> & J,
187 const AssignmentOperatorType op = SETTO)
188 {
189 176 Eigen::MatrixBase<JacobianOut_t> & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
190
3/4
✓ Branch 0 taken 72 times.
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 176 switch (op)
191 {
192 144 case SETTO:
193 144 Jout.setIdentity();
194 144 break;
195 16 case ADDTO:
196
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 16 Jout.diagonal().array() += Scalar(1);
197 16 break;
198 16 case RMTO:
199
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 16 Jout.diagonal().array() -= Scalar(1);
200 16 break;
201 ✗ default:
202 ✗ assert(false && "Wrong Op requesed value");
203 break;
204 }
205 }
206
207 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
208 ✗ static void dIntegrate_product_impl(
209 const Config_t &,
210 const Tangent_t &,
211 const JacobianIn_t & Jin,
212 JacobianOut_t & Jout,
213 bool,
214 const ArgumentPosition,
215 const AssignmentOperatorType op)
216 {
217 ✗ switch (op)
218 {
219 ✗ case SETTO:
220 ✗ Jout = Jin;
221 ✗ break;
222 ✗ case ADDTO:
223 ✗ Jout += Jin;
224 ✗ break;
225 ✗ case RMTO:
226 ✗ Jout -= Jin;
227 ✗ break;
228 ✗ default:
229 ✗ assert(false && "Wrong Op requesed value");
230 break;
231 }
232 }
233
234 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
235 16 static void dIntegrateTransport_dq_impl(
236 const Eigen::MatrixBase<Config_t> & /*q*/,
237 const Eigen::MatrixBase<Tangent_t> & /*v*/,
238 const Eigen::MatrixBase<JacobianIn_t> & Jin,
239 const Eigen::MatrixBase<JacobianOut_t> & Jout)
240 {
241 16 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin;
242 }
243
244 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
245 16 static void dIntegrateTransport_dv_impl(
246 const Eigen::MatrixBase<Config_t> & /*q*/,
247 const Eigen::MatrixBase<Tangent_t> & /*v*/,
248 const Eigen::MatrixBase<JacobianIn_t> & Jin,
249 const Eigen::MatrixBase<JacobianOut_t> & Jout)
250 {
251 16 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin;
252 }
253
254 template<class Config_t, class Tangent_t, class Jacobian_t>
255 16 static void dIntegrateTransport_dq_impl(
256 const Eigen::MatrixBase<Config_t> & /*q*/,
257 const Eigen::MatrixBase<Tangent_t> & /*v*/,
258 const Eigen::MatrixBase<Jacobian_t> & /*J*/)
259 {
260 }
261
262 template<class Config_t, class Tangent_t, class Jacobian_t>
263 16 static void dIntegrateTransport_dv_impl(
264 const Eigen::MatrixBase<Config_t> & /*q*/,
265 const Eigen::MatrixBase<Tangent_t> & /*v*/,
266 const Eigen::MatrixBase<Jacobian_t> & /*J*/)
267 {
268 }
269
270 // template <class ConfigL_t, class ConfigR_t>
271 // static context::Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
272 // const Eigen::MatrixBase<ConfigR_t> & q1)
273
274 template<class Config_t>
275 284 static void normalize_impl(const Eigen::MatrixBase<Config_t> & /*qout*/)
276 {
277 }
278
279 template<class Config_t>
280 static bool
281 112 isNormalized_impl(const Eigen::MatrixBase<Config_t> & /*qout*/, const Scalar & /*prec*/)
282 {
283 112 return true;
284 }
285
286 template<class Config_t>
287 ✗ static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
288 {
289 ✗ PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).setRandom();
290 }
291
292 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
293 146360 void randomConfiguration_impl(
294 const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
295 const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
296 const Eigen::MatrixBase<ConfigOut_t> & qout) const
297 {
298 146360 ConfigOut_t & res = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).derived();
299
2/2
✓ Branch 1 taken 77876 times.
✓ Branch 2 taken 73180 times.
 302112 for (int i = 0; i < nq(); ++i)
300 {
301 311504 if (check_expression_if_real<Scalar, false>(
302 155752 lower_pos_limit[i] == -std::numeric_limits<typename ConfigL_t::Scalar>::infinity()
303
4/8
✓ Branch 0 taken 77876 times.
✗ Branch 1 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 77876 times.
✓ Branch 7 taken 77876 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 77876 times.
 155752 || upper_pos_limit[i] == std::numeric_limits<typename ConfigR_t::Scalar>::infinity()))
304 {
305 ✗ std::ostringstream error;
306 ✗ error << "non bounded limit. Cannot uniformly sample joint at rank " << i;
307 ✗ throw std::range_error(error.str());
308 }
309 155752 res[i] =
310 155752 lower_pos_limit[i] + ((upper_pos_limit[i] - lower_pos_limit[i]) * rand()) / RAND_MAX;
311 }
312 146360 }
313
314 ✗ bool isEqual_impl(const VectorSpaceOperationTpl & other) const
315 {
316 ✗ return size_.value() == other.size_.value();
317 }
318
319 private:
320 Eigen::internal::variable_if_dynamic<Index, Dim> size_;
321 }; // struct VectorSpaceOperationTpl
322
323 } // namespace pinocchio
324
325 #endif // ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__
326