/*! One possible version of the GFO problem.

	Given a matrix of joint torques applied to the robot joints, this will check
	if there exists a set of legal contact forces that balance them. If so, it
	will compute the set of contact forces that adds up to the wrench of least
	magnitude on the object.

	For now, this output of this function is to set the computed contact forces
	on each contact as dynamic forces, and also to accumulate the resulting
	wrench on the object in its external wrench accumulator.

	Return codes: 0 is success, >0 means finger slip, no legal contact forces 
	exist; <0 means error in computation 
*/
int 
Grasp::computeQuasistaticForces(const Matrix &robotTau)
{
	//WARNING: for now, this ignores contacts on the palm. That might change in the future

	//for now, if the hand is touching anything other then the object, abort
	for (int c=0; c<hand->getNumChains(); c++) {
		if ( hand->getChain(c)->getNumContacts(NULL) !=
			hand->getChain(c)->getNumContacts(object) ) {
				DBGA("Hand contacts not on object");
				return 1;
			}
	}

	std::list<Contact*> contacts;
	std::list<Joint*> joints;

	bool freeChainForces = false;
	for(int c=0; c<hand->getNumChains(); c++) {
		//for now, we look at all contacts
		std::list<Contact*> chainContacts = hand->getChain(c)->getContacts(object);
		contacts.insert(contacts.end(), chainContacts.begin(), chainContacts.end());
		if (!chainContacts.empty()) {
			std::list<Joint*> chainJoints = hand->getChain(c)->getJoints();
			joints.insert(joints.end(), chainJoints.begin(), chainJoints.end());
		} else {
			//the chain has no contacts
			//check if any joint forces are not 0
			Matrix chainTau = hand->getChain(c)->jointTorquesVector(robotTau);
			//torque units should be N * 1.0e6 * mm
			if (chainTau.absMax() > 1.0e3) {
				DBGA("Joint torque " << chainTau.absMax() << " on chain " << c 
									 << " with no contacts");
				freeChainForces = true;
			}
		}
	}
	//if there are no contacts, nothing to compute!
	if (contacts.empty()) return 0;

	//assemble the joint forces matrix
	Matrix tau((int)joints.size(), 1);
	int jc; std::list<Joint*>::iterator jit;
	for (jc=0, jit = joints.begin(); jit!=joints.end(); jc++,jit++) {
		tau.elem(jc,0) = robotTau.elem( (*jit)->getNum(), 0 );
	}
	//if all the joint forces we care about are zero, do an early exit 
	//as zero contact forces are guaranteed to balance the chain
	//we should probably be able to use a much larger threshold here, if
	//units are what I think they are
	if (tau.absMax() < 1.0e-3) {
		return 0;
	}

	//if there are forces on chains with no contacts, we have no hope to balance them
	if (freeChainForces) {
		return 1;
	}

	Matrix J(contactJacobian(joints, contacts));
	Matrix D(Contact::frictionForceBlockMatrix(contacts));
	Matrix F(Contact::frictionConstraintsBlockMatrix(contacts));
	Matrix R(Contact::localToWorldWrenchBlockMatrix(contacts));

	//grasp map G = S*R*D
	Matrix G(graspMapMatrix(R, D));

	//left-hand equality matrix JTD = JTran * D
	Matrix JTran(J.transposed());
	Matrix JTD(JTran.rows(), D.cols());
	matrixMultiply(JTran, D, JTD);

	//matrix of zeroes for right-hand of friction inequality
	Matrix zeroes(Matrix::ZEROES<Matrix>(F.rows(), 1));

	//matrix of unknowns
	Matrix c_beta(D.cols(), 1);

	//bounds: all variables greater than 0
	Matrix lowerBounds(Matrix::ZEROES<Matrix>(D.cols(),1));
	Matrix upperBounds(Matrix::MAX_VECTOR(D.cols()));

	//solve QP
	double objVal;
	int result = factorizedQPSolver(G, JTD, tau, F, zeroes, lowerBounds, upperBounds, 
									c_beta, &objVal);
	if (result) {
		if( result > 0) {
			DBGP("Grasp: problem unfeasible");
		} else {
			DBGA("Grasp: QP solver error");
		}
		return result;
	}

	//retrieve contact wrenchs in local contact coordinate systems
	Matrix cWrenches(D.rows(), 1);
	matrixMultiply(D, c_beta, cWrenches);
	DBGP("Contact wrenches:\n" << cWrenches);

	//compute 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);

	//simple sanity check: JT * c = tau
	Matrix fCheck(tau.rows(), 1);
	matrixMultiply(JTran, cWrenches, fCheck);
	for (int j=0; j<tau.rows(); j++) {
		//I am not sure this works well for universal and ball joints
		double err = fabs(tau.elem(j, 0) - fCheck.elem(j,0));
		//take error as a percentage of desired force, if force is non-zero
		if ( fabs(tau.elem(j,0)) > 1.0e-2) {
			err = err / fabs(tau.elem(j, 0));
		} else {
			//for zero desired torque, out of thin air we pull an error threshold of 1.0e2
			//which is 0.1% of the normal range of torques at 1.0e6
			if (err < 1.0e2) err = 0;
		}
		// 0.1% error is considered too much
		if (  err > 1.0e-1) {
			DBGA("Desired torque not obtained on joint " << j << ", error " << err << 
				" out of " << fabs(tau.elem(j, 0)) );
			return -1;
		}
	}

	//complex sanity check: is object force same as QP optimization result?
	//this is only expected to work if all contacts are on the same object
	double* extWrench = object->getExtWrenchAcc();
	vec3 force(extWrench[0], extWrench[1], extWrench[2]);
	vec3 torque(extWrench[3], extWrench[4], extWrench[5]);
	//take into account the scaling that has taken place
	double wrenchError = objVal*1.0e-6 - (force.len_sq() + torque.len_sq()) * 1.0e6;
	//units here are N * 1.0e-6; errors should be in the range on miliN
	if (wrenchError > 1.0e3) {
		DBGA("Wrench sanity check error: " << wrenchError);
		return -1;
	}
	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 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;
}
Exemple #3
0
/*! Optimizes contact forces AND tendon route (defined by the insertion point
  "arms" l_i) and some construction parameters (joint radius r and link
  length l) to get 0 resultant wrench on object, with all contact forces
  legally inside their friction cones.

  The optimization objective is the equilibrium criterion, where joint
  torques should be balanced by contact forces. However, joint torques
  are expressed as a function of tendon force (which is assumed known since
  there is just one tendon) and tendon and hand parameters.

  JTD * beta = tau = f * (B * p + a)
  p = [l r d]

  To get rid of the free term, the relationship becomes:

  [JTD -B -I] * [beta p a] = 0

  with the variables in a fixed to their known values.

  We also have to constrain the sum contact amplitudes so at least some force
  is applied to the object. If we don't, it might find solutions where tendon
  forces just cancel out in this particular configuration. This is not ideal,
  as the value that we constrain it to is somewhat arbitrary. However, it should
  be compensated since the equilibrium constraint is not hard, but rather the
  optimization objective.

  Alternatively, we can multiply the B and a matrices by the desired tendon
  force which does the same thing a lot more elegantly.

  Here, the constraints themselves are nothing but the force-closure criterion.
  Therefore, they should be satisfiable anytime the grasp has f-c. The addition
  is the equilibrium, which is hand specific, but is the objective rather than
  a constraint.

  The overall optimization problem has the form:

  minimize
  [beta p a] [JTD -B -I]^T [JTD -B -I] [beta p a]^t      (joint equilibrium)
  constrained by:
  [G 0 0] [beta p a]^T  = |0|                (0 resultant wrench)
  [S 0 0]         |1|                (some force applied on object)
  [F 0 0] [beta p a]^T <= 0                (all forces inside cone)
  beta >= 0                        (legal contact forces)
  p_min <= p <= p_max                    (limits on the parameters)
  a = a_known                        (free term is known)

  Return results: we report that the problem is unfeasible if either the
  optimization itself if unfeasible OR the objective is not 0. However, if
  the optimization is successful but with non-0 objective, we accumulate and
  display contact forces as usual, and return the optimized parameters.
  Codes: 0 is success; >0 means problem unfeasible; <0 means error in
  computation
*/
int
McGripGrasp::tendonAndHandOptimization(Matrix *parameters, double &objValRet)
{
  //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. for now we get all the joints and in
  //the specific order by chain. keep in mind that saved grasps do not have
  //self-contacts so that will bias optimizations badly
  //RECOMMENDED to use this for now only for grasps where both chains are
  //touching the object and there are no self-contacts
  std::list<Joint *> joints;
  for (int c = 0; c < hand->getNumChains(); c++) {
    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 other equilibrium functions.
  Matrix J(contactJacobian(joints, contacts));
  Matrix D(Contact::frictionForceBlockMatrix(contacts));
  Matrix F(Contact::frictionConstraintsBlockMatrix(contacts));
  Matrix R(Contact::localToWorldWrenchBlockMatrix(contacts));
  //grasp map that relates contact amplitudes to object wrench G = S*R*D
  Matrix G(graspMapMatrixFrictionEdges(R, D));
  //matrix that relates contact forces to joint torques JTD = JTran * D
  Matrix JTran(J.transposed());
  Matrix JTD(JTran.cols(), D.cols());
  matrixMultiply(JTran, D, JTD);

  //get the routing equation matrices
  Matrix *a, *negB;
  static_cast<McGrip *>(hand)->getRoutingMatrices(&negB, &a);
  negB->multiply(-1.0);
  assert(JTD.cols() == negB->rows());
  assert(JTD.cols() == a->rows());
  //scale B and a by tendon force
  double tendonForce = 1.0e6;
  negB->multiply(tendonForce);
  a->multiply(tendonForce);

  //int numBetas = JTD.cols();
  int numParameters = negB->cols();
  //int numFree = a->rows();

  //matrix of unknowns
  Matrix p(JTD.cols() + negB->cols() + a->rows(), 1);

  //optimization objective
  Matrix Q(JTD.cols(), p.cols());
  Q.copySubMatrix(0, 0, JTD);
  Q.copySubMatrix(0, JTD.cols(), *negB);
  Q.copySubMatrix(0, JTD.cols() + negB->cols(), Matrix::NEGEYE(a->rows(), a->rows()));

  //friction matrix padded with zeroes
  Matrix FO(Matrix::ZEROES<Matrix>(F.cols(), p.cols()));
  FO.copySubMatrix(0, 0, F);

  //equality constraint is just grasp map padded with zeroes
  Matrix EqLeft(Matrix::ZEROES<Matrix>(G.cols(), p.cols()));
  EqLeft.copySubMatrix(0, 0, G);
  //and right hand side is just zeroes
  Matrix eqRight(Matrix::ZEROES<Matrix>(G.cols(), 1));

  /*
  //let's add the summation to the equality constraint
  Matrix EqLeft( Matrix::ZEROES<Matrix>(G.cols() + 1, p.cols()) );
  EqLeft.copySubMatrix(0, 0, G);
  Matrix sum(1, G.cols());
  sum.setAllElements(1.0);
  EqLeft.copySubMatrix(G.cols(), 0, sum);
  //and the right side of the equality constraint
  Matrix eqRight( Matrix::ZEROES<Matrix>(G.cols()+1, 1) );
  //betas must sum to one
  eqRight.elem(G.cols(), 0) = 1.0;
  */

  //lower and upper bounds
  Matrix lowerBounds(Matrix::MIN_VECTOR(p.cols()));
  Matrix upperBounds(Matrix::MAX_VECTOR(p.cols()));
  //betas are >= 0
  for (int i = 0; i < JTD.cols(); i++) {
    lowerBounds.elem(i, 0) = 0.0;
  }
  //l's have hard-coded upper and lower limits
  for (int i = 0; i < 6; i++) {
    lowerBounds.elem(JTD.cols() + i, 0) = -5.0;
    upperBounds.elem(JTD.cols() + i, 0) =  5.0;
  }

  //use this if you already know the hand parameters and are just using this to check
  //how close this grasp is to equilibrium
  //tendon insertion points
  lowerBounds.elem(JTD.cols() + 0, 0) = 5;
  lowerBounds.elem(JTD.cols() + 1, 0) = 5;
  lowerBounds.elem(JTD.cols() + 2, 0) = 1.65;
  upperBounds.elem(JTD.cols() + 0, 0) = 5;
  upperBounds.elem(JTD.cols() + 1, 0) = 5;
  upperBounds.elem(JTD.cols() + 2, 0) = 1.65;
  //--------------
  lowerBounds.elem(JTD.cols() + 3, 0) = 5;
  lowerBounds.elem(JTD.cols() + 4, 0) = 5;
  lowerBounds.elem(JTD.cols() + 5, 0) = 1.65;
  upperBounds.elem(JTD.cols() + 3, 0) = 5;
  upperBounds.elem(JTD.cols() + 4, 0) = 5;
  upperBounds.elem(JTD.cols() + 5, 0) = 1.65;

  //r and d have their own hard-coded limits, fixed for now
  lowerBounds.elem(JTD.cols() + 6, 0) = static_cast<McGrip *>(hand)->getJointRadius();
  upperBounds.elem(JTD.cols() + 6, 0) = static_cast<McGrip *>(hand)->getJointRadius();
  lowerBounds.elem(JTD.cols() + 7, 0) = static_cast<McGrip *>(hand)->getLinkLength();
  upperBounds.elem(JTD.cols() + 7, 0) = static_cast<McGrip *>(hand)->getLinkLength();
  //the "fake" variables in a are fixed
  for (int i = 0; i < a->rows(); i++) {
    lowerBounds.elem(JTD.cols() + 8 + i, 0) = a->elem(i, 0);
    upperBounds.elem(JTD.cols() + 8 + i, 0) = a->elem(i, 0);
  }

  //we don't need the routing matrices any more
  delete negB;
  delete a;

  //solve the whole thing
  double objVal;
  int result = factorizedQPSolver(Q,
                                  EqLeft, eqRight,
                                  FO, Matrix::ZEROES<Matrix>(FO.cols(), 1),
                                  lowerBounds, upperBounds,
                                  p, &objVal);
  objValRet = objVal;

  if (result) {
    if (result > 0) {
      DBGA("McGrip constr optimization: problem unfeasible");
    } else {
      DBGA("McGrip constr optimization: QP solver error");
    }
    return result;
  }
  DBGA("Construction optimization objective: " << objVal);
  DBGP("Result:\n" << p);

  //get the contact forces and the parameters; display contacts as usual
  Matrix beta(JTD.cols(), 1);
  beta.copySubBlock(0, 0, JTD.cols(), 1, p, 0, 0);
  //scale betas by tendon foce
  beta.multiply(1.0e7);
  parameters->copySubBlock(0, 0, numParameters, 1, p, JTD.cols(), 0);

  //retrieve contact wrenches in local contact coordinate systems
  Matrix cWrenches(D.cols(), 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.cols(), 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);

  //we actually say the problem is also unfeasible if objective is not 0
  //this is magnitude squared of contact wrench, where force units are N*1.0e6
  //acceptable force is 1mN -> (1.0e3)^2 magnitude
  if (objVal > 1.0e6) {
    return 1;
  }
  return 0;
}
void
McGripOptimizer::processGrasp()
{
	//get all the contacts from the grasp
	std::list<Contact*> contacts;
	for (int i=0; i<mHand->getGrasp()->getNumContacts(); i++) {
		contacts.push_back(mHand->getGrasp()->getContact(i));
	}
	//get all the joints, the same way as in all optimization routines
	std::list<Joint*> joints;
	for (int c=0; c<mHand->getNumChains(); c++) {
		std::list<Joint*> chainJoints = mHand->getChain(c)->getJoints();
		joints.insert(joints.end(), chainJoints.begin(), chainJoints.end());
	}


	//process them like usual in all the optimization routines
	Matrix J(mHand->getGrasp()->contactJacobian(joints, contacts));
	Matrix D(Contact::frictionForceBlockMatrix(contacts));
	Matrix F(Contact::frictionConstraintsBlockMatrix(contacts));
	Matrix R(Contact::localToWorldWrenchBlockMatrix(contacts));
	Matrix G(mHand->getGrasp()->graspMapMatrix(R,D));
	Matrix JTran(J.transposed());
	Matrix JTD(JTran.rows(), D.cols());
	matrixMultiply(JTran, D, JTD);

	//get the routing matrices
	Matrix *B, *a;
	static_cast<McGrip*>(mHand)->getRoutingMatrices(&B, &a);

	//place each in the appropriate list

	Matrix *JTD_negI = new Matrix( Matrix::BLOCKROW<Matrix>(JTD, Matrix::NEGEYE(a->rows(), a->rows())) );
	JTD_negI_list.push_back(JTD_negI);

	Matrix *NegB = new Matrix(*B);
	NegB->multiply(-1.0);
	NegB_list.push_back(NegB);

	Matrix *GO = new Matrix( Matrix::BLOCKROW<Matrix>(G, Matrix::ZEROES<Matrix>(G.rows(), a->rows())) );
	GO_list.push_back(GO);

	Matrix *FO = new Matrix( Matrix::BLOCKROW<Matrix>(F, Matrix::ZEROES<Matrix>(F.rows(), a->rows())) );
	FO_list.push_back(FO);

	Matrix S(1, JTD.cols());
	S.setAllElements(1.0);
	Matrix *SO = new Matrix( Matrix::BLOCKROW<Matrix>(S, Matrix::ZEROES<Matrix>(1, a->rows())) );
	SO_list.push_back(SO);

	Matrix *lowerBounds = new Matrix( Matrix::BLOCKCOLUMN<Matrix>(Matrix::ZEROES<Matrix>(JTD.cols(),1), *a) );
	lowerBounds_list.push_back(lowerBounds);
	Matrix *upperBounds = new Matrix( Matrix::BLOCKCOLUMN<Matrix>(Matrix::MAX_VECTOR(JTD.cols()), *a) );
	upperBounds_list.push_back(upperBounds);

	//the joint displacement matrix
	Matrix *Th;
	static_cast<McGrip*>(mHand)->getJointDisplacementMatrix(&Th);
	Th_list.push_back(Th);

	delete a;
	delete B;
}
Exemple #5
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;
}