pinocchio  2.3.1-dirty
cartesian-product.hpp
1 //
2 // Copyright (c) 2016-2018 CNRS
3 //
4 
5 #ifndef __pinocchio_cartesian_product_operation_hpp__
6 #define __pinocchio_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>
32 
33  template<typename LieGroup1, typename LieGroup2>
34  struct traits<CartesianProductOperation<LieGroup1, LieGroup2> > {
35  typedef typename traits<LieGroup1>::Scalar Scalar;
36  enum {
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(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) const
129  {
130  J12(J).setZero();
131  J21(J).setZero();
132  lg1_.dIntegrate_dq(Q1(q), V1(v), J11(J));
133  lg2_.dIntegrate_dq(Q2(q), V2(v), J22(J));
134  }
135 
136  template <class Config_t, class Tangent_t, class JacobianOut_t>
137  void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,
138  const Eigen::MatrixBase<Tangent_t> & v,
139  const Eigen::MatrixBase<JacobianOut_t> & J) const
140  {
141  J12(J).setZero();
142  J21(J).setZero();
143  lg1_.dIntegrate_dv(Q1(q), V1(v), J11(J));
144  lg2_.dIntegrate_dv(Q2(q), V2(v), J22(J));
145  }
146 
147  template <class ConfigL_t, class ConfigR_t>
148  Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
149  const Eigen::MatrixBase<ConfigR_t> & q1) const
150  {
151  return lg1_.squaredDistance(Q1(q0), Q1(q1))
152  + lg2_.squaredDistance(Q2(q0), Q2(q1));
153  }
154 
155  template <class Config_t>
156  void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const
157  {
158  lg1_.normalize(Qo1(qout));
159  lg2_.normalize(Qo2(qout));
160  }
161 
162  template <class Config_t>
163  void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
164  {
165  lg1_.random(Qo1(qout));
166  lg2_.random(Qo2(qout));
167  }
168 
169  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
170  void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
171  const Eigen::MatrixBase<ConfigR_t> & upper,
172  const Eigen::MatrixBase<ConfigOut_t> & qout)
173  const
174  {
175  lg1_.randomConfiguration(Q1(lower), Q1(upper), Qo1(qout));
176  lg2_.randomConfiguration(Q2(lower), Q2(upper), Qo2(qout));
177  }
178 
179  template <class ConfigL_t, class ConfigR_t>
180  bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
181  const Eigen::MatrixBase<ConfigR_t> & q1,
182  const Scalar & prec) const
183  {
184  return lg1_.isSameConfiguration(Q1(q0), Q1(q1), prec)
185  && lg2_.isSameConfiguration(Q2(q0), Q2(q1), prec);
186  }
187  private:
188  LieGroup1 lg1_;
189  LieGroup2 lg2_;
190 
191  // VectorSpaceOperationTpl<-1> within CartesianProductOperation will not work
192  // if Eigen version is lower than 3.2.1
193 #if EIGEN_VERSION_AT_LEAST(3,2,1)
194 # define REMOVE_IF_EIGEN_TOO_LOW(x) x
195 #else
196 # define REMOVE_IF_EIGEN_TOO_LOW(x)
197 #endif
198 
199  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())); }
200  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())); }
201  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())); }
202  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())); }
203 
204  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())); }
205  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())); }
206  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())); }
207  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())); }
208 
209  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()); }
210  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()); }
211  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()); }
212  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()); }
213 #undef REMOVE_IF_EIGEN_TOO_LOW
214 
215  }; // struct CartesianProductOperation
216 
217 } // namespace pinocchio
218 
219 #endif // ifndef __pinocchio_cartesian_product_operation_hpp__
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
void dDifference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVector1 > &q0, const Eigen::MatrixBase< ConfigVector2 > &q1, const Eigen::MatrixBase< JacobianMatrix > &J, const ArgumentPosition arg)
Computes the Jacobian of a small variation of the configuration vector into the tangent space at iden...
Source from #include <cppad/example/cppad_eigen.hpp>
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
Main pinocchio namespace.
Definition: treeview.dox:24
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32