void Jacobian::ComputeJacobian(const Tree& tree)
{
Invalidate();
jacobian_ = MatrixX(3,tree.GetNumJoint()-1); // à cause de son incrémentation débile
// Traverse this to find all end effectors
Vector3 temp;
Joint* n = tree.GetRoot();
while (n) {
if (n->IsEffector())
{
int i = n->GetEffectorNum();
// Find all ancestors (they will usually all be joints)
// Set the corresponding entries in the Jacobian J
Joint* m = tree.GetParent(n);
while (m) {
int j = m->GetJointNum();
assert (0 <=i && i<tree.GetNumEffector() && 0<=j && j<tree.GetNumJoint());
temp = m->GetS(); // joint pos.
temp -= n->GetS(); // -(end effector pos. - joint pos.)
Vector3 tmp2 = temp.cross(m->GetW()); // cross product with joint rotation axis
jacobian_.col(j-1) = tmp2;
m = tree.GetParent(m);
}
}
n = (n->IsEffector() ? 0 : tree.GetSuccessor(n));
}
}
Sample::Sample(Tree& tree)
{
Joint * j = tree.GetRoot();
while(j)
{
angles_.push_back(j->GetTheta());
position_ = j->GetS();
j = j->pChild_;
}
position_ -= tree.GetPosition();
jacobianProd_ = tree.GetJacobian()->GetJacobianProduct();
jacobianProdInverse_ = tree.GetJacobian()->GetJacobianProductInverse();
}