Commit b1908a1b authored by jcarpent's avatar jcarpent
Browse files

[Algo] Correct jacobian of frames algo

parent 8c95e5c7
......@@ -129,20 +129,19 @@ namespace se3
const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
// Lever between the joint center and the frame center expressed in the global frame
const SE3::Vector3 lever(data.oMi[parent].rotation() * frame.placement.translation());
getJacobian<local_frame>(model, data, parent, J);
if(!local_frame)
getJacobian<local_frame>(model, data, parent, J);
if (!frame.placement.isIdentity())
// Lever between the joint center and the frame center expressed in the global frame
const SE3::Vector3 lever(data.oMi[parent].rotation() * frame.placement.translation());
for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
{
for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
{
if(!local_frame)
J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
else
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
}
if(!local_frame)
J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
else
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
}
}
......
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