GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/liegroup/cartesian-product-variant.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 34 0.0%
Branches: 0 28 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2018 CNRS
3 //
4
5 #ifndef __pinocchio_cartesian_product_variant_hpp__
6 #define __pinocchio_cartesian_product_variant_hpp__
7
8 #include "pinocchio/multibody/liegroup/liegroup-base.hpp"
9 #include "pinocchio/multibody/liegroup/liegroup-collection.hpp"
10
11 #include "pinocchio/container/aligned-vector.hpp"
12
13 namespace pinocchio
14 {
15
16 template<
17 typename Scalar,
18 int Options = context::Options,
19 template<typename, int> class LieGroupCollectionTpl = LieGroupCollectionDefaultTpl>
20 struct CartesianProductOperationVariantTpl;
21 typedef CartesianProductOperationVariantTpl<
22 context::Scalar,
23 context::Options,
24 LieGroupCollectionDefaultTpl>
25 CartesianProductOperationVariant;
26
27 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
28 struct traits<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>>
29 {
30 typedef _Scalar Scalar;
31 enum
32 {
33 Options = _Options,
34 NQ = Eigen::Dynamic,
35 NV = Eigen::Dynamic
36 };
37 };
38
39 ///
40 /// \brief Dynamic Cartesian product composed of elementary Lie groups defined in LieGroupVariant
41 ///
42 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
43 struct CartesianProductOperationVariantTpl
44 : public LieGroupBase<
45 CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>>
46 {
47 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48
49 PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(CartesianProductOperationVariantTpl);
50
51 typedef LieGroupCollectionTpl<Scalar, Options> LieGroupCollection;
52 typedef typename LieGroupCollection::LieGroupVariant LieGroupVariant;
53 typedef LieGroupGenericTpl<LieGroupCollection> LieGroupGeneric;
54
55 /// \brief Default constructor
56 ✗ CartesianProductOperationVariantTpl()
57 ✗ : m_nq(0)
58 ✗ , m_nv(0)
59 ✗ , lg_nqs(0)
60 ✗ , lg_nvs(0)
61 ✗ , m_neutral(0) {};
62
63 ///
64 /// \brief Constructor with one single Lie group
65 ///
66 /// \param[in] lg Lie group variant to insert inside the Cartesian product
67 ///
68 ✗ explicit CartesianProductOperationVariantTpl(const LieGroupGeneric & lg)
69 ✗ : m_nq(0)
70 ✗ , m_nv(0)
71 ✗ , lg_nqs(0)
72 ✗ , lg_nvs(0)
73 ✗ , m_neutral(0)
74 {
75 ✗ append(lg);
76 ✗ };
77
78 ///
79 /// \brief Constructor with two Lie groups
80 ///
81 /// \param[in] lg1 Lie group variant to insert inside the Cartesian product
82 /// \param[in] lg2 Lie group variant to insert inside the Cartesian product
83 ///
84 ✗ CartesianProductOperationVariantTpl(const LieGroupGeneric & lg1, const LieGroupGeneric & lg2)
85 ✗ : m_nq(0)
86 ✗ , m_nv(0)
87 ✗ , lg_nqs(0)
88 ✗ , lg_nvs(0)
89 ✗ , m_neutral(0)
90 {
91 ✗ append(lg1);
92 ✗ append(lg2);
93 ✗ };
94
95 ///
96 /// \brief Append a Lie group to the Cartesian product
97 ///
98 /// \param[in] lg Lie group variant to insert inside the Cartesian product
99 ///
100 void append(const LieGroupGeneric & lg);
101
102 ///
103 /// \brief Append a Lie group to the Cartesian product
104 ///
105 /// \param[in] lg Lie group to insert inside the Cartesian product
106 ///
107 template<typename LieGroupDerived>
108 ✗ void append(const LieGroupBase<LieGroupDerived> & lg)
109 {
110 ✗ LieGroupGeneric lgGeneric(lg);
111 ✗ append(lgGeneric);
112 }
113
114 ///
115 /// \brief Cartesian product between *this and other.
116 ///
117 /// \param[in] other CartesianProductOperation to compose with this
118 ///
119 /// \returns A new Cartesian product betwenn *this and other.
120 ///
121 CartesianProductOperationVariantTpl
122 operator*(const CartesianProductOperationVariantTpl & other) const;
123
124 ///
125 /// \brief Append other to *this.
126 ///
127 /// \param[in] other CartesianProductOperation to append to *this.
128 ///
129 CartesianProductOperationVariantTpl &
130 operator*=(const CartesianProductOperationVariantTpl & other);
131
132 ///
133 /// \brief Append a Lie group to *this.
134 ///
135 /// \param[in] lg LieGroupGeneric to append to *this.
136 ///
137 inline CartesianProductOperationVariantTpl & operator*=(const LieGroupGeneric & lg)
138 {
139 append(lg);
140 return *this;
141 }
142
143 ///
144 /// \brief Append a Lie group to *this.
145 ///
146 /// \param[in] lg LieGroupGeneric to append to *this.
147 ///
148 template<typename LieGroupDerived>
149 inline CartesianProductOperationVariantTpl &
150 operator*=(const LieGroupBase<LieGroupDerived> & lg)
151 {
152 append<LieGroupDerived>(lg);
153 return *this;
154 }
155
156 ✗ int nq() const
157 {
158 ✗ return m_nq;
159 }
160 ✗ int nv() const
161 {
162 ✗ return m_nv;
163 }
164
165 ✗ std::string name() const
166 {
167 ✗ return m_name;
168 }
169
170 ✗ ConfigVector_t neutral() const
171 {
172 ✗ return m_neutral;
173 }
174
175 template<class ConfigL_t, class ConfigR_t, class Tangent_t>
176 void difference_impl(
177 const Eigen::MatrixBase<ConfigL_t> & q0,
178 const Eigen::MatrixBase<ConfigR_t> & q1,
179 const Eigen::MatrixBase<Tangent_t> & d) const;
180
181 template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
182 void dDifference_impl(
183 const Eigen::MatrixBase<ConfigL_t> & q0,
184 const Eigen::MatrixBase<ConfigR_t> & q1,
185 const Eigen::MatrixBase<JacobianOut_t> & J) const;
186
187 template<
188 ArgumentPosition arg,
189 class ConfigL_t,
190 class ConfigR_t,
191 class JacobianIn_t,
192 class JacobianOut_t>
193 void dDifference_product_impl(
194 const ConfigL_t & q0,
195 const ConfigR_t & q1,
196 const JacobianIn_t & Jin,
197 JacobianOut_t & Jout,
198 bool dDifferenceOnTheLeft,
199 const AssignmentOperatorType op) const;
200
201 template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
202 void integrate_impl(
203 const Eigen::MatrixBase<ConfigIn_t> & q,
204 const Eigen::MatrixBase<Velocity_t> & v,
205 const Eigen::MatrixBase<ConfigOut_t> & qout) const;
206
207 template<class Config_t, class Jacobian_t>
208 void integrateCoeffWiseJacobian_impl(
209 const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J) const;
210
211 template<class Config_t, class Tangent_t, class JacobianOut_t>
212 void dIntegrate_dq_impl(
213 const Eigen::MatrixBase<Config_t> & q,
214 const Eigen::MatrixBase<Tangent_t> & v,
215 const Eigen::MatrixBase<JacobianOut_t> & J,
216 const AssignmentOperatorType op = SETTO) const;
217
218 template<class Config_t, class Tangent_t, class JacobianOut_t>
219 void dIntegrate_dv_impl(
220 const Eigen::MatrixBase<Config_t> & q,
221 const Eigen::MatrixBase<Tangent_t> & v,
222 const Eigen::MatrixBase<JacobianOut_t> & J,
223 const AssignmentOperatorType op = SETTO) const;
224
225 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
226 void dIntegrate_product_impl(
227 const Config_t & q,
228 const Tangent_t & v,
229 const JacobianIn_t & Jin,
230 JacobianOut_t & Jout,
231 bool dIntegrateOnTheLeft,
232 const ArgumentPosition arg,
233 const AssignmentOperatorType op) const;
234
235 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
236 void dIntegrateTransport_dq_impl(
237 const Eigen::MatrixBase<Config_t> & q,
238 const Eigen::MatrixBase<Tangent_t> & v,
239 const Eigen::MatrixBase<JacobianIn_t> & J_in,
240 const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
241
242 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
243 void dIntegrateTransport_dv_impl(
244 const Eigen::MatrixBase<Config_t> & q,
245 const Eigen::MatrixBase<Tangent_t> & v,
246 const Eigen::MatrixBase<JacobianIn_t> & J_in,
247 const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
248
249 template<class Config_t, class Tangent_t, class JacobianOut_t>
250 void dIntegrateTransport_dq_impl(
251 const Eigen::MatrixBase<Config_t> & q,
252 const Eigen::MatrixBase<Tangent_t> & v,
253 const Eigen::MatrixBase<JacobianOut_t> & J) const;
254
255 template<class Config_t, class Tangent_t, class JacobianOut_t>
256 void dIntegrateTransport_dv_impl(
257 const Eigen::MatrixBase<Config_t> & q,
258 const Eigen::MatrixBase<Tangent_t> & v,
259 const Eigen::MatrixBase<JacobianOut_t> & J) const;
260
261 template<class ConfigL_t, class ConfigR_t>
262 Scalar squaredDistance_impl(
263 const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const;
264
265 template<class Config_t>
266 void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const;
267
268 template<class Config_t>
269 bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qout, const Scalar & prec) const;
270
271 template<class Config_t>
272 void random_impl(const Eigen::MatrixBase<Config_t> & qout) const;
273
274 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
275 void randomConfiguration_impl(
276 const Eigen::MatrixBase<ConfigL_t> & lower,
277 const Eigen::MatrixBase<ConfigR_t> & upper,
278 const Eigen::MatrixBase<ConfigOut_t> & qout) const;
279
280 template<class ConfigL_t, class ConfigR_t>
281 bool isSameConfiguration_impl(
282 const Eigen::MatrixBase<ConfigL_t> & q0,
283 const Eigen::MatrixBase<ConfigR_t> & q1,
284 const Scalar & prec) const;
285
286 bool isEqual_impl(const CartesianProductOperationVariantTpl & other) const;
287
288 template<typename LieGroup1, typename LieGroup2>
289 bool isEqual(const CartesianProductOperation<LieGroup1, LieGroup2> & other) const;
290
291 protected:
292 PINOCCHIO_ALIGNED_STD_VECTOR(LieGroupGeneric) liegroups;
293 Index m_nq, m_nv;
294 std::vector<Index> lg_nqs, lg_nvs;
295 std::string m_name;
296
297 ConfigVector_t m_neutral;
298 };
299
300 } // namespace pinocchio
301
302 #include <pinocchio/multibody/liegroup/cartesian-product-variant.hxx>
303
304 #endif // ifndef __pinocchio_cartesian_product_variant_hpp__
305