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