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(); }