/*! One possible formulation of the core GFO problem. Finds the contacts
forces that result in 0 object wrench and are as far as possible
from the edges of the friction cones. Assumes that at least one set
of contact forces that satisfy this criterion exist; see
contactForceExistence for that problem. See inner comments for exact
mathematical formulation. Not for standalone use; is called by the GFO
functions in the Grasp class.
*/
int
contactForceOptimization(Matrix &F, Matrix &N, Matrix &Q, Matrix &beta, double *objVal, Matrix * objectWrench = NULL)
{
// exact problem formulation
// unknowns: beta (contact forces)
// minimize sum * F * beta (crude measure of friction resistance abilities)
// subject to:
// Q * beta = 0 (0 resultant object wrench)
// sum_normal * beta = 1 (we are applying some contact forces)
// F * beta <= 0 (each individual forces inside friction cones)
// beta >= 0 (all forces must be positive)
// overall equality constraint:
// | Q | |beta| |0|
// | N | = |1|
//right hand of equality
Matrix right_hand = Matrix::ZEROES<Matrix>(Q.rows()+1, 1);
if(objectWrench)
{
assert(objectWrench->rows() == Q.rows());
for(int i = 0; i < Q.rows(); ++i)
right_hand.elem(i,0) = objectWrench->elem(i,0);
}
//a total of 10N of normal force
right_hand.elem(Q.rows(),0) = 1.0e7;
//left hand of equality
Matrix LeftHand(Q.rows() + 1, Q.cols());
LeftHand.copySubMatrix(0, 0, Q);
LeftHand.copySubMatrix(Q.rows(), 0, N);
//bounds: all variables greater than 0
Matrix lowerBounds(Matrix::ZEROES<Matrix>(beta.rows(),1));
Matrix upperBounds(Matrix::MAX_VECTOR(beta.rows()));
//objective: sum of F
Matrix FSum(1,F.rows());
FSum.setAllElements(1.0);
Matrix FObj(1,F.cols());
matrixMultiply(FSum, F, FObj);
/*
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"Left Hand:\n");
LeftHand.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
F.print(fp);
fprintf(fp,"Objective:\n");
Q.print(fp);
fclose(fp);
*/
int result = LPSolver(FObj, LeftHand, right_hand, F, Matrix::ZEROES<Matrix>(F.rows(), 1),
lowerBounds, upperBounds,
beta, objVal);
return result;
}
/*! One possible formulation of the core GFO problem. Finds the joint
forces that result in 0 object wrench such that contact forces are as
far as possible from the edges of the friction cones. Assumes that at
least one set of contact forces that satisfy this criterion exist; see
contactForceExistence for that problem. See inner comments for exact
mathematical formulation. Not for standalone use; is called by the GFO
functions in the Grasp class.
*/
int
graspForceOptimization(Matrix &JTD, Matrix &D, Matrix &F, Matrix &G,
Matrix &beta, Matrix &tau, double *objVal)
{
// exact problem formulation
// unknowns: [beta tau] (contact forces and joint forces)
// minimize [sum] [F 0] [beta tau] (sort of as far inside the friction cone as possible, not ideal)
// subject to:
// [JTD -I] * [beta tau] = 0 (contact forces balance joint forces)
// [G 0]* [beta tau] = 0 (0 resultant object wrench)
// [0 sum] * [beta tau] = 1 (we are applying some joint forces)
// [F 0] [beta tau] <= 0 (all forces inside friction cones)
// [beta tau] >= 0 (all forces must be positive)
// overall equality constraint:
// | JTD -I | | beta | |0|
// | G 0 | | tau | = |0|
// | 0 sum| |1|
Matrix beta_tau(beta.rows() + tau.rows(), 1);
int numJoints = tau.rows();
//right-hand side of equality constraint
Matrix right_hand( JTD.rows() + G.rows() + 1, 1);
right_hand.setAllElements(0.0);
//actually, we use 1.0e8 here as units are in N * 1.0e-6 * mm
//so we want a total joint torque of 100 N mm
right_hand.elem( right_hand.rows()-1, 0) = 1.0e10;
//left-hand side of equality constraint
Matrix LeftHand( JTD.rows() + G.rows() + 1, D.cols() + numJoints);
LeftHand.setAllElements(0.0);
LeftHand.copySubMatrix(0, 0, JTD);
LeftHand.copySubMatrix(0, D.cols(), Matrix::NEGEYE(numJoints, numJoints) );
LeftHand.copySubMatrix(JTD.rows(), 0, G);
for (int i=0; i<numJoints; i++) {
LeftHand.elem( JTD.rows() + G.rows(), D.cols() + i) = 1.0;
}
//objective matrix
//matrix F padded with zeroes for tau
//will also serve as the left hand side of the inequality matrix
Matrix FO(F.rows(), F.cols() + numJoints);
FO.setAllElements(0.0);
FO.copySubMatrix(0, 0, F);
//summing matrix and objective matrix
Matrix FSum(1, F.rows());
FSum.setAllElements(1.0);
Matrix FObj(1, FO.cols());
matrixMultiply(FSum, FO, FObj);
//bounds: all variables greater than 0
Matrix lowerBounds(Matrix::ZEROES<Matrix>(beta_tau.rows(),1));
Matrix upperBounds(Matrix::MAX_VECTOR(beta_tau.rows()));
//right-hand side of inequality matrix
Matrix inEqZeroes(FO.rows(), 1);
inEqZeroes.setAllElements(0.0);
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"left hand:\n");
LeftHand.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
FO.print(fp);
fprintf(fp,"Objective:\n");
FObj.print(fp);
fclose(fp);
// assembled system:
// minimize FObj * beta_tau subject to:
// LeftHand * beta_tau = right_hand
// FO * beta_tau <= inEqZeroes
// beta_tau >= 0
int result = LPSolver(FObj, LeftHand, right_hand, FO, inEqZeroes,
lowerBounds, upperBounds,
beta_tau, objVal);
beta.copySubBlock(0, 0, beta.rows(), 1, beta_tau, 0, 0);
tau.copySubBlock(0, 0, tau.rows(), 1, beta_tau, beta.rows(), 0);
return result;
}
