/*! 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); }