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 |
|
|
|