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);
51 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
53 :
public fusion::JointUnaryVisitorBase< CATForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> >
55 typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
56 typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
58 typedef boost::fusion::vector<
const Model &,
60 const ConfigVectorType &,
61 const TangentVectorType &
64 template<
typename Jo
intModel>
65 static void algo(
const JointModelBase<JointModel> & jmodel,
66 JointDataBase<typename JointModel::JointDataDerived> & jdata,
69 const Eigen::MatrixBase<ConfigVectorType> & q,
70 const Eigen::MatrixBase<TangentVectorType> & v)
72 typedef typename Model::JointIndex JointIndex;
73 typedef typename Data::Inertia Inertia;
75 const JointIndex & i = jmodel.id();
76 const JointIndex & parent = model.parents[i];
78 jmodel.calc(jdata.derived(),q.derived(),v.derived());
81 data.liMi[i] = model.jointPlacements[i]*jdata.M();
82 data.Ycrb[i] = model.inertias[i];
85 data.v[i] = jdata.v();
89 data.oMi[i] = data.oMi[parent]*data.liMi[i];
90 data.v[i] += data.liMi[i].actInv(data.v[parent]);
93 data.oMi[i] = data.liMi[i];
95 jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
97 data.a_gf[i] = data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
99 data.a[i] += data.liMi[i].actInv(data.a[parent]);
101 data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
103 data.f[i] = model.inertias[i]*data.a_gf[i] + model.inertias[i].vxiv(data.v[i]);
106 const Scalar & mass = model.inertias[i].mass();
107 const typename Inertia::Vector3 & lever = model.inertias[i].lever();
109 data.com[i].noalias() = mass * lever;
112 data.vcom[i].noalias() = mass * (data.v[i].angular().cross(lever) + data.v[i].linear());
117 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
118 struct CATBackwardStep
119 :
public fusion::JointUnaryVisitorBase <CATBackwardStep<Scalar,Options,JointCollectionTpl> >
121 typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
122 typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
124 typedef boost::fusion::vector<
const Model &,
128 template<
typename Jo
intModel>
129 static void algo(
const JointModelBase<JointModel> & jmodel,
130 JointDataBase<typename JointModel::JointDataDerived> & jdata,
141 typedef typename Model::JointIndex JointIndex;
142 typedef typename Data::SE3 SE3;
144 const JointIndex & i = jmodel.id();
145 const JointIndex & parent = model.parents[i];
146 const SE3 & oMi = data.oMi[i];
149 jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
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]);
156 jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose()*data.f[i];
160 data.Ycrb[parent] += data.liMi[i].
act(data.Ycrb[i]);
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]);
169 data.f[parent] += data.liMi[i].act(data.f[i]);
173 const SE3 & liMi = data.liMi[i];
175 data.com[parent] += (liMi.rotation()*data.com[i]
176 + data.mass[i] * liMi.translation());
178 typename SE3::Vector3 com_in_world(oMi.rotation() * data.com[i] + data.mass[i] * oMi.translation());
180 data.vcom[parent] += liMi.rotation()*data.vcom[i];
181 data.mass[parent] += data.mass[i];
184 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
186 ColBlock Jcols = jmodel.jointCols(data.J);
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>()) ;
193 jmodel.jointCols(data.Jcom)
194 = data.mass[i] * Jcols.template topRows<3>()
195 -
skew(com_in_world) * Jcols.template bottomRows<3>();
197 data.com[i] /= data.mass[i];
198 data.vcom[i] /= data.mass[i];
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)
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");
212 typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
213 typedef typename Model::JointIndex JointIndex;
217 data.a_gf[0] = -model.gravity;
220 data.com[0].setZero();
221 data.vcom[0].setZero();
223 typedef CATForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Pass1;
224 for(JointIndex i=1;i<(JointIndex) model.njoints;++i)
226 Pass1::run(model.joints[i],data.joints[i],
227 typename Pass1::ArgsType(model,data,q.derived(),v.derived()));
230 typedef CATBackwardStep<Scalar,Options,JointCollectionTpl> Pass2;
231 for(JointIndex i=(JointIndex)(model.njoints-1);i>0;--i)
233 Pass2::run(model.joints[i],data.joints[i],
234 typename Pass2::ArgsType(model,data));
238 data.com[0] /= data.mass[0];
239 data.vcom[0] /= data.mass[0];
242 data.Jcom /= data.mass[0];
252 #endif // ifndef __pinocchio_compute_all_terms_hpp__ 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...
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
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.
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...