/*! This function is the equivalent of the Grasp Force Optimization, but done with
the quasi-static formulation cast as a Quadratic Program.
It can perform four types of computation:
- contact force existence: are there contact forces that balance out on the object
- contact force optimization: what are the optimal contact forces (as far as possible
from the edges of the friction cones) that balance out on the object
- grasp force existence: are there joint forces which produce contact forces which
balance out on the object
- grasp force optimization: what are the optimal joint forces, producing contact
forces that are as far as possible from the edges of the friction cones and
balance out on the object.
See individual computation routines for more details.
There might exist cases of grasps that are reported as form-closed where grasp force
existence fails, as this function also asks that the contact forces that balance
the object must be possible to apply from actuated DOF's.
For now, this function displays the computed contact forces on the contacts,
rather than returning them. It also copies the computed joint forces in the
matrix \a robotTau which is assumed to be large enough for all the joints of
the robot and use the robot's numbering scheme.
Return codes: 0 is success, >0 means problem is unfeasible, no equilibrium forces
exist; <0 means error in computation
*/
int
Grasp::computeQuasistaticForcesAndTorques(Matrix *robotTau, int computation)
{
//use the pre-set list of contacts. This includes contacts on the palm, but
//not contacts with other objects or obstacles
std::list<Contact*> contacts;
contacts.insert(contacts.begin(),contactVec.begin(), contactVec.end());
//if there are no contacts we are done
if (contacts.empty()) return 0;
//get only the joints on chains that make contact;
std::list<Joint*> joints = getJointsOnContactChains();
//build the Jacobian and the other matrices that are needed.
//this is the same as in the equilibrium function above.
Matrix J(contactJacobian(joints, contacts));
Matrix D(Contact::frictionForceBlockMatrix(contacts));
Matrix F(Contact::frictionConstraintsBlockMatrix(contacts));
Matrix R(Contact::localToWorldWrenchBlockMatrix(contacts));
Matrix N(Contact::normalForceSumMatrix(contacts));
//grasp map that relates contact amplitudes to object wrench G = S*R*D
Matrix G(graspMapMatrix(R,D));
//matrix that relates contact forces to joint torques JTD = JTran * D
Matrix JTran(J.transposed());
Matrix JTD(JTran.rows(), D.cols());
matrixMultiply(JTran, D, JTD);
// vectors of unknowns
Matrix beta( D.cols(), 1);
Matrix tau( (int)joints.size(), 1);
double objVal;
/* This is the core computation. There are many ways of combining the
optimization criterion and the constraints. Four of them are presented
here, each encapsulated in its own helper function.
*/
int result;
switch(computation)
{
case GRASP_FORCE_EXISTENCE:
result = graspForceExistence(JTD, D, F, G, beta, tau, &objVal);
break;
case GRASP_FORCE_OPTIMIZATION:
result = graspForceOptimization(JTD, D, F, G, beta, tau, &objVal);
break;
case CONTACT_FORCE_EXISTENCE:
result = contactForceExistence(F, N, G, beta, &objVal);
matrixMultiply(JTD, beta, tau);
break;
case CONTACT_FORCE_OPTIMIZATION:
result = contactForceOptimization(F, N, G, beta, &objVal);
matrixMultiply(JTD, beta, tau);
break;
default:
DBGA("Unknown computation type requested");
return -1;
}
if (result) {
if( result > 0) {
DBGA("Grasp: problem unfeasible");
} else {
DBGA("Grasp: solver error");
}
return result;
}
DBGA("Optimization solved; objective: " << objVal);
DBGP("beta:\n" << beta);
DBGP("tau:\n" << tau);
DBGP("Joint forces sum: " << tau.elementSum());
Matrix Gbeta(G.rows(), beta.cols());
matrixMultiply(G, beta, Gbeta);
DBGP("Total object wrench:\n" << Gbeta);
//retrieve contact wrenches in local contact coordinate systems
Matrix cWrenches(D.rows(), 1);
matrixMultiply(D, beta, cWrenches);
DBGP("Contact forces:\n " << cWrenches);
//compute object wrenches relative to object origin and expressed in world coordinates
Matrix objectWrenches(R.rows(), cWrenches.cols());
matrixMultiply(R, cWrenches, objectWrenches);
DBGP("Object wrenches:\n" << objectWrenches);
//display them on the contacts and accumulate them on the object
displayContactWrenches(&contacts, cWrenches);
accumulateAndDisplayObjectWrenches(&contacts, objectWrenches);
//set the robot joint values for the return
std::list<Joint*>::iterator it;
int jc;
for(it=joints.begin(), jc=0; it!=joints.end(); it++,jc++) {
robotTau->elem( (*it)->getNum(), 0 ) = 1.0 * tau.elem(jc,0);
}
//sanity check: contact forces balance joint forces
//sanity check: resultant object wrench is 0
return 0;
}
/*! This function is the equivalent of the Grasp Force Optimization, but done with
the quasi-static formulation cast as a Quadratic Program.
It will attempt to compute both the feasible contact forces and joint torques
that produce a 0 resultant wrench on the object, and keep the contact forces
as far as possible from the boundaries of the friction cones. This implicitly
assumes that the grasp has form-closure, in other words, some combination of
legal contact forces exists that produces 0 wrench on the object.
There might exist cases of grasps that are reported as form-closed where this
function fails, as this function also asks that the contact forces that balance
the object must be possible to apply from actuated DOF's.
For now, this function displays the computed contact forces on the contacts,
rather than returning them. It also copies the computed joint forces in the
matrix \a robotTau which is assumed to be large enough for all the joints of
the robot and use the robot's numbering scheme.
Return codes: 0 is success, >0 means problem is unfeasible, no equilibrium forces
exist; <0 means error in computation
*/
int
Grasp::computeQuasistaticForcesAndTorques(Matrix *robotTau, Matrix * objWrench, double * objectiveValue)
{
//use the pre-set list of contacts. This includes contacts on the palm, but
//not contacts with other objects or obstacles
std::list<Contact*> contacts;
contacts.insert(contacts.begin(),contactVec.begin(), contactVec.end());
//if there are no contacts we are done
if (contacts.empty()) return 0;
//retrieve all the joints of the robot, but only if their chains have contacts on
//them. Joints on chains with no contact have a trivial 0 solution so they only
//make the problem larger for no good reason.
//we could go even further and only keep the joints that come *before* the contacts
//in the chain
std::list<Joint*> joints;
for (int c=0; c<hand->getNumChains(); c++) {
if (hand->getChain(c)->getNumContacts(object) != 0) {
std::list<Joint*> chainJoints = hand->getChain(c)->getJoints();
joints.insert(joints.end(), chainJoints.begin(), chainJoints.end());
}
}
//build the Jacobian and the other matrices that are needed.
//this is the same as in the equilibrium function above.
Matrix J(contactJacobian(joints, contacts));
Matrix D(Contact::frictionForceBlockMatrix(contacts));
Matrix F(Contact::frictionConstraintsBlockMatrix(contacts));
Matrix R(Contact::localToWorldWrenchBlockMatrix(contacts));
Matrix N(Contact::normalForceSumMatrix(contacts));
//grasp map that relates contact amplitudes to object wrench G = S*R*D
Matrix G(graspMapMatrix(R,D));
//matrix that relates contact forces to joint torques JTD = JTran * D
Matrix JTran(J.transposed());
Matrix JTD(JTran.rows(), D.cols());
matrixMultiply(JTran, D, JTD);
// vectors of unknowns
Matrix beta( D.cols(), 1);
Matrix tau( (int)joints.size(), 1);
double objVal;
/* This is the core computation. There are many ways of combining the
optimization criterion and the constraints. Four of them are presented
here, each encapsulated in its own helper function.
*/
//int result = graspForceExistence(JTD, D, F, G, beta, tau, &objVal);
//int result = graspForceOptimization(JTD, D, F, G, beta, tau, &objVal, objWrench);
//int result = contactForceExistence(F, N, G, beta, &objVal);
//if(result)
// return result;
//matrixMultiply(JTD, beta, tau);
int result = contactForceOptimization(F, N, G, beta, &objVal, objWrench);
matrixMultiply(JTD, beta, tau);
if (result) {
if( result > 0) {
DBGA("Grasp: problem unfeasible");
} else {
DBGA("Grasp: solver error");
}
return result;
}
DBGP("Optimization solved; objective: " << objVal);
DBGP("beta:\n" << beta);
DBGP("tau:\n" << tau);
DBGP("Joint forces sum: " << tau.elementSum());
Matrix Gbeta(G.rows(), beta.cols());
matrixMultiply(G, beta, Gbeta);
DBGP("Total object wrench:\n" << Gbeta);
//retrieve contact wrenches in local contact coordinate systems
Matrix cWrenches(D.rows(), 1);
matrixMultiply(D, beta, cWrenches);
DBGP("Contact forces:\n " << cWrenches);
//compute object wrenches relative to object origin and expressed in world coordinates
Matrix objectWrenches(R.rows(), cWrenches.cols());
matrixMultiply(R, cWrenches, objectWrenches);
DBGP("Object wrenches:\n" << objectWrenches);
//display them on the contacts and accumulate them on the object
displayContactWrenches(&contacts, cWrenches);
accumulateAndDisplayObjectWrenches(&contacts, objectWrenches);
//set the robot joint values for the return
std::list<Joint*>::iterator it;
int jc;
for(it=joints.begin(), jc=0; it!=joints.end(); it++,jc++) {
robotTau->elem( (*it)->getNum(), 0 ) = 1.0 * tau.elem(jc,0);
}
//sanity check: contact forces balance joint forces
//sanity check: resultant object wrench is 0
if (objectiveValue)
*objectiveValue = objVal;
return 0;
}
