Verified Commit 774ffbc1 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo/com: factorize code

parent 460cf61d
......@@ -233,37 +233,12 @@ namespace pinocchio
ColBlock Jcols = jmodel.jointCols(data.J);
Jcols = data.oMi[i].act(jdata.S());
if(JointModel::NV == Eigen::Dynamic)
for(Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id)
{
if(jmodel.nv() == 1)
{
data.Jcom.col(jmodel.idx_v())
= data.mass[i] * Jcols.template topLeftCorner<3,1>()
- data.com[i].cross(Jcols.template bottomLeftCorner<3,1>()) ;
}
else
{
jmodel.jointCols(data.Jcom)
= data.mass[i] * Jcols.template topRows<3>()
- skew(data.com[i]) * Jcols.template bottomRows<3>();
}
jmodel.jointCols(data.Jcom).col(col_id)
= data.mass[i] * Jcols.col(col_id).template segment<3>(Motion::LINEAR)
- data.com[i].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR)) ;
}
else
{
if(JointModel::NV == 1)
{
data.Jcom.col(jmodel.idx_v())
= data.mass[i] * Jcols.template topLeftCorner<3,1>()
- data.com[i].cross(Jcols.template bottomLeftCorner<3,1>()) ;
}
else
{
jmodel.jointCols(data.Jcom)
= data.mass[i] * Jcols.template topRows<3>()
- skew(data.com[i]) * Jcols.template bottomRows<3>();
}
}
if(computeSubtreeComs)
data.com[i] /= data.mass[i];
......
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