Commit e93b3763 authored by jcarpent's avatar jcarpent
Browse files

[All] Change oYo to oYcrb

parent 85d1c51c
......@@ -292,8 +292,8 @@ namespace se3
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::Index & parent = model.parents[i];
const Inertia & Y = data.oYo[i];
const Inertia::Matrix6 & doYo = data.doYo[i];
const Inertia & Y = data.oYcrb[i];
const Inertia::Matrix6 & doYcrb = data.doYcrb[i];
ColsBlock J_cols = jmodel.jointCols(data.J);
J_cols = data.oMi[i].act(jdata.S());
......@@ -301,9 +301,9 @@ namespace se3
ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
motionSet::motionAction(data.ov[i],J_cols,dJ_cols);
data.oYo[parent] += Y;
data.oYcrb[parent] += Y;
if(parent > 0)
data.doYo[parent] += doYo;
data.doYcrb[parent] += doYcrb;
// Calc Ag
ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
......@@ -312,7 +312,7 @@ namespace se3
// Calc dAg = Ivx + vxI
ColsBlock dAg_cols = jmodel.jointCols(data.dAg);
rhsInertiaMult(Y,dJ_cols,dAg_cols);
dAg_cols += doYo * J_cols;
dAg_cols += doYcrb * J_cols;
}
template<typename Min, typename Mout>
......@@ -359,12 +359,12 @@ namespace se3
typedef Eigen::Block <Data::Matrix6x,3,-1> Block3x;
forwardKinematics(model,data,q,v);
data.oYo[0].setZero();
data.oYcrb[0].setZero();
for(Model::Index i=1;i<(Model::Index)(model.njoints);++i)
{
data.oYo[i] = data.oMi[i].act(model.inertias[i]);
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
data.ov[i] = data.oMi[i].act(data.v[i]); // v_i expressed in the world frame
data.doYo[i] = data.oYo[i].variation(data.ov[i]);
data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]);
}
for(Model::Index i=(Model::Index)(model.njoints-1);i>0;--i)
......@@ -372,7 +372,7 @@ namespace se3
DCcrbaBackwardStep::run(model.joints[i],data.joints[i],
DCcrbaBackwardStep::ArgsType(model,data));
}
data.com[0] = data.oYo[0].lever();
data.com[0] = data.oYcrb[0].lever();
const Block3x Ag_lin = data.Ag.middleRows<3> (Force::LINEAR);
Block3x Ag_ang = data.Ag.middleRows<3> (Force::ANGULAR);
......@@ -380,16 +380,16 @@ namespace se3
Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
data.hg = data.Ag*v;
data.vcom[0] = data.hg.linear()/data.oYo[0].mass();
data.vcom[0] = data.hg.linear()/data.oYcrb[0].mass();
const Block3x dAg_lin = data.dAg.middleRows<3>(Force::LINEAR);
Block3x dAg_ang = data.dAg.middleRows<3>(Force::ANGULAR);
for (long i = 0; i<model.nv; ++i)
dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]);
data.Ig.mass() = data.oYo[0].mass();
data.Ig.mass() = data.oYcrb[0].mass();
data.Ig.lever().setZero();
data.Ig.inertia() = data.oYo[0].inertia();
data.Ig.inertia() = data.oYcrb[0].inertia();
return data.dAg;
}
......
......@@ -53,7 +53,7 @@ namespace se3
else
data.oMi[i] = data.liMi[i];
data.oYo[i] = data.oMi[i].act(model.inertias[i]);
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
data.f[i].setZero();
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
......@@ -92,22 +92,22 @@ namespace se3
ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq);
motionSet::inertiaAction(data.oYo[i],dAdq_cols,dFdq_cols);
motionSet::inertiaAction(data.oYcrb[i],dAdq_cols,dFdq_cols);
gravity_partial_dq.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
= J_cols.transpose()*data.dFdq.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
data.f[i] = data.oYo[i] * oa;
data.f[i] = data.oYcrb[i] * oa;
motionSet::act<ADDTO>(J_cols,data.f[i],dFdq_cols);
lhsInertiaMult(data.oYo[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv()));
lhsInertiaMult(data.oYcrb[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv()));
for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
gravity_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdq.col(j);
jmodel.jointVelocitySelector(data.g).noalias() = J_cols.transpose()*data.f[i].toVector();
if(parent>0)
{
data.oYo[parent] += data.oYo[i];
data.oYcrb[parent] += data.oYcrb[i];
}
}
......@@ -191,12 +191,12 @@ namespace se3
data.a[i] += data.liMi[i].actInv(data.a[parent]);
}
data.oYo[i] = data.oMi[i].act(model.inertias[i]);
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
ov = data.oMi[i].act(data.v[i]);
oa = data.oMi[i].act(data.a[i]) + data.oa[0]; // add gravity contribution
data.h[i] = data.oYo[i] * ov;
data.f[i] = data.oYo[i] * oa + ov.cross(data.h[i]);
data.h[i] = data.oYcrb[i] * ov;
data.f[i] = data.oYcrb[i] * oa + ov.cross(data.h[i]);
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
ColsBlock J_cols = jmodel.jointCols(data.J);
......@@ -219,9 +219,9 @@ namespace se3
dVdq_cols.setZero();
// computes variation of inertias
data.doYo[i] = data.oYo[i].variation(ov);
data.doYcrb[i] = data.oYcrb[i].variation(ov);
addForceCrossMatrix(data.h[i],data.doYo[i]);
addForceCrossMatrix(data.h[i],data.doYcrb[i]);
}
template<typename ForceDerived, typename M6>
......@@ -274,21 +274,21 @@ namespace se3
jmodel.jointVelocitySelector(data.tau).noalias() = J_cols.transpose()*data.f[i].toVector();
// dtau/da similar to data.M
motionSet::inertiaAction(data.oYo[i],J_cols,dFda_cols);
motionSet::inertiaAction(data.oYcrb[i],J_cols,dFda_cols);
rnea_partial_da.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
= J_cols.transpose()*data.dFda.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
// dtau/dv
motionSet::inertiaAction(data.oYo[i],dAdv_cols,dFdv_cols);
dFdv_cols += data.doYo[i] * J_cols;
motionSet::inertiaAction(data.oYcrb[i],dAdv_cols,dFdv_cols);
dFdv_cols += data.doYcrb[i] * J_cols;
rnea_partial_dv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
= J_cols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
// dtau/dq
motionSet::inertiaAction(data.oYo[i],dAdq_cols,dFdq_cols);
motionSet::inertiaAction(data.oYcrb[i],dAdq_cols,dFdq_cols);
if(parent>0)
dFdq_cols += data.doYo[i] * dVdq_cols;
dFdq_cols += data.doYcrb[i] * dVdq_cols;
rnea_partial_dq.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
= J_cols.transpose()*data.dFdq.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
......@@ -297,13 +297,13 @@ namespace se3
if(parent > 0)
{
lhsInertiaMult(data.oYo[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv()));
lhsInertiaMult(data.oYcrb[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv()));
for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
rnea_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdq.col(j);
for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
rnea_partial_dv.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdv.col(j);
M6tmpR.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.doYo[i];
M6tmpR.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.doYcrb[i];
for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
rnea_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += M6tmpR.topRows(jmodel.nv()) * data.dVdq.col(j);
for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
......@@ -312,8 +312,8 @@ namespace se3
if(parent>0)
{
data.oYo[parent] += data.oYo[i];
data.doYo[parent] += data.doYo[i];
data.oYcrb[parent] += data.oYcrb[i];
data.doYcrb[parent] += data.doYcrb[i];
data.f[parent] += data.f[i];
}
}
......
......@@ -325,7 +325,7 @@ namespace se3
else data.oMi[i] = data.liMi[i];
// express quantities in the world frame
data.oYo[i] = data.oMi[i].act(model.inertias[i]);
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
data.v[i] = jdata.v();
if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]);
......@@ -341,7 +341,7 @@ namespace se3
motionSet::motionAction(data.ov[i],Jcols,dJcols);
// computes vxI
Inertia::vxi(data.ov[i],data.oYo[i],data.vxI[i]);
Inertia::vxi(data.ov[i],data.oYcrb[i],data.vxI[i]);
}
};
......@@ -367,13 +367,13 @@ namespace se3
ColsBlock dJcols = jmodel.jointCols(data.dJ);
ColsBlock Jcols = jmodel.jointCols(data.J);
motionSet::inertiaAction(data.oYo[i],dJcols,jmodel.jointCols(data.dFdv));
motionSet::inertiaAction(data.oYcrb[i],dJcols,jmodel.jointCols(data.dFdv));
jmodel.jointCols(data.dFdv) += data.vxI[i] * Jcols;
data.C.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
= Jcols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
lhsInertiaMult(data.oYo[i],Jcols.transpose(),M6tmpR.topRows(jmodel.nv()));
lhsInertiaMult(data.oYcrb[i],Jcols.transpose(),M6tmpR.topRows(jmodel.nv()));
for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dJ.col(j);
......@@ -383,7 +383,7 @@ namespace se3
if(parent>0)
{
data.oYo[parent] += data.oYo[i];
data.oYcrb[parent] += data.oYcrb[i];
data.vxI[parent] += data.vxI[i];
}
......
......@@ -437,10 +437,10 @@ namespace se3
container::aligned_vector<Inertia::Matrix6> Ivx;
/// \brief Inertia quantities expressed in the world frame
container::aligned_vector<Inertia> oYo;
container::aligned_vector<Inertia> oYcrb;
/// \brief Time variation of the inertia quantities expressed in the world frame
container::aligned_vector<Inertia::Matrix6> doYo;
container::aligned_vector<Inertia::Matrix6> doYcrb;
/// \brief Temporary for derivative algorithms
Inertia::Matrix6 Itmp;
......
......@@ -256,8 +256,8 @@ namespace se3
,dFda(6,model.nv)
,vxI((std::size_t)model.njoints)
,Ivx((std::size_t)model.njoints)
,oYo((std::size_t)model.njoints)
,doYo((std::size_t)model.njoints)
,oYcrb((std::size_t)model.njoints)
,doYcrb((std::size_t)model.njoints)
,ddq(model.nv)
,Yaba((std::size_t)model.njoints)
,u(model.nv)
......
......@@ -210,7 +210,7 @@ BOOST_AUTO_TEST_CASE (test_ccrb)
dccrba(model,data,q,0*v);
BOOST_CHECK(data.dAg.isZero());
// Check that dYcrb is equal to doYo
// Check that dYcrb is equal to doYcrb
{
// Compute dYcrb
Data data_ref(model), data_ref_plus(model), data(model);
......@@ -229,7 +229,7 @@ BOOST_AUTO_TEST_CASE (test_ccrb)
{
Inertia::Matrix6 dYcrb = (data_ref_plus.oMi[i].act(data_ref_plus.Ycrb[i]).matrix() -
data_ref.oMi[i].act(data_ref.Ycrb[i]).matrix())/alpha;
BOOST_CHECK(data.doYo[i].isApprox(dYcrb,sqrt(alpha)));
BOOST_CHECK(data.doYcrb[i].isApprox(dYcrb,sqrt(alpha)));
}
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment