pinocchio  UNKNOWN
aba.hxx
1 //
2 // Copyright (c) 2016-2018 CNRS
3 //
4 // This file is part of Pinocchio
5 // Pinocchio is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // Pinocchio is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // Pinocchio If not, see
16 // <http://www.gnu.org/licenses/>.
17 
18 #ifndef __se3_aba_hxx__
19 #define __se3_aba_hxx__
20 
21 #include "pinocchio/spatial/act-on-set.hpp"
22 #include "pinocchio/multibody/visitor.hpp"
23 #include "pinocchio/algorithm/check.hpp"
24 
26 
27 namespace se3
28 {
29  struct AbaForwardStep1 : public fusion::JointVisitor<AbaForwardStep1>
30  {
31  typedef boost::fusion::vector<const se3::Model &,
32  se3::Data &,
33  const Eigen::VectorXd &,
34  const Eigen::VectorXd &
35  > ArgsType;
36 
37  JOINT_VISITOR_INIT(AbaForwardStep1);
38 
39  template<typename JointModel>
40  static void algo(const se3::JointModelBase<JointModel> & jmodel,
41  se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
42  const se3::Model & model,
43  se3::Data & data,
44  const Eigen::VectorXd & q,
45  const Eigen::VectorXd & v)
46  {
47  const Model::JointIndex & i = jmodel.id();
48  jmodel.calc(jdata.derived(),q,v);
49 
50  const Model::Index & parent = model.parents[i];
51  data.liMi[i] = model.jointPlacements[i] * jdata.M();
52 
53  data.v[i] = jdata.v();
54  if (parent>0)
55  data.v[i] += data.liMi[i].actInv(data.v[parent]);
56 
57  data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
58 
59  data.Yaba[i] = model.inertias[i].matrix();
60  data.f[i] = model.inertias[i].vxiv(data.v[i]); // -f_ext
61  }
62 
63  };
64 
65  struct AbaBackwardStep : public fusion::JointVisitor<AbaBackwardStep>
66  {
67  typedef boost::fusion::vector<const Model &,
68  Data &> ArgsType;
69 
70  JOINT_VISITOR_INIT(AbaBackwardStep);
71 
72  template<typename JointModel>
73  static void algo(const JointModelBase<JointModel> & jmodel,
74  JointDataBase<typename JointModel::JointDataDerived> & jdata,
75  const Model & model,
76  Data & data)
77  {
78  const Model::JointIndex & i = jmodel.id();
79  const Model::Index & parent = model.parents[i];
80  Inertia::Matrix6 & Ia = data.Yaba[i];
81 
82  jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose()*data.f[i];
83  jmodel.calc_aba(jdata.derived(), Ia, parent > 0);
84 
85  if (parent > 0)
86  {
87  Force & pa = data.f[i];
88  pa.toVector() += Ia * data.a[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u);
89  data.Yaba[parent] += SE3actOn(data.liMi[i], Ia);
90  data.f[parent] += data.liMi[i].act(pa);
91  }
92 
93  // Data::Matrix6x & U = data.U_aba[i];
94  // Eigen::MatrixXd & D_inv = data.D_inv_aba[i];
95  //
97  //
98  // U = data.Yaba[i] * ((ConstraintXd)jdata.S()).matrix();
100  // D_inv = (jdata.S().transpose() * U.block(0,0,U.rows(), U.cols())).inverse();
102  // jmodel.jointVelocitySelector(data.tau) -= jdata.S().transpose()*data.f[i];
103  // if(parent>0)
104  // {
105  // Inertia::Matrix6 & Ia = data.Yaba[i];
106  // Force & pa = data.f[i];
107  //
108  // Ia -= U * D_inv * U.transpose();
109  //
110  // pa.toVector() += Ia * data.a[i].toVector() + U * D_inv * jmodel.jointVelocitySelector(data.tau);
113  // data.Yaba[parent] += SE3actOn(data.liMi[i], Ia);
114  // data.f[parent] += data.liMi[i].act(pa);
115  // }
116  }
117 
118  inline static Inertia::Matrix6 SE3actOn(const SE3 & M, const Inertia::Matrix6 & I)
119  {
120  typedef Inertia::Matrix6 Matrix6;
121  typedef SE3::Matrix3 Matrix3;
122  typedef SE3::Vector3 Vector3;
123  typedef Eigen::Block<const Matrix6,3,3> constBlock3;
124  typedef Eigen::Block<Matrix6,3,3> Block3;
125 
126  const constBlock3 & Ai = I.block<3,3> (Inertia::LINEAR, Inertia::LINEAR);
127  const constBlock3 & Bi = I.block<3,3> (Inertia::LINEAR, Inertia::ANGULAR);
128  const constBlock3 & Di = I.block<3,3> (Inertia::ANGULAR, Inertia::ANGULAR);
129 
130  const Matrix3 & R = M.rotation();
131  const Vector3 & t = M.translation();
132 
133  Matrix6 res;
134  Block3 Ao = res.block<3,3> (Inertia::LINEAR, Inertia::LINEAR);
135  Block3 Bo = res.block<3,3> (Inertia::LINEAR, Inertia::ANGULAR);
136  Block3 Co = res.block<3,3> (Inertia::ANGULAR, Inertia::LINEAR);
137  Block3 Do = res.block<3,3> (Inertia::ANGULAR, Inertia::ANGULAR);
138 
139  Do.noalias() = R*Ai; // tmp variable
140  Ao.noalias() = Do*R.transpose();
141 
142  Do.noalias() = R*Bi; // tmp variable
143  Bo.noalias() = Do*R.transpose();
144 
145  Co.noalias() = R*Di; // tmp variable
146  Do.noalias() = Co*R.transpose();
147 
148  Do.row(0) += t.cross(Bo.col(0));
149  Do.row(1) += t.cross(Bo.col(1));
150  Do.row(2) += t.cross(Bo.col(2));
151 
152  Co.col(0) = t.cross(Ao.col(0));
153  Co.col(1) = t.cross(Ao.col(1));
154  Co.col(2) = t.cross(Ao.col(2));
155  Co += Bo.transpose();
156 
157  Bo = Co.transpose();
158  Do.col(0) += t.cross(Bo.col(0));
159  Do.col(1) += t.cross(Bo.col(1));
160  Do.col(2) += t.cross(Bo.col(2));
161  return res;
162  }
163  };
164 
165  struct AbaForwardStep2 : public fusion::JointVisitor<AbaForwardStep2>
166  {
167  typedef boost::fusion::vector<const se3::Model &,
168  se3::Data &
169  > ArgsType;
170 
171  JOINT_VISITOR_INIT(AbaForwardStep2);
172 
173  template<typename JointModel>
174  static void algo(const se3::JointModelBase<JointModel> & jmodel,
175  se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
176  const se3::Model & model,
177  se3::Data & data)
178  {
179  const Model::JointIndex & i = jmodel.id();
180  const Model::Index & parent = model.parents[i];
181 
182  data.a[i] += data.liMi[i].actInv(data.a[parent]);
183  jmodel.jointVelocitySelector(data.ddq).noalias() =
184  jdata.Dinv() * jmodel.jointVelocitySelector(data.u) - jdata.UDinv().transpose() * data.a[i].toVector();
185 
186  data.a[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq);
187  }
188 
189  };
190 
191  inline const Eigen::VectorXd &
192  aba(const Model & model,
193  Data & data,
194  const Eigen::VectorXd & q,
195  const Eigen::VectorXd & v,
196  const Eigen::VectorXd & tau)
197  {
198  assert(model.check(data) && "data is not consistent with model.");
199 
200  data.v[0].setZero();
201  data.a[0] = -model.gravity;
202  data.u = tau;
203 
204  for(Model::Index i=1;i<(Model::Index)model.njoints;++i)
205  {
206  AbaForwardStep1::run(model.joints[i],data.joints[i],
207  AbaForwardStep1::ArgsType(model,data,q,v));
208  }
209 
210  for( Model::Index i=(Model::Index)model.njoints-1;i>0;--i )
211  {
212  AbaBackwardStep::run(model.joints[i],data.joints[i],
213  AbaBackwardStep::ArgsType(model,data));
214  }
215 
216  for(Model::Index i=1;i<(Model::Index)model.njoints;++i)
217  {
218  AbaForwardStep2::run(model.joints[i],data.joints[i],
219  AbaForwardStep2::ArgsType(model,data));
220  }
221 
222  return data.ddq;
223  }
224 
225  inline const Eigen::VectorXd &
226  aba(const Model & model,
227  Data & data,
228  const Eigen::VectorXd & q,
229  const Eigen::VectorXd & v,
230  const Eigen::VectorXd & tau,
231  const container::aligned_vector<Force> & fext)
232 
233  {
234  assert(model.check(data) && "data is not consistent with model.");
235 
236  data.v[0].setZero();
237  data.a[0] = -model.gravity;
238  data.u = tau;
239 
240  for(Model::Index i=1;i<(Model::Index)model.njoints;++i)
241  {
242  AbaForwardStep1::run(model.joints[i],data.joints[i],
243  AbaForwardStep1::ArgsType(model,data,q,v));
244  data.f[i] -= fext[i];
245  }
246 
247  for( Model::Index i=(Model::Index)model.njoints-1;i>0;--i )
248  {
249  AbaBackwardStep::run(model.joints[i],data.joints[i],
250  AbaBackwardStep::ArgsType(model,data));
251  }
252 
253  for(Model::Index i=1;i<(Model::Index)model.njoints;++i)
254  {
255  AbaForwardStep2::run(model.joints[i],data.joints[i],
256  AbaForwardStep2::ArgsType(model,data));
257  }
258 
259  return data.ddq;
260  }
261 
262  struct computeMinverseForwardStep1 : public fusion::JointVisitor<computeMinverseForwardStep1>
263  {
264  typedef boost::fusion::vector<const se3::Model &,
265  se3::Data &,
266  const Eigen::VectorXd &
267  > ArgsType;
268 
269  JOINT_VISITOR_INIT(computeMinverseForwardStep1);
270 
271  template<typename JointModel>
272  static void algo(const se3::JointModelBase<JointModel> & jmodel,
273  se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
274  const se3::Model & model,
275  se3::Data & data,
276  const Eigen::VectorXd & q)
277  {
278  const Model::JointIndex & i = jmodel.id();
279  jmodel.calc(jdata.derived(),q);
280 
281  const Model::Index & parent = model.parents[i];
282  data.liMi[i] = model.jointPlacements[i] * jdata.M();
283 
284  if (parent>0)
285  data.oMi[i] = data.oMi[parent] * data.liMi[i];
286  else
287  data.oMi[i] = data.liMi[i];
288 
289  typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
290  ColsBlock J_cols = jmodel.jointCols(data.J);
291  J_cols = data.oMi[i].act(jdata.S());
292 
293  data.Yaba[i] = model.inertias[i].matrix();
294  }
295 
296  };
297 
298  struct computeMinverseBackwardStep : public fusion::JointVisitor<computeMinverseBackwardStep>
299  {
300  typedef boost::fusion::vector<const Model &,
301  Data &> ArgsType;
302 
303  JOINT_VISITOR_INIT(computeMinverseBackwardStep);
304 
305  template<typename JointModel>
306  static void algo(const JointModelBase<JointModel> & jmodel,
307  JointDataBase<typename JointModel::JointDataDerived> & jdata,
308  const Model & model,
309  Data & data)
310  {
311  const Model::JointIndex & i = jmodel.id();
312  const Model::Index & parent = model.parents[i];
313  Inertia::Matrix6 & Ia = data.Yaba[i];
314  Data::RowMatrixXd & Minv = data.Minv;
315  Data::Matrix6x & Fcrb = data.Fcrb[0];
316  Data::Matrix6x & FcrbTmp = data.Fcrb.back();
317 
318  jmodel.calc_aba(jdata.derived(), Ia, parent > 0);
319 
320  typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
321 
322  ColsBlock U_cols = jmodel.jointCols(data.IS);
323  forceSet::se3Action(data.oMi[i],jdata.U(),U_cols); // expressed in the world frame
324 
325  Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),jmodel.nv()) = jdata.Dinv();
326  const int nv_children = data.nvSubtree[i] - jmodel.nv();
327  if(nv_children > 0)
328  {
329  ColsBlock J_cols = jmodel.jointCols(data.J);
330  ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv);
331  SDinv_cols.noalias() = J_cols * jdata.Dinv();
332 
333  Minv.block(jmodel.idx_v(),jmodel.idx_v()+jmodel.nv(),jmodel.nv(),nv_children).noalias()
334  = -SDinv_cols.transpose() * Fcrb.middleCols(jmodel.idx_v()+jmodel.nv(),nv_children);
335 
336  if(parent > 0)
337  {
338  FcrbTmp.leftCols(data.nvSubtree[i]).noalias()
339  = U_cols * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]);
340  Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]) += FcrbTmp.leftCols(data.nvSubtree[i]);
341  }
342  }
343  else
344  {
345  Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]).noalias()
346  = U_cols * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]);
347  }
348 
349  if(parent > 0)
350  data.Yaba[parent] += AbaBackwardStep::SE3actOn(data.liMi[i], Ia);
351  }
352  };
353 
354  struct computeMinverseForwardStep2 : public fusion::JointVisitor<computeMinverseForwardStep2>
355  {
356  typedef boost::fusion::vector<const se3::Model &,
357  se3::Data &
358  > ArgsType;
359 
360  JOINT_VISITOR_INIT(computeMinverseForwardStep2);
361 
362  template<typename JointModel>
363  static void algo(const se3::JointModelBase<JointModel> & jmodel,
364  se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
365  const se3::Model & model,
366  se3::Data & data)
367  {
368  const Model::JointIndex & i = jmodel.id();
369  const Model::Index & parent = model.parents[i];
370  Data::RowMatrixXd & Minv = data.Minv;
371  Data::Matrix6x & FcrbTmp = data.Fcrb.back();
372 
373  typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
374  ColsBlock UDinv_cols = jmodel.jointCols(data.UDinv);
375  forceSet::se3Action(data.oMi[i],jdata.UDinv(),UDinv_cols); // expressed in the world frame
376  ColsBlock J_cols = jmodel.jointCols(data.J);
377 
378  if(parent > 0)
379  {
380  FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v()).noalias()
381  = UDinv_cols.transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v());
382  Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v())
383  -= FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v());
384  }
385 
386  data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()).noalias() = J_cols * Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v());
387  if(parent > 0)
388  data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) += data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v());
389  }
390 
391  };
392 
393  inline const Data::RowMatrixXd &
394  computeMinverse(const Model & model,
395  Data & data,
396  const Eigen::VectorXd & q)
397  {
398  assert(model.check(data) && "data is not consistent with model.");
399 
400  for(Model::Index i=1;i<(Model::Index)model.njoints;++i)
401  {
402  computeMinverseForwardStep1::run(model.joints[i],data.joints[i],
403  computeMinverseForwardStep1::ArgsType(model,data,q));
404  }
405 
406  data.Fcrb[0].setZero();
407  for( Model::Index i=(Model::Index)model.njoints-1;i>0;--i )
408  {
409  computeMinverseBackwardStep::run(model.joints[i],data.joints[i],
410  computeMinverseBackwardStep::ArgsType(model,data));
411  }
412 
413  for(Model::Index i=1;i<(Model::Index)model.njoints;++i)
414  {
415  computeMinverseForwardStep2::run(model.joints[i],data.joints[i],
416  computeMinverseForwardStep2::ArgsType(model,data));
417  }
418 
419  return data.Minv;
420  }
421 
422 
423  // --- CHECKER ---------------------------------------------------------------
424  // --- CHECKER ---------------------------------------------------------------
425  // --- CHECKER ---------------------------------------------------------------
426 
427  // Check whether all masses are nonzero and diagonal of inertia is nonzero
428  // The second test is overconstraining.
429  inline bool ABAChecker::checkModel_impl( const Model& model ) const
430  {
431  for(JointIndex j=1;int(j)<model.njoints;j++)
432  if( (model.inertias[j].mass () < 1e-5)
433  || (model.inertias[j].inertia().data()[0] < 1e-5)
434  || (model.inertias[j].inertia().data()[3] < 1e-5)
435  || (model.inertias[j].inertia().data()[5] < 1e-5) )
436  return false;
437  return true;
438  }
439 
440 } // namespace se3
441 
443 
444 #endif // ifndef __se3_aba_hxx__
se3::Data::a
container::aligned_vector< Motion > a
Vector of joint accelerations expressed at the centers of the joints.
Definition: data.hpp:58
se3::Model::nv
int nv
Dimension of the velocity vector space.
Definition: model.hpp:52
se3::Model::jointPlacements
container::aligned_vector< SE3 > jointPlacements
Placement (SE3) of the input of joint i regarding to the parent joint output li.
Definition: model.hpp:67
se3::Data::nvSubtree
std::vector< int > nvSubtree
Dimension of the subtree motion space (for CRBA)
Definition: data.hpp:195
se3::Data::u
Eigen::VectorXd u
Intermediate quantity corresponding to apparent torque [ABA].
Definition: data.hpp:168
se3::Data::Matrix6x
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6x
The 6d jacobian type (temporary)
Definition: data.hpp:45
se3::Model::joints
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
Definition: model.hpp:70
se3::aba
const Eigen::VectorXd & aba(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &tau)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
Definition: aba.hxx:192
se3::Data::J
Matrix6x J
Jacobian of joint placements.
Definition: data.hpp:217
se3::Data::ddq
Eigen::VectorXd ddq
The joint accelerations computed from ABA.
Definition: data.hpp:161
se3::Model::parents
std::vector< JointIndex > parents
Joint parent of joint i, denoted li (li==parents[i]).
Definition: model.hpp:73
se3::Data::UDinv
Matrix6x UDinv
Used in computeMinverse.
Definition: data.hpp:137
se3::Data::SDinv
Matrix6x SDinv
Used in computeMinverse.
Definition: data.hpp:134
se3::Data::Yaba
container::aligned_vector< Inertia::Matrix6 > Yaba
Inertia matrix of the subtree expressed as dense matrix [ABA].
Definition: data.hpp:165
se3::Data::Fcrb
container::aligned_vector< Matrix6x > Fcrb
Spatial forces set, used in CRBA and CCRBA.
Definition: data.hpp:190
se3::Data::IS
Matrix6x IS
Used in computeMinverse.
Definition: data.hpp:140
se3::Data::joints
JointDataVector joints
Vector of se3::JointData associated to the se3::JointModel stored in model, encapsulated in JointData...
Definition: data.hpp:55
se3::Model::gravity
Motion gravity
Spatial gravity of the model.
Definition: model.hpp:106
se3::Data::liMi
container::aligned_vector< SE3 > liMi
Vector of relative joint placements (wrt the body parent).
Definition: data.hpp:90
se3::Data::oMi
container::aligned_vector< SE3 > oMi
Vector of absolute joint placements (wrt the world).
Definition: data.hpp:87
se3::Model::check
bool check(const AlgorithmCheckerBase< D > &checker=AlgorithmCheckerBase< D >()) const
Definition: model.hpp:331
se3::Model::inertias
container::aligned_vector< Inertia > inertias
Spatial inertias of the body i expressed in the supporting joint frame i.
Definition: model.hpp:64
se3::Data::Minv
RowMatrixXd Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Definition: data.hpp:119
se3::Data::f
container::aligned_vector< Force > f
Vector of body forces expressed in the local frame of the joint. For each body, the force represents ...
Definition: data.hpp:74
se3::container::aligned_vector
Specialization of an std::vector with an aligned allocator. This specialization might be used when th...
Definition: aligned-vector.hpp:34
se3::ForceBase::toVector
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
Definition: force-base.hpp:99
se3::Data::v
container::aligned_vector< Motion > v
Vector of joint velocities expressed at the centers of the joints.
Definition: data.hpp:67
se3::computeMinverse
const Data::RowMatrixXd & computeMinverse(const Model &model, Data &data, const Eigen::VectorXd &q)
Computes the inverse of the joint space inertia matrix using Articulated Body formulation.
Definition: aba.hxx:394
se3::Model::njoints
int njoints
Number of joints.
Definition: model.hpp:55