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 |