/*! Computes the contact jacobian J. J relates joint angle motion to
contact motion. Its transpose, JT, relates contact forces to joint
forces.
The joints and the contacts must both be passed in. Their order in
the incoming vectors will determine their indices in the Jacobian.
This function will make sure that only the right joints affect each
contact: joints that come before the link in contact, on the same
chain.
The Jacobian is ALWAYS computed in the local coordinate system of each
contact. When multiplied by joint torques, it will thus yield contact
forces and torques in the local contact coordinate system of each contact.
It is easy to do computations in world coordinates too, but then it
is impossible to discards rows that correspond to directions that the
contact can not apply force or torque in.
*/
Matrix
Grasp::contactJacobian(const std::list<Joint*> &joints,
const std::list<Contact*> &contacts)
{
//compute the world locations of all the joints of the robot
//this is overkill, as we might not need all of them
std::vector< std::vector<transf> > jointTransf(hand->getNumChains());
for (int c=0; c<hand->getNumChains(); c++) {
jointTransf[c].resize(hand->getChain(c)->getNumJoints(), transf::IDENTITY);
hand->getChain(c)->getJointLocations(NULL, jointTransf[c]);
}
Matrix J( Matrix::ZEROES<Matrix>(int(contacts.size()) * 6, (int)joints.size()) );
std::list<Joint*>::const_iterator joint_it;
int joint_count = 0;
int numRows = 0;
for (joint_it=joints.begin(); joint_it!=joints.end(); joint_it++) {
std::list<Contact*>::const_iterator contact_it;
numRows = 0;
for(contact_it=contacts.begin(); contact_it!=contacts.end(); contact_it++, numRows+=6) {
if ((*contact_it)->getBody1()->getOwner() != hand) {
DBGA("Grasp jacobian: contact not on hand");
continue;
}
Link *link = static_cast<Link*>((*contact_it)->getBody1());
//check if the contact is on the same chain that this joint belongs too
if ( (*joint_it)->getChainNum() != link->getChainNum() ) {
continue;
}
KinematicChain *chain = hand->getChain( link->getChainNum() );
//check that the joint comes before the contact in the chain
int last_joint = chain->getLastJoint( link->getLinkNum() );
//remember that the index of a joint in a chain is different from
//the index of a joint in a robot
int jointNumInChain = (*joint_it)->getNum() - chain->getFirstJointNum();
assert(jointNumInChain >= 0);
if ( jointNumInChain > last_joint) continue;
//compute the individual jacobian
transf joint_tran = jointTransf.at(link->getChainNum()).at(jointNumInChain);
transf contact_tran = (*contact_it)->getContactFrame() * link->getTran();
//always get the individual jacobian in local coordinates
Matrix indJ(Joint::jacobian(*joint_it, joint_tran, contact_tran, false));
//place it at the correct spot in the global jacobian
J.copySubMatrix(numRows, joint_count, indJ);
}
joint_count++;
}
return J;
}
/*! The overall matrices B and a relate hand construction parameters and
tendon force to joint torques:
(B [l r d]^t + a) * f = tau
By definition, the matrix of unknowns is thus
[l0 l1 l2 l3 l4 l5 r d]^T
This takes into account the influence of all insertion points on *all*
joints that come before them (like a Jacobian for example).
The contribution of each individual insertion point on all joints is
computed in the reference system of the link that the insertion point
is on, and considered on all proximal joints. The contributions are
computed separately, in the \a assembleTorqueMatrices(...) function.
With the current assumptions, it turns out that only l_i influences
the torque at joint i, not any l_j with j > i. Therefore, the part of
the B matrix that corresponds to the l entries is diagonal.
*/
void
McGrip::getRoutingMatrices(Matrix **B, Matrix **a)
{
*B = new Matrix(Matrix::ZEROES<Matrix>(6, 8));
*a = new Matrix(Matrix::ZEROES<Matrix>(6, 1));
//compute the world locations of all the joints of the robot
//this is overkill, as we might not need all of them
std::vector< std::vector<transf> > jointTransf(getNumChains());
for (int c = 0; c < getNumChains(); c++) {
jointTransf[c].resize(getChain(c)->getNumJoints(), transf::IDENTITY);
getChain(c)->getJointLocations(NULL, jointTransf[c]);
}
assert(numChains == 2);
for (int c = 0; c < 2; c++) {
assert(getChain(c)->getNumJoints() == 3);
for (int j = 0; j < 3; j++) {
int refJointNum = c * 3 + j;
transf refTran = jointTransf.at(c).at(j);
for (int k = j; k < 3; k++) {
int thisJointNum = c * 3 + k;
int nextJointNum = -1;
if (k != 2) {
nextJointNum = c * 3 + k + 1;
}
//compute the transform from one joint to the other
transf thisTran = jointTransf.at(c).at(k);
//relative transform in thisJoint's coordinate system
transf relTran = thisTran.inverse() % refTran;
vec3 translation = relTran.translation();
//for this hand, the z component should always be 0
if (translation.z() > 1.0e-3) {
DBGA("Z translation in McGrip routing matrix");
}
double tx, ty;
//in the math, I have used a different convention than link
//axes in hand geometry.
tx = translation.y();
ty = translation.x();
//this obviously means we will need to also flip the sign of the result
//since we changed the "handed-ness" of the coordinate system
DBGP("Joint translation: " << tx << " " << ty);
//get the joint angles
double theta = getChain(c)->getJoint(k)->getVal() +
getChain(c)->getJoint(k)->getOffset();
double theta_n = 0.0;
if (k != 2) {
theta_n = getChain(c)->getJoint(k + 1)->getVal() +
getChain(c)->getJoint(k + 1)->getOffset();
}
//compute the contributions
//negate the translation, as we want the vector from refJoint to thisJoint
//but still in thisJoint's system
assembleTorqueMatrices(refJointNum, thisJointNum, nextJointNum,
-tx, -ty,
theta, theta_n,
**B, **a);
}
}
}
//flip the signs; in the math a positive torque is an extension
//in the model, a positive torque is flexion
(*B)->multiply(-1.0);
(*a)->multiply(-1.0);
DBGP("B matrix:\n" << **B);
DBGP("a vector:\n" << **a);
}
