/*! One possible formulation of the core GFO problem. Checks if some
combination of joint forces exists so that the resultant object
wrench is 0. See inner comments for exact mathematical formulation.
Not for standalone use; is called by the GFO functions in the
Grasp class.
*/
int
graspForceExistence(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 [beta tau]T*[G 0]T*[G 0]*[beta tau] (magnitude of resultant object wrench)
// subject to:
// [JTD -I] * [beta tau] = 0 (contact forces balance joint forces)
// [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|
// | 0 sum| | tau | = |1|
int numJoints = tau.rows();
Matrix beta_tau(beta.rows() + tau.rows(), 1);
//right-hand side of equality constraint
Matrix right_hand( JTD.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.0e8;
//left-hand side of equality constraint
Matrix LeftHand( JTD.rows() + 1, D.cols() + numJoints);
LeftHand.setAllElements(0.0);
LeftHand.copySubMatrix(0, 0, JTD);
LeftHand.copySubMatrix(0, D.cols(), Matrix::NEGEYE(numJoints, numJoints) );
for (int i=0; i<numJoints; i++) {
LeftHand.elem( JTD.rows(), D.cols() + i) = 1.0;
}
//matrix F padded with zeroes for tau
//left hand side of the inequality matrix
Matrix FO(F.rows(), F.cols() + numJoints);
FO.setAllElements(0.0);
FO.copySubMatrix(0, 0, F);
//right-hand side of inequality matrix
Matrix inEqZeroes(FO.rows(), 1);
inEqZeroes.setAllElements(0.0);
//objective matrix: G padded with zeroes
Matrix GO(Matrix::ZEROES<Matrix>(G.rows(), G.cols() + numJoints));
GO.copySubMatrix(0, 0, G);
//bounds: all variables greater than 0
// CHANGE: only beta >= 0, tau is not
Matrix lowerBounds(Matrix::MIN_VECTOR(beta_tau.rows()));
lowerBounds.copySubMatrix( 0, 0, Matrix::ZEROES<Matrix>(beta.rows(), 1) );
Matrix upperBounds(Matrix::MAX_VECTOR(beta_tau.rows()));
/*
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");
GO.print(fp);
fclose(fp);
*/
// assembled system:
// minimize beta_tauT*QOT*QO*beta_tau subject to:
// LeftHand * beta_tau = right_hand
// FO * beta_tau <= inEqZeroes
// beta_tau >= 0
// CHANGE: only beta >= 0, tau is not
int result = factorizedQPSolver(GO, 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;
}
//---------------------------------------------------------------------------
// コンストラクタ
//---------------------------------------------------------------------------
void GeometryInit(void)
{
InitMatrix();
//RightHand();
LeftHand();
}
/*! 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 &Q,
Matrix &beta, Matrix &tau, double *objVal, Matrix * objectWrench = NULL)
{
// 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)
// [Q 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|
// | Q 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() + Q.rows() + 1, 1);
right_hand.setAllElements(0.0);
//actually, we use 1.0e7 here as units are in N * 1.0e-6 * mm
//so we want a total joint torque of 10 N mm
right_hand.elem( right_hand.rows()-1, 0) = 1.0e7;
if(objectWrench)
for (int i = 0; i < 6; ++i)
right_hand.elem(JTD.rows() + i,0) = objectWrench->elem(i,0);
std::cout << "QRows " << Q.rows() <<" QCols " << Q.cols() << std::endl;
//left-hand side of equality constraint
Matrix LeftHand( JTD.rows() + Q.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, Q);
for (int i=0; i<numJoints; i++) {
LeftHand.elem( JTD.rows() + Q.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;
}
