Exemple #1
0
/*! 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;
}
Exemple #2
0
//---------------------------------------------------------------------------
//	コンストラクタ
//---------------------------------------------------------------------------
void GeometryInit(void)
{
  InitMatrix();
  //RightHand();
  LeftHand();
}
Exemple #3
0
/*! 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;
}