pinocchio  2.4.4
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  lg1_.dIntegrate_dq(Q1(q), V1(v), J11(J),SETTO);
137  lg2_.dIntegrate_dq(Q2(q), V2(v), J22(J),SETTO);
138  break;
139  case ADDTO:
140  lg1_.dIntegrate_dq(Q1(q), V1(v), J11(J),ADDTO);
141  lg2_.dIntegrate_dq(Q2(q), V2(v), J22(J),ADDTO);
142  break;
143  case RMTO:
144  lg1_.dIntegrate_dq(Q1(q), V1(v), J11(J),RMTO);
145  lg2_.dIntegrate_dq(Q2(q), V2(v), J22(J),RMTO);
146  break;
147  default:
148  assert(false && "Wrong Op requesed value");
149  break;
150  }
151  }
152 
153  template <class Config_t, class Tangent_t, class JacobianOut_t>
154  void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,
155  const Eigen::MatrixBase<Tangent_t> & v,
156  const Eigen::MatrixBase<JacobianOut_t> & J,
157  const AssignmentOperatorType op=SETTO) const
158  {
159  switch(op)
160  {
161  case SETTO:
162  J12(J).setZero();
163  J21(J).setZero();
164  lg1_.dIntegrate_dv(Q1(q), V1(v), J11(J),SETTO);
165  lg2_.dIntegrate_dv(Q2(q), V2(v), J22(J),SETTO);
166  break;
167  case ADDTO:
168  lg1_.dIntegrate_dv(Q1(q), V1(v), J11(J),ADDTO);
169  lg2_.dIntegrate_dv(Q2(q), V2(v), J22(J),ADDTO);
170  break;
171  case RMTO:
172  lg1_.dIntegrate_dv(Q1(q), V1(v), J11(J),RMTO);
173  lg2_.dIntegrate_dv(Q2(q), V2(v), J22(J),RMTO);
174  break;
175  default:
176  assert(false && "Wrong Op requesed value");
177  break;
178  }
179  }
180 
181  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
182  void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
183  const Eigen::MatrixBase<Tangent_t> & v,
184  const Eigen::MatrixBase<JacobianIn_t> & J_in,
185  const Eigen::MatrixBase<JacobianOut_t> & J_out) const
186  {
187  JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
188  JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in);
189  lg1_.dIntegrateTransport_dq(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
190  lg2_.dIntegrateTransport_dq(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
191  }
192 
193  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
194  void dIntegrateTransport_dv_impl(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_dv(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
202  lg2_.dIntegrateTransport_dv(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
203  }
204 
205 
206  template <class Config_t, class Tangent_t, class Jacobian_t>
207  void dIntegrateTransport_dq_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_dq(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
213  lg2_.dIntegrateTransport_dq(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
214  }
215 
216  template <class Config_t, class Tangent_t, class Jacobian_t>
217  void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
218  const Eigen::MatrixBase<Tangent_t> & v,
219  const Eigen::MatrixBase<Jacobian_t> & Jin) const
220  {
221  Jacobian_t& J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,Jin);
222  lg1_.dIntegrateTransport_dv(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
223  lg2_.dIntegrateTransport_dv(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
224  }
225 
226  template <class ConfigL_t, class ConfigR_t>
227  Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
228  const Eigen::MatrixBase<ConfigR_t> & q1) const
229  {
230  return lg1_.squaredDistance(Q1(q0), Q1(q1))
231  + lg2_.squaredDistance(Q2(q0), Q2(q1));
232  }
233 
234  template <class Config_t>
235  void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const
236  {
237  lg1_.normalize(Qo1(qout));
238  lg2_.normalize(Qo2(qout));
239  }
240 
241  template <class Config_t>
242  void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
243  {
244  lg1_.random(Qo1(qout));
245  lg2_.random(Qo2(qout));
246  }
247 
248  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
249  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
250  const Eigen::MatrixBase<ConfigR_t> & upper,
251  const Eigen::MatrixBase<ConfigOut_t> & qout)
252  const
253  {
254  lg1_.randomConfiguration(Q1(lower), Q1(upper), Qo1(qout));
255  lg2_.randomConfiguration(Q2(lower), Q2(upper), Qo2(qout));
256  }
257 
258  template <class ConfigL_t, class ConfigR_t>
259  bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
260  const Eigen::MatrixBase<ConfigR_t> & q1,
261  const Scalar & prec) const
262  {
263  return lg1_.isSameConfiguration(Q1(q0), Q1(q1), prec)
264  && lg2_.isSameConfiguration(Q2(q0), Q2(q1), prec);
265  }
266  private:
267  LieGroup1 lg1_;
268  LieGroup2 lg2_;
269 
270  // VectorSpaceOperationTpl<-1> within CartesianProductOperation will not work
271  // if Eigen version is lower than 3.2.1
272 #if EIGEN_VERSION_AT_LEAST(3,2,1)
273 # define REMOVE_IF_EIGEN_TOO_LOW(x) x
274 #else
275 # define REMOVE_IF_EIGEN_TOO_LOW(x)
276 #endif
277 
278  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())); }
279  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())); }
280  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())); }
281  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())); }
282 
283  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())); }
284  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())); }
285  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())); }
286  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())); }
287 
288  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()); }
289  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()); }
290  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()); }
291  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()); }
292 #undef REMOVE_IF_EIGEN_TOO_LOW
293 
294  }; // struct CartesianProductOperation
295 
296 } // namespace pinocchio
297 
298 #endif // ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__
pinocchio::nv
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
pinocchio::nq
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
Eigen
Definition: casadi.hpp:47
pinocchio::name
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Definition: joint-configuration.hpp:257
pinocchio::eval_set_dim
Definition: cartesian-product.hpp:13
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35