Commit fc9a41f2 authored by jcarpent's avatar jcarpent
Browse files

[C++][Fix Bug] Correct potential geometry algorithm

parent bee8b412
......@@ -91,12 +91,16 @@ namespace se3
{
data.potential_energy = 0.;
const Motion::ConstLinear_t & g = model.gravity.linear();
SE3::Vector3 com_global;
if (update_kinematics)
forwardKinematics(model,data,q);
for(Model::Index i=1;i<(Model::Index)(model.nbody);++i)
data.potential_energy += model.inertias[i].mass() * data.oMi[i].translation().dot(g);
{
com_global = data.oMi[i].translation() + data.oMi[i].rotation() * model.inertias[i].lever();
data.potential_energy += model.inertias[i].mass() * com_global.dot(g);
}
return data.potential_energy;
}
......
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