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));
}
}
void Sample::LoadIntoTree(Tree& tree) const
{
Joint * j = tree.GetRoot();
assert(angles_.size() == tree.GetNumJoint());
{
for(Sample::LAngles::const_iterator it = angles_.begin(); it < angles_.end() && (j != 0); ++it)
{
j->SetTheta(*it);
j = j->pChild_;
}
tree.Compute();
}
}
