pinocchio  2.7.1
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 { value = dim1 + dim2 };
16  };
17 
18  template<int dim>
19  struct eval_set_dim<dim,Eigen::Dynamic>
20  {
21  enum { value = Eigen::Dynamic };
22  };
23 
24  template<int dim>
25  struct eval_set_dim<Eigen::Dynamic,dim>
26  {
27  enum { value = Eigen::Dynamic };
28  };
29 
30  template<typename LieGroup1, typename LieGroup2>
31  struct CartesianProductOperation;
32 
33  template<typename LieGroup1, typename LieGroup2>
34  struct traits<CartesianProductOperation<LieGroup1, LieGroup2> > {
35  typedef typename traits<LieGroup1>::Scalar Scalar;
36  enum {
37  Options = traits<LieGroup1>::Options,
38  NQ = eval_set_dim<LieGroup1::NQ,LieGroup2::NQ>::value,
39  NV = eval_set_dim<LieGroup1::NV,LieGroup2::NV>::value
40  };
41  };
42 
43  template<typename LieGroup1, typename LieGroup2>
44  struct CartesianProductOperation : public LieGroupBase <CartesianProductOperation<LieGroup1, LieGroup2> >
45  {
46  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(CartesianProductOperation);
47 
48  CartesianProductOperation () : lg1 (), lg2 ()
49  {
50  }
51  // Get dimension of Lie Group vector representation
52  //
53  // For instance, for SO(3), the dimension of the vector representation is
54  // 4 (quaternion) while the dimension of the tangent space is 3.
55  Index nq () const
56  {
57  return lg1.nq () + lg2.nq ();
58  }
59 
60  // Get dimension of Lie Group tangent space
61  Index nv () const
62  {
63  return lg1.nv () + lg2.nv ();
64  }
65 
66  ConfigVector_t neutral () const
67  {
68  ConfigVector_t n;
69  n.resize (nq ());
70  Qo1(n) = lg1.neutral ();
71  Qo2(n) = lg2.neutral ();
72  return n;
73  }
74 
75  std::string name () const
76  {
77  std::ostringstream oss; oss << lg1.name () << "*" << lg2.name ();
78  return oss.str ();
79  }
80 
81  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
82  void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
83  const Eigen::MatrixBase<ConfigR_t> & q1,
84  const Eigen::MatrixBase<Tangent_t> & d) const
85  {
86  lg1.difference(Q1(q0), Q1(q1), Vo1(d));
87  lg2.difference(Q2(q0), Q2(q1), Vo2(d));
88  }
89 
90  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
91  void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
92  const Eigen::MatrixBase<ConfigR_t> & q1,
93  const Eigen::MatrixBase<JacobianOut_t> & J) const
94  {
95  J12(J).setZero();
96  J21(J).setZero();
97 
98  lg1.template dDifference<arg> (Q1(q0), Q1(q1), J11(J));
99  lg2.template dDifference<arg> (Q2(q0), Q2(q1), J22(J));
100  }
101 
102  template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
103  void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
104  const Eigen::MatrixBase<Velocity_t> & v,
105  const Eigen::MatrixBase<ConfigOut_t> & qout) const
106  {
107  lg1.integrate(Q1(q), V1(v), Qo1(qout));
108  lg2.integrate(Q2(q), V2(v), Qo2(qout));
109  }
110 
111  template <class Config_t, class Jacobian_t>
112  void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
113  const Eigen::MatrixBase<Jacobian_t> & J) const
114  {
115  assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
116  Jacobian_t & J_ = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
117  J_.topRightCorner(lg1.nq(),lg2.nv()).setZero();
118  J_.bottomLeftCorner(lg2.nq(),lg1.nv()).setZero();
119 
120  lg1.integrateCoeffWiseJacobian(Q1(q),
121  J_.topLeftCorner(lg1.nq(),lg1.nv()));
122  lg2.integrateCoeffWiseJacobian(Q2(q), J_.bottomRightCorner(lg2.nq(),lg2.nv()));
123  }
124 
125  template <class Config_t, class Tangent_t, class JacobianOut_t>
126  void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & q,
127  const Eigen::MatrixBase<Tangent_t> & v,
128  const Eigen::MatrixBase<JacobianOut_t> & J,
129  const AssignmentOperatorType op=SETTO) const
130  {
131  switch(op)
132  {
133  case SETTO:
134  J12(J).setZero();
135  J21(J).setZero();
136  // fallthrough
137  case ADDTO:
138  case RMTO:
139  lg1.dIntegrate_dq(Q1(q), V1(v), J11(J),op);
140  lg2.dIntegrate_dq(Q2(q), V2(v), J22(J),op);
141  break;
142  default:
143  assert(false && "Wrong Op requesed value");
144  break;
145  }
146  }
147 
148  template <class Config_t, class Tangent_t, class JacobianOut_t>
149  void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,
150  const Eigen::MatrixBase<Tangent_t> & v,
151  const Eigen::MatrixBase<JacobianOut_t> & J,
152  const AssignmentOperatorType op=SETTO) const
153  {
154  switch(op)
155  {
156  case SETTO:
157  J12(J).setZero();
158  J21(J).setZero();
159  // fallthrough
160  case ADDTO:
161  case RMTO:
162  lg1.dIntegrate_dv(Q1(q), V1(v), J11(J),op);
163  lg2.dIntegrate_dv(Q2(q), V2(v), J22(J),op);
164  break;
165  default:
166  assert(false && "Wrong Op requesed value");
167  break;
168  }
169  }
170 
171  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
172  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
173  const Eigen::MatrixBase<Tangent_t> & v,
174  const Eigen::MatrixBase<JacobianIn_t> & J_in,
175  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
176  {
177  JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
178  JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in);
179  lg1.dIntegrateTransport_dq(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
180  lg2.dIntegrateTransport_dq(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
181  }
182 
183  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
184  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
185  const Eigen::MatrixBase<Tangent_t> & v,
186  const Eigen::MatrixBase<JacobianIn_t> & J_in,
187  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
188  {
189  JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
190  JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in);
191  lg1.dIntegrateTransport_dv(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
192  lg2.dIntegrateTransport_dv(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
193  }
194 
195 
196  template <class Config_t, class Tangent_t, class Jacobian_t>
197  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
198  const Eigen::MatrixBase<Tangent_t> & v,
199  const Eigen::MatrixBase<Jacobian_t> & Jin) const
200  {
201  Jacobian_t& J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,Jin);
202  lg1.dIntegrateTransport_dq(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
203  lg2.dIntegrateTransport_dq(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
204  }
205 
206  template <class Config_t, class Tangent_t, class Jacobian_t>
207  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
208  const Eigen::MatrixBase<Tangent_t> & v,
209  const Eigen::MatrixBase<Jacobian_t> & Jin) const
210  {
211  Jacobian_t& J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,Jin);
212  lg1.dIntegrateTransport_dv(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
213  lg2.dIntegrateTransport_dv(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
214  }
215 
216  template <class ConfigL_t, class ConfigR_t>
217  Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
218  const Eigen::MatrixBase<ConfigR_t> & q1) const
219  {
220  return lg1.squaredDistance(Q1(q0), Q1(q1))
221  + lg2.squaredDistance(Q2(q0), Q2(q1));
222  }
223 
224  template <class Config_t>
225  void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const
226  {
227  lg1.normalize(Qo1(qout));
228  lg2.normalize(Qo2(qout));
229  }
230 
231  template <class Config_t>
232  bool isNormalized_impl (const Eigen::MatrixBase<Config_t>& qin, const Scalar& prec) const
233  {
234  return lg1.isNormalized(Qo1(qin), prec) && lg2.isNormalized(Qo2(qin), prec);
235  }
236 
237  template <class Config_t>
238  void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
239  {
240  lg1.random(Qo1(qout));
241  lg2.random(Qo2(qout));
242  }
243 
244  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
245  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
246  const Eigen::MatrixBase<ConfigR_t> & upper,
247  const Eigen::MatrixBase<ConfigOut_t> & qout)
248  const
249  {
250  lg1.randomConfiguration(Q1(lower), Q1(upper), Qo1(qout));
251  lg2.randomConfiguration(Q2(lower), Q2(upper), Qo2(qout));
252  }
253 
254  template <class ConfigL_t, class ConfigR_t>
255  bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
256  const Eigen::MatrixBase<ConfigR_t> & q1,
257  const Scalar & prec) const
258  {
259  return lg1.isSameConfiguration(Q1(q0), Q1(q1), prec)
260  && lg2.isSameConfiguration(Q2(q0), Q2(q1), prec);
261  }
262 
263  bool isEqual_impl (const CartesianProductOperation& other) const
264  {
265  return lg1 == other.lg1 && lg2 == other.lg2;
266  }
267 
268  LieGroup1 lg1;
269  LieGroup2 lg2;
270 
271  private:
272  // VectorSpaceOperationTpl<-1> within CartesianProductOperation will not work
273  // if Eigen version is lower than 3.2.1
274 #if EIGEN_VERSION_AT_LEAST(3,2,1)
275 # define REMOVE_IF_EIGEN_TOO_LOW(x) x
276 #else
277 # define REMOVE_IF_EIGEN_TOO_LOW(x)
278 #endif
279 
280  template <typename Config > typename Config ::template ConstFixedSegmentReturnType<LieGroup1::NQ>::Type Q1 (const Eigen::MatrixBase<Config >& q) const { return q.derived().template head<LieGroup1::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nq())); }
281  template <typename Config > typename Config ::template ConstFixedSegmentReturnType<LieGroup2::NQ>::Type Q2 (const Eigen::MatrixBase<Config >& q) const { return q.derived().template tail<LieGroup2::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nq())); }
282  template <typename Tangent> typename Tangent::template ConstFixedSegmentReturnType<LieGroup1::NV>::Type V1 (const Eigen::MatrixBase<Tangent>& v) const { return v.derived().template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nv())); }
283  template <typename Tangent> typename Tangent::template ConstFixedSegmentReturnType<LieGroup2::NV>::Type V2 (const Eigen::MatrixBase<Tangent>& v) const { return v.derived().template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nv())); }
284 
285  template <typename Config > typename Config ::template FixedSegmentReturnType<LieGroup1::NQ>::Type Qo1 (const Eigen::MatrixBase<Config >& q) const { return PINOCCHIO_EIGEN_CONST_CAST(Config,q).template head<LieGroup1::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nq())); }
286  template <typename Config > typename Config ::template FixedSegmentReturnType<LieGroup2::NQ>::Type Qo2 (const Eigen::MatrixBase<Config >& q) const { return PINOCCHIO_EIGEN_CONST_CAST(Config,q).template tail<LieGroup2::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nq())); }
287  template <typename Tangent> typename Tangent::template FixedSegmentReturnType<LieGroup1::NV>::Type Vo1 (const Eigen::MatrixBase<Tangent>& v) const { return PINOCCHIO_EIGEN_CONST_CAST(Tangent,v).template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nv())); }
288  template <typename Tangent> typename Tangent::template FixedSegmentReturnType<LieGroup2::NV>::Type Vo2 (const Eigen::MatrixBase<Tangent>& v) const { return PINOCCHIO_EIGEN_CONST_CAST(Tangent,v).template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nv())); }
289 
290  template <typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup1::NV> J11 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template topLeftCorner<LieGroup1::NV, LieGroup1::NV>(lg1.nv(),lg1.nv()); }
291  template <typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup2::NV> J12 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template topRightCorner<LieGroup1::NV, LieGroup2::NV>(lg1.nv(),lg2.nv()); }
292  template <typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup1::NV> J21 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template bottomLeftCorner<LieGroup2::NV, LieGroup1::NV>(lg2.nv(),lg1.nv()); }
293  template <typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup2::NV> J22 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template bottomRightCorner<LieGroup2::NV, LieGroup2::NV>(lg2.nv(),lg2.nv()); }
294 #undef REMOVE_IF_EIGEN_TOO_LOW
295 
296  }; // struct CartesianProductOperation
297 
298 } // namespace pinocchio
299 
300 #endif // ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__
pinocchio::CartesianProductOperation
Definition: cartesian-product.hpp:31
pinocchio::eval_set_dim
Definition: cartesian-product.hpp:13
pinocchio::LieGroupBase
Definition: liegroup-base.hpp:40
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11