Commit 29b48170 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

subtreeJacobianCenterOfMass returns com of subtree in world frame.

* Clean code
parent 832cb7b9
......@@ -43,14 +43,12 @@ namespace hpp {
assert(computeCOM && "This does nothing");
assert (!(computeJac && !computeCOM)); // JACOBIAN => COM
if (computeJac) {
se3::jacobianCenterOfMass(model, data,
robot_->currentConfiguration(), true, false);
com_.setZero();
jacobianCom_.setZero ();
for (std::size_t i = 0; i < joints_.size(); ++i) {
se3::subtreeJacobianCenterOfMass(model, data, joints_[i]);
se3::JointIndex j = joints_[i].root;
com_ += data.mass[j] * data.oMi[j].act(data.com[j]);
com_ += data.mass[j] * data.com[j];
jacobianCom_ += data.mass[j] * data.Jcom;
}
com_ /= mass_;
......
......@@ -96,10 +96,10 @@ namespace se3
const SubtreeModel & stModel)
{
Model::JointIndex r = stModel.root;
// data.com[0].setZero ();
// data.mass[0] = 0;
assert (r == stModel.joints.back());
data.Jcom.setZero();
for(size_t j = 0; j < stModel.joints.size(); ++j)
for(std::size_t j = 0; j < stModel.joints.size(); ++j)
{
Model::JointIndex i = stModel.joints[j];
......@@ -111,7 +111,7 @@ namespace se3
}
// Backward step
for(size_t j = 0; j < stModel.joints.size(); ++j)
for(std::size_t j = 0; j < stModel.joints.size(); ++j)
{
Model::JointIndex i = stModel.joints[j];
SubtreeJacobianCenterOfMassBackwardStep
......@@ -119,7 +119,6 @@ namespace se3
SubtreeJacobianCenterOfMassBackwardStep::ArgsType(model,data, r));
}
// data.com[root] /= data.mass[root];
data.Jcom /= data.mass[r];
return data.Jcom;
......
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