5 #ifndef __pinocchio_compute_all_terms_hpp__
6 #define __pinocchio_compute_all_terms_hpp__
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"
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);
52 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
59 typedef boost::fusion::vector<
const Model &,
61 const ConfigVectorType &,
62 const TangentVectorType &
65 template<
typename Jo
intModel>
70 const Eigen::MatrixBase<ConfigVectorType> & q,
71 const Eigen::MatrixBase<TangentVectorType> & v)
73 typedef typename Model::JointIndex JointIndex;
76 const JointIndex & i = jmodel.id();
77 const JointIndex & parent = model.
parents[i];
79 jmodel.calc(jdata.derived(),q.derived(),v.derived());
82 data.liMi[i] = model.jointPlacements[i]*jdata.M();
83 data.Ycrb[i] = model.inertias[i];
86 data.v[i] = jdata.v();
90 data.oMi[i] = data.oMi[parent]*data.liMi[i];
91 data.v[i] += data.liMi[i].actInv(data.v[parent]);
94 data.oMi[i] = data.liMi[i];
96 jmodel.jointCols(data.
J) = data.oMi[i].act(jdata.S());
98 data.a_gf[i] = data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
100 data.a[i] += data.liMi[i].actInv(data.a[parent]);
102 data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
104 data.f[i] = model.inertias[i]*data.a_gf[i] + model.inertias[i].vxiv(data.v[i]);
107 const Scalar & mass = model.inertias[i].mass();
108 const typename Inertia::Vector3 & lever = model.inertias[i].lever();
110 data.com[i].noalias() = mass * lever;
113 data.vcom[i].noalias() = mass * (data.v[i].angular().cross(lever) + data.v[i].linear());
118 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
125 typedef boost::fusion::vector<
const Model &,
129 template<
typename Jo
intModel>
142 typedef typename Model::JointIndex JointIndex;
145 const JointIndex & i = jmodel.id();
146 const JointIndex & parent = model.
parents[i];
147 const SE3 & oMi = data.oMi[i];
150 jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
153 data.
M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.
nvSubtree[i])
154 = jdata.S().transpose()*data.Fcrb[i].middleCols(jmodel.idx_v(),data.
nvSubtree[i]);
157 jmodel.jointVelocitySelector(data.
nle) = jdata.S().transpose()*data.f[i];
161 data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
164 Eigen::Block<typename Data::Matrix6x> jF
165 = data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.
nvSubtree[i]);
166 Eigen::Block<typename Data::Matrix6x> iF
167 = data.Fcrb[i].block(0,jmodel.idx_v(),6,data.
nvSubtree[i]);
170 data.f[parent] += data.liMi[i].act(data.f[i]);
174 const SE3 & liMi = data.liMi[i];
176 data.com[parent] += (liMi.rotation()*data.com[i]
177 + data.
mass[i] * liMi.translation());
179 typename SE3::Vector3 com_in_world(oMi.rotation() * data.com[i] + data.
mass[i] * oMi.translation());
181 data.vcom[parent] += liMi.rotation()*data.vcom[i];
187 ColBlock Jcols = jmodel.jointCols(data.
J);
189 if( JointModel::NV==1 )
190 data.
Jcom.col(jmodel.idx_v())
191 = data.
mass[i] * Jcols.template topLeftCorner<3,1>()
192 - com_in_world.cross(Jcols.template bottomLeftCorner<3,1>()) ;
194 jmodel.jointCols(data.
Jcom)
195 = data.
mass[i] * Jcols.template topRows<3>()
196 -
skew(com_in_world) * Jcols.template bottomRows<3>();
198 data.com[i] /= data.
mass[i];
199 data.vcom[i] /= data.
mass[i];
203 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
206 const Eigen::MatrixBase<ConfigVectorType> & q,
207 const Eigen::MatrixBase<TangentVectorType> & v)
209 assert(model.
check(data) &&
"data is not consistent with model.");
210 PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.
nq,
"The configuration vector is not of right size");
211 PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.
nv,
"The velocity vector is not of right size");
214 typedef typename Model::JointIndex JointIndex;
221 data.com[0].setZero();
222 data.vcom[0].setZero();
225 for(JointIndex i=1;i<(JointIndex) model.
njoints;++i)
228 typename Pass1::ArgsType(model,data,q.derived(),v.derived()));
232 for(JointIndex i=(JointIndex)(model.
njoints-1);i>0;--i)
235 typename Pass2::ArgsType(model,data));
239 data.com[0] /= data.
mass[0];
240 data.vcom[0] /= data.
mass[0];
254 #endif // ifndef __pinocchio_compute_all_terms_hpp__