Commit 1f913f34 authored by jcarpent's avatar jcarpent
Browse files

[Algo] Fix computation of potential energy

parent cf2babdb
......@@ -32,7 +32,7 @@ namespace se3
"Update kinematics (bool)"),
"Computes the kinematic energy of the model for the "
"given joint configuration and velocity and stores the result "
" in data.kinetic_energy. By default, the kinematics of model is updated.");
"in data.kinetic_energy. By default, the first order kinematic quantities of the model are updated.");
bp::def("potentialEnergy",potentialEnergy,
bp::args("Model","Data",
......@@ -40,7 +40,7 @@ namespace se3
"Update kinematics (bool)"),
"Computes the potential energy of the model for the "
"given the joint configuration and stores the result "
" in data.potential_energy. By default, the kinematics of model is updated.");
"in data.potential_energy. By default, the first order kinematic quantities of the model are updated.");
}
} // namespace python
......
......@@ -99,7 +99,7 @@ namespace se3
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i)
{
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);
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