pinocchio  2.3.1-dirty
compute-all-terms.hpp
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_compute_all_terms_hpp__
6 #define __pinocchio_compute_all_terms_hpp__
7 
8 #include "pinocchio/multibody/visitor.hpp"
9 #include "pinocchio/multibody/model.hpp"
10 #include "pinocchio/multibody/data.hpp"
11 #include "pinocchio/spatial/act-on-set.hpp"
12 #include "pinocchio/algorithm/center-of-mass.hpp"
13 #include "pinocchio/algorithm/energy.hpp"
14 #include "pinocchio/algorithm/check.hpp"
15 
16 namespace pinocchio
17 {
39  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
40  inline void computeAllTerms(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
41  DataTpl<Scalar,Options,JointCollectionTpl> & data,
42  const Eigen::MatrixBase<ConfigVectorType> & q,
43  const Eigen::MatrixBase<TangentVectorType> & v);
44 
45 } // namespace pinocchio
46 
47 
49 namespace pinocchio
50 {
51  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
52  struct CATForwardStep
53  : public fusion::JointUnaryVisitorBase< CATForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> >
54  {
55  typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
56  typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
57 
58  typedef boost::fusion::vector<const Model &,
59  Data &,
60  const ConfigVectorType &,
61  const TangentVectorType &
62  > ArgsType;
63 
64  template<typename JointModel>
65  static void algo(const JointModelBase<JointModel> & jmodel,
66  JointDataBase<typename JointModel::JointDataDerived> & jdata,
67  const Model & model,
68  Data & data,
69  const Eigen::MatrixBase<ConfigVectorType> & q,
70  const Eigen::MatrixBase<TangentVectorType> & v)
71  {
72  typedef typename Model::JointIndex JointIndex;
73  typedef typename Data::Inertia Inertia;
74 
75  const JointIndex & i = jmodel.id();
76  const JointIndex & parent = model.parents[i];
77 
78  jmodel.calc(jdata.derived(),q.derived(),v.derived());
79 
80  // CRBA
81  data.liMi[i] = model.jointPlacements[i]*jdata.M();
82  data.Ycrb[i] = model.inertias[i];
83 
84  // Jacobian + NLE
85  data.v[i] = jdata.v();
86 
87  if(parent>0)
88  {
89  data.oMi[i] = data.oMi[parent]*data.liMi[i];
90  data.v[i] += data.liMi[i].actInv(data.v[parent]);
91  }
92  else
93  data.oMi[i] = data.liMi[i];
94 
95  jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
96 
97  data.a_gf[i] = data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
98  if (parent > 0)
99  data.a[i] += data.liMi[i].actInv(data.a[parent]);
100 
101  data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
102 
103  data.f[i] = model.inertias[i]*data.a_gf[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
104 
105  // CoM
106  const Scalar & mass = model.inertias[i].mass();
107  const typename Inertia::Vector3 & lever = model.inertias[i].lever();
108 
109  data.com[i].noalias() = mass * lever;
110  data.mass[i] = mass;
111 
112  data.vcom[i].noalias() = mass * (data.v[i].angular().cross(lever) + data.v[i].linear());
113  }
114 
115  };
116 
117  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
118  struct CATBackwardStep
119  : public fusion::JointUnaryVisitorBase <CATBackwardStep<Scalar,Options,JointCollectionTpl> >
120  {
121  typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
122  typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
123 
124  typedef boost::fusion::vector<const Model &,
125  Data &
126  > ArgsType;
127 
128  template<typename JointModel>
129  static void algo(const JointModelBase<JointModel> & jmodel,
130  JointDataBase<typename JointModel::JointDataDerived> & jdata,
131  const Model & model,
132  Data & data)
133  {
134  /*
135  * F[1:6,i] = Y*S
136  * M[i,SUBTREE] = S'*F[1:6,SUBTREE]
137  * if li>0
138  * Yli += liXi Yi
139  * F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
140  */
141  typedef typename Model::JointIndex JointIndex;
142  typedef typename Data::SE3 SE3;
143 
144  const JointIndex & i = jmodel.id();
145  const JointIndex & parent = model.parents[i];
146  const SE3 & oMi = data.oMi[i];
147 
148  /* F[1:6,i] = Y*S */
149  jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
150 
151  /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
152  data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i])
153  = jdata.S().transpose()*data.Fcrb[i].middleCols(jmodel.idx_v(),data.nvSubtree[i]);
154 
155 
156  jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose()*data.f[i];
157  if(parent>0)
158  {
159  /* Yli += liXi Yi */
160  data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
161 
162  /* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
163  Eigen::Block<typename Data::Matrix6x> jF
164  = data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
165  Eigen::Block<typename Data::Matrix6x> iF
166  = data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
167  forceSet::se3Action(data.liMi[i], iF, jF);
168 
169  data.f[parent] += data.liMi[i].act(data.f[i]);
170  }
171 
172  // CoM
173  const SE3 & liMi = data.liMi[i];
174 
175  data.com[parent] += (liMi.rotation()*data.com[i]
176  + data.mass[i] * liMi.translation());
177 
178  typename SE3::Vector3 com_in_world(oMi.rotation() * data.com[i] + data.mass[i] * oMi.translation());
179 
180  data.vcom[parent] += liMi.rotation()*data.vcom[i];
181  data.mass[parent] += data.mass[i];
182 
183  typedef typename Data::Matrix6x Matrix6x;
184  typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
185 
186  ColBlock Jcols = jmodel.jointCols(data.J);
187 
188  if( JointModel::NV==1 )
189  data.Jcom.col(jmodel.idx_v())
190  = data.mass[i] * Jcols.template topLeftCorner<3,1>()
191  - com_in_world.cross(Jcols.template bottomLeftCorner<3,1>()) ;
192  else
193  jmodel.jointCols(data.Jcom)
194  = data.mass[i] * Jcols.template topRows<3>()
195  - skew(com_in_world) * Jcols.template bottomRows<3>();
196 
197  data.com[i] /= data.mass[i];
198  data.vcom[i] /= data.mass[i];
199  }
200  };
201 
202  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
203  inline void computeAllTerms(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
204  DataTpl<Scalar,Options,JointCollectionTpl> & data,
205  const Eigen::MatrixBase<ConfigVectorType> & q,
206  const Eigen::MatrixBase<TangentVectorType> & v)
207  {
208  assert(model.check(data) && "data is not consistent with model.");
209  PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
210  PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The velocity vector is not of right size");
211 
212  typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
213  typedef typename Model::JointIndex JointIndex;
214 
215  data.v[0].setZero();
216  data.a[0].setZero();
217  data.a_gf[0] = -model.gravity;
218 
219  data.mass[0] = 0;
220  data.com[0].setZero();
221  data.vcom[0].setZero();
222 
223  typedef CATForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Pass1;
224  for(JointIndex i=1;i<(JointIndex) model.njoints;++i)
225  {
226  Pass1::run(model.joints[i],data.joints[i],
227  typename Pass1::ArgsType(model,data,q.derived(),v.derived()));
228  }
229 
230  typedef CATBackwardStep<Scalar,Options,JointCollectionTpl> Pass2;
231  for(JointIndex i=(JointIndex)(model.njoints-1);i>0;--i)
232  {
233  Pass2::run(model.joints[i],data.joints[i],
234  typename Pass2::ArgsType(model,data));
235  }
236 
237  // CoM
238  data.com[0] /= data.mass[0];
239  data.vcom[0] /= data.mass[0];
240 
241  // JCoM
242  data.Jcom /= data.mass[0];
243 
244  // Energy
245  computeKineticEnergy(model, data);
246  computePotentialEnergy(model, data);
247 
248  }
249 }
250 
251 
252 #endif // ifndef __pinocchio_compute_all_terms_hpp__
253 
Scalar computePotentialEnergy(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Computes the potential energy of the system, i.e. the potential energy linked to the gravity field...
void computeAllTerms(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes efficiently all the terms needed for dynamic simulation. It is equivalent to the call at the...
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
static void se3Action(const SE3Tpl< Scalar, Options > &m, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
SE3 action on a set of forces, represented by a 6xN matrix whose each column represent a spatial forc...
Main pinocchio namespace.
Definition: treeview.dox:24
Scalar computeKineticEnergy(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Computes the kinetic energy of the system. The result is accessible through data.kinetic_energy.
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
Definition: skew.hpp:21
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
Definition: se3-base.hpp:86