pinocchio  3.3.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
cartesian-product.hpp
1 //
2 // Copyright (c) 2016-2020 CNRS CNRS
3 //
4 
5 #ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__
6 #define __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__
7 
8 #include <pinocchio/multibody/liegroup/liegroup-base.hpp>
9 
10 namespace pinocchio
11 {
12  template<int dim1, int dim2>
13  struct eval_set_dim
14  {
15  enum
16  {
17  value = dim1 + dim2
18  };
19  };
20 
21  template<int dim>
22  struct eval_set_dim<dim, Eigen::Dynamic>
23  {
24  enum
25  {
26  value = Eigen::Dynamic
27  };
28  };
29 
30  template<int dim>
31  struct eval_set_dim<Eigen::Dynamic, dim>
32  {
33  enum
34  {
35  value = Eigen::Dynamic
36  };
37  };
38 
39  template<typename LieGroup1, typename LieGroup2>
40  struct CartesianProductOperation;
41 
42  template<typename LieGroup1, typename LieGroup2>
43  struct traits<CartesianProductOperation<LieGroup1, LieGroup2>>
44  {
45  typedef typename traits<LieGroup1>::Scalar Scalar;
46  enum
47  {
48  Options = traits<LieGroup1>::Options,
49  NQ = eval_set_dim < LieGroup1::NQ,
50  LieGroup2::NQ > ::value,
51  NV = eval_set_dim < LieGroup1::NV,
52  LieGroup2::NV > ::value
53  };
54  };
55 
56  template<typename LieGroup1, typename LieGroup2>
57  struct CartesianProductOperation
58  : public LieGroupBase<CartesianProductOperation<LieGroup1, LieGroup2>>
59  {
60  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(CartesianProductOperation);
61 
62  CartesianProductOperation()
63  : lg1()
64  , lg2()
65  {
66  }
67  // Get dimension of Lie Group vector representation
68  //
69  // For instance, for SO(3), the dimension of the vector representation is
70  // 4 (quaternion) while the dimension of the tangent space is 3.
71  Index nq() const
72  {
73  return lg1.nq() + lg2.nq();
74  }
75 
76  // Get dimension of Lie Group tangent space
77  Index nv() const
78  {
79  return lg1.nv() + lg2.nv();
80  }
81 
82  ConfigVector_t neutral() const
83  {
84  ConfigVector_t n;
85  n.resize(nq());
86  Qo1(n) = lg1.neutral();
87  Qo2(n) = lg2.neutral();
88  return n;
89  }
90 
91  std::string name() const
92  {
93  std::ostringstream oss;
94  oss << lg1.name() << "*" << lg2.name();
95  return oss.str();
96  }
97 
98  template<class ConfigL_t, class ConfigR_t, class Tangent_t>
99  void difference_impl(
100  const Eigen::MatrixBase<ConfigL_t> & q0,
101  const Eigen::MatrixBase<ConfigR_t> & q1,
102  const Eigen::MatrixBase<Tangent_t> & d) const
103  {
104  lg1.difference(Q1(q0), Q1(q1), Vo1(d));
105  lg2.difference(Q2(q0), Q2(q1), Vo2(d));
106  }
107 
108  template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
109  void dDifference_impl(
110  const Eigen::MatrixBase<ConfigL_t> & q0,
111  const Eigen::MatrixBase<ConfigR_t> & q1,
112  const Eigen::MatrixBase<JacobianOut_t> & J) const
113  {
114  J12(J).setZero();
115  J21(J).setZero();
116 
117  lg1.template dDifference<arg>(Q1(q0), Q1(q1), J11(J));
118  lg2.template dDifference<arg>(Q2(q0), Q2(q1), J22(J));
119  }
120 
121  template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
122  void integrate_impl(
123  const Eigen::MatrixBase<ConfigIn_t> & q,
124  const Eigen::MatrixBase<Velocity_t> & v,
125  const Eigen::MatrixBase<ConfigOut_t> & qout) const
126  {
127  lg1.integrate(Q1(q), V1(v), Qo1(qout));
128  lg2.integrate(Q2(q), V2(v), Qo2(qout));
129  }
130 
131  template<class Config_t, class Jacobian_t>
132  void integrateCoeffWiseJacobian_impl(
133  const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J) const
134  {
135  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
136  Jacobian_t & J_ = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J);
137  J_.topRightCorner(lg1.nq(), lg2.nv()).setZero();
138  J_.bottomLeftCorner(lg2.nq(), lg1.nv()).setZero();
139 
140  lg1.integrateCoeffWiseJacobian(Q1(q), J_.topLeftCorner(lg1.nq(), lg1.nv()));
141  lg2.integrateCoeffWiseJacobian(Q2(q), J_.bottomRightCorner(lg2.nq(), lg2.nv()));
142  }
143 
144  template<class Config_t, class Tangent_t, class JacobianOut_t>
145  void dIntegrate_dq_impl(
146  const Eigen::MatrixBase<Config_t> & q,
147  const Eigen::MatrixBase<Tangent_t> & v,
148  const Eigen::MatrixBase<JacobianOut_t> & J,
149  const AssignmentOperatorType op = SETTO) const
150  {
151  switch (op)
152  {
153  case SETTO:
154  J12(J).setZero();
155  J21(J).setZero();
156  // fallthrough
157  case ADDTO:
158  case RMTO:
159  lg1.dIntegrate_dq(Q1(q), V1(v), J11(J), op);
160  lg2.dIntegrate_dq(Q2(q), V2(v), J22(J), op);
161  break;
162  default:
163  assert(false && "Wrong Op requesed value");
164  break;
165  }
166  }
167 
168  template<class Config_t, class Tangent_t, class JacobianOut_t>
169  void dIntegrate_dv_impl(
170  const Eigen::MatrixBase<Config_t> & q,
171  const Eigen::MatrixBase<Tangent_t> & v,
172  const Eigen::MatrixBase<JacobianOut_t> & J,
173  const AssignmentOperatorType op = SETTO) const
174  {
175  switch (op)
176  {
177  case SETTO:
178  J12(J).setZero();
179  J21(J).setZero();
180  // fallthrough
181  case ADDTO:
182  case RMTO:
183  lg1.dIntegrate_dv(Q1(q), V1(v), J11(J), op);
184  lg2.dIntegrate_dv(Q2(q), V2(v), J22(J), op);
185  break;
186  default:
187  assert(false && "Wrong Op requesed value");
188  break;
189  }
190  }
191 
192  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
193  void dIntegrateTransport_dq_impl(
194  const Eigen::MatrixBase<Config_t> & q,
195  const Eigen::MatrixBase<Tangent_t> & v,
196  const Eigen::MatrixBase<JacobianIn_t> & J_in,
197  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
198  {
199  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
200  JacobianOut_t & Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t, J_in);
201  lg1.dIntegrateTransport_dq(
202  Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),
203  Jout.template topRows<LieGroup1::NV>());
204  lg2.dIntegrateTransport_dq(
205  Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),
206  Jout.template bottomRows<LieGroup2::NV>());
207  }
208 
209  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
210  void dIntegrateTransport_dv_impl(
211  const Eigen::MatrixBase<Config_t> & q,
212  const Eigen::MatrixBase<Tangent_t> & v,
213  const Eigen::MatrixBase<JacobianIn_t> & J_in,
214  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
215  {
216  JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
217  JacobianOut_t & Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t, J_in);
218  lg1.dIntegrateTransport_dv(
219  Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),
220  Jout.template topRows<LieGroup1::NV>());
221  lg2.dIntegrateTransport_dv(
222  Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),
223  Jout.template bottomRows<LieGroup2::NV>());
224  }
225 
226  template<class Config_t, class Tangent_t, class Jacobian_t>
227  void dIntegrateTransport_dq_impl(
228  const Eigen::MatrixBase<Config_t> & q,
229  const Eigen::MatrixBase<Tangent_t> & v,
230  const Eigen::MatrixBase<Jacobian_t> & Jin) const
231  {
232  Jacobian_t & J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, Jin);
233  lg1.dIntegrateTransport_dq(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
234  lg2.dIntegrateTransport_dq(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
235  }
236 
237  template<class Config_t, class Tangent_t, class Jacobian_t>
238  void dIntegrateTransport_dv_impl(
239  const Eigen::MatrixBase<Config_t> & q,
240  const Eigen::MatrixBase<Tangent_t> & v,
241  const Eigen::MatrixBase<Jacobian_t> & Jin) const
242  {
243  Jacobian_t & J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, Jin);
244  lg1.dIntegrateTransport_dv(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
245  lg2.dIntegrateTransport_dv(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
246  }
247 
248  template<class ConfigL_t, class ConfigR_t>
249  Scalar squaredDistance_impl(
250  const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const
251  {
252  return lg1.squaredDistance(Q1(q0), Q1(q1)) + lg2.squaredDistance(Q2(q0), Q2(q1));
253  }
254 
255  template<class Config_t>
256  void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const
257  {
258  lg1.normalize(Qo1(qout));
259  lg2.normalize(Qo2(qout));
260  }
261 
262  template<class Config_t>
263  bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec) const
264  {
265  return lg1.isNormalized(Qo1(qin), prec) && lg2.isNormalized(Qo2(qin), prec);
266  }
267 
268  template<class Config_t>
269  void random_impl(const Eigen::MatrixBase<Config_t> & qout) const
270  {
271  lg1.random(Qo1(qout));
272  lg2.random(Qo2(qout));
273  }
274 
275  template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
276  void randomConfiguration_impl(
277  const Eigen::MatrixBase<ConfigL_t> & lower,
278  const Eigen::MatrixBase<ConfigR_t> & upper,
279  const Eigen::MatrixBase<ConfigOut_t> & qout) const
280  {
281  lg1.randomConfiguration(Q1(lower), Q1(upper), Qo1(qout));
282  lg2.randomConfiguration(Q2(lower), Q2(upper), Qo2(qout));
283  }
284 
285  template<class ConfigL_t, class ConfigR_t>
286  bool isSameConfiguration_impl(
287  const Eigen::MatrixBase<ConfigL_t> & q0,
288  const Eigen::MatrixBase<ConfigR_t> & q1,
289  const Scalar & prec) const
290  {
291  return lg1.isSameConfiguration(Q1(q0), Q1(q1), prec)
292  && lg2.isSameConfiguration(Q2(q0), Q2(q1), prec);
293  }
294 
295  bool isEqual_impl(const CartesianProductOperation & other) const
296  {
297  return lg1 == other.lg1 && lg2 == other.lg2;
298  }
299 
300  LieGroup1 lg1;
301  LieGroup2 lg2;
302 
303  private:
304  // VectorSpaceOperationTpl<-1> within CartesianProductOperation will not work
305  // if Eigen version is lower than 3.2.1
306 #if EIGEN_VERSION_AT_LEAST(3, 2, 1)
307  #define REMOVE_IF_EIGEN_TOO_LOW(x) x
308 #else
309  #define REMOVE_IF_EIGEN_TOO_LOW(x)
310 #endif
311 
312  template<typename Config>
313  typename Config ::template ConstFixedSegmentReturnType<LieGroup1::NQ>::Type
314  Q1(const Eigen::MatrixBase<Config> & q) const
315  {
316  return q.derived().template head<LieGroup1::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nq()));
317  }
318  template<typename Config>
319  typename Config ::template ConstFixedSegmentReturnType<LieGroup2::NQ>::Type
320  Q2(const Eigen::MatrixBase<Config> & q) const
321  {
322  return q.derived().template tail<LieGroup2::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nq()));
323  }
324  template<typename Tangent>
325  typename Tangent::template ConstFixedSegmentReturnType<LieGroup1::NV>::Type
326  V1(const Eigen::MatrixBase<Tangent> & v) const
327  {
328  return v.derived().template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nv()));
329  }
330  template<typename Tangent>
331  typename Tangent::template ConstFixedSegmentReturnType<LieGroup2::NV>::Type
332  V2(const Eigen::MatrixBase<Tangent> & v) const
333  {
334  return v.derived().template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nv()));
335  }
336 
337  template<typename Config>
338  typename Config ::template FixedSegmentReturnType<LieGroup1::NQ>::Type
339  Qo1(const Eigen::MatrixBase<Config> & q) const
340  {
341  return PINOCCHIO_EIGEN_CONST_CAST(Config, q).template head<LieGroup1::NQ>(
342  REMOVE_IF_EIGEN_TOO_LOW(lg1.nq()));
343  }
344  template<typename Config>
345  typename Config ::template FixedSegmentReturnType<LieGroup2::NQ>::Type
346  Qo2(const Eigen::MatrixBase<Config> & q) const
347  {
348  return PINOCCHIO_EIGEN_CONST_CAST(Config, q).template tail<LieGroup2::NQ>(
349  REMOVE_IF_EIGEN_TOO_LOW(lg2.nq()));
350  }
351  template<typename Tangent>
352  typename Tangent::template FixedSegmentReturnType<LieGroup1::NV>::Type
353  Vo1(const Eigen::MatrixBase<Tangent> & v) const
354  {
355  return PINOCCHIO_EIGEN_CONST_CAST(Tangent, v)
356  .template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nv()));
357  }
358  template<typename Tangent>
359  typename Tangent::template FixedSegmentReturnType<LieGroup2::NV>::Type
360  Vo2(const Eigen::MatrixBase<Tangent> & v) const
361  {
362  return PINOCCHIO_EIGEN_CONST_CAST(Tangent, v)
363  .template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nv()));
364  }
365 
366  template<typename Jac>
367  Eigen::Block<Jac, LieGroup1::NV, LieGroup1::NV> J11(const Eigen::MatrixBase<Jac> & J) const
368  {
369  return PINOCCHIO_EIGEN_CONST_CAST(Jac, J)
370  .template topLeftCorner<LieGroup1::NV, LieGroup1::NV>(lg1.nv(), lg1.nv());
371  }
372  template<typename Jac>
373  Eigen::Block<Jac, LieGroup1::NV, LieGroup2::NV> J12(const Eigen::MatrixBase<Jac> & J) const
374  {
375  return PINOCCHIO_EIGEN_CONST_CAST(Jac, J)
376  .template topRightCorner<LieGroup1::NV, LieGroup2::NV>(lg1.nv(), lg2.nv());
377  }
378  template<typename Jac>
379  Eigen::Block<Jac, LieGroup2::NV, LieGroup1::NV> J21(const Eigen::MatrixBase<Jac> & J) const
380  {
381  return PINOCCHIO_EIGEN_CONST_CAST(Jac, J)
382  .template bottomLeftCorner<LieGroup2::NV, LieGroup1::NV>(lg2.nv(), lg1.nv());
383  }
384  template<typename Jac>
385  Eigen::Block<Jac, LieGroup2::NV, LieGroup2::NV> J22(const Eigen::MatrixBase<Jac> & J) const
386  {
387  return PINOCCHIO_EIGEN_CONST_CAST(Jac, J)
388  .template bottomRightCorner<LieGroup2::NV, LieGroup2::NV>(lg2.nv(), lg2.nv());
389  }
390 #undef REMOVE_IF_EIGEN_TOO_LOW
391 
392  }; // struct CartesianProductOperation
393 
394 } // namespace pinocchio
395 
396 #endif // ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::Dynamic
const int Dynamic
Definition: fwd.hpp:140
pinocchio::eval_set_dim
Definition: cartesian-product.hpp:14
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72