Commit cbc7c1b3 authored by jcarpent's avatar jcarpent
Browse files

[C++] Add iF in CRBA

parent 74c0f892
......@@ -111,17 +111,17 @@ namespace se3
const Model::Index & parent = model.parents[i];
if(parent>0)
{
/* Yli += liXi Yi */
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
/* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
Eigen::Block<typename Data::Matrix6x> jF
= data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
forceSet::se3Action(data.liMi[i],
data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]),
jF);
}
{
/* Yli += liXi Yi */
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
/* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
Eigen::Block<typename Data::Matrix6x> jF
= data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
Eigen::Block<typename Data::Matrix6x> iF
= data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
forceSet::se3Action(data.liMi[i], iF, jF);
}
// std::cout << "iYi = " << (Inertia::Matrix6)data.Ycrb[i] << std::endl;
// std::cout << "iSi = " << ConstraintXd(jdata.S()).matrix() << std::endl;
......
......@@ -129,9 +129,9 @@ namespace se3
/* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
Eigen::Block<typename Data::Matrix6x> jF
= data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
forceSet::se3Action(data.liMi[i],
data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]),
jF);
Eigen::Block<typename Data::Matrix6x> iF
= data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
forceSet::se3Action(data.liMi[i], iF, jF);
data.f[parent] += data.liMi[i].act(data.f[i]);
}
......
Supports Markdown
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