/*! 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. Checks if some combination of contacts 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 contactForceExistence(Matrix &F, Matrix &N, Matrix &Q, Matrix &beta, double *objVal) { // exact problem formulation // unknowns: beta (contact forces) // minimize betaT*QT*Q*beta (magnitude of resultant object wrench) // subject to: // sum_normal * beta = 1 (we are applying some contact forces) // F * beta <= 0 (all forces inside friction cones) // beta >= 0 (all forces must be positive) Matrix right_hand(1,1); //a total of 10N of normal force right_hand.elem(0,0) = 1.0e7; //right-hand side of inequality matrix Matrix inEqZeroes(F.rows(), 1); inEqZeroes.setAllElements(0.0); //bounds: all variables greater than 0 Matrix lowerBounds(Matrix::ZEROES<Matrix>(beta.rows(),1)); Matrix upperBounds(Matrix::MAX_VECTOR(beta.rows())); /* FILE *fp = fopen("gfo.txt","w"); fprintf(fp,"N:\n"); N.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 = factorizedQPSolver(Q, N, right_hand, F, inEqZeroes, lowerBounds, upperBounds, beta, objVal); return result; }
/*! 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.0e9 here as units are in N * 1.0e-6 * mm //so we want a total joint torque of 1000 N mm right_hand.elem( right_hand.rows()-1, 0) = 1.0e10; //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; }
/*! 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; }
/*! 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; }
/*! This is the overall form of the optimization: x = [beta_1 a_1 beta_2 a_2 ... beta_n a_n l r d]^T | J_1TD_1 -I -B_1 | | J_2TD_2 -I -B_2 | | ... | = Q | J_nTD_n -I -B_n | | F_1 0 0 | | F_2 0 0 | | ... 0 | = F | F_n 0 0 | | G_1 0 0 | | G_2 0 0 | | ... 0 | = G | G_n 0 0 | | 1 0 0 | | 1 0 0 | | ... 0 | = S | 1 0 0 | lowerBounds = [ 0 a_1 0 a_2 ... 0 a_n l_min r_min d_min ] ^T upperBounds = [ inf a_1 inf a_2 ... inf a_n l_max r_max d_max ] ^T Minimize x^T Q^T Q x (joint equilibrium) subject to: G x = 0 (no resultant wrench on object) S x = 1 (some contact force applied to object) F x <= 0 (all contact forces inside friction cones) lowerBounds <= x <= upperBounds */ void McGripOptimizer::finalize() { DBGA("Processing " << JTD_negI_list.size() << " matrices"); int bCols = NegB_list.front()->cols(); //objective SparseMatrix Q( Matrix::BLOCKROW<SparseMatrix>( Matrix::BLOCKDIAG<SparseMatrix>(&JTD_negI_list), Matrix::BLOCKCOLUMN<SparseMatrix>(&NegB_list) ) ); //equality constraint SparseMatrix GO_block( Matrix::BLOCKDIAG<SparseMatrix>(&GO_list) ); SparseMatrix SO_block( Matrix::BLOCKDIAG<SparseMatrix>(&SO_list) ); assert(GO_block.cols() == SO_block.cols()); bool optimize_r_d; //version with fixed r and d /* SparseMatrix LeftHandEq( Matrix::ZEROES<SparseMatrix>(GO_block.rows() + SO_block.rows(), GO_block.cols() + bCols) ); Matrix rightHandEq( Matrix::ZEROES<Matrix>(GO_block.rows() + SO_block.rows(), 1) ); optimize_r_d = false; */ //version with mobile r and d SparseMatrix LeftHandEq( Matrix::ZEROES<SparseMatrix>(GO_block.rows() + SO_block.rows() + 1, GO_block.cols() + bCols) ); Matrix rightHandEq( Matrix::ZEROES<Matrix>(GO_block.rows() + SO_block.rows() + 1, 1) ); optimize_r_d = true; LeftHandEq.copySubMatrix(0, 0, GO_block); LeftHandEq.copySubMatrix(GO_block.rows(), 0, SO_block); for (int i=0; i<SO_block.rows(); i++) { rightHandEq.elem( GO_block.rows() + i, 0) = 1.0; } if (optimize_r_d) { Matrix rd(1,2); rd.setAllElements(1.0); //copy to lower right corner LeftHandEq.copySubMatrix( LeftHandEq.rows()-1, LeftHandEq.cols()-2, rd); //constraint: sum of r and d rightHandEq.elem( rightHandEq.rows()-1, 0) = 25.0; } //inequality constraint SparseMatrix FO_block( Matrix::BLOCKDIAG<SparseMatrix>(&FO_list) ); SparseMatrix LeftHandInEq( Matrix::ZEROES<SparseMatrix>(FO_block.rows(), FO_block.cols() + bCols) ); LeftHandInEq.copySubMatrix(0, 0, FO_block); Matrix rightHandInEq( Matrix::ZEROES<Matrix>(FO_block.rows(), 1) ); //bounds //the bounds on the overall hand parameters Matrix lowerParameterBounds(8, 1); Matrix upperParameterBounds(8, 1); //tendon routing l's for (int i=0; i<6; i++) { lowerParameterBounds.elem(i,0) = -5.0; upperParameterBounds.elem(i,0) = 5.0; } if (!optimize_r_d) { //joint radius lowerParameterBounds.elem(6,0) = static_cast<McGrip*>(mHand)->getJointRadius(); upperParameterBounds.elem(6,0) = static_cast<McGrip*>(mHand)->getJointRadius(); //link length lowerParameterBounds.elem(7,0) = static_cast<McGrip*>(mHand)->getLinkLength(); upperParameterBounds.elem(7,0) = static_cast<McGrip*>(mHand)->getLinkLength(); } else { //joint radius lowerParameterBounds.elem(6,0) = 3.0; upperParameterBounds.elem(6,0) = 10.0; //link length lowerParameterBounds.elem(7,0) = 15.0; upperParameterBounds.elem(7,0) = 22.0; } //overall bounds for the system Matrix lowerBounds( Matrix::BLOCKCOLUMN<Matrix>( Matrix::BLOCKCOLUMN<Matrix>(&lowerBounds_list), lowerParameterBounds ) ); Matrix upperBounds( Matrix::BLOCKCOLUMN<Matrix>( Matrix::BLOCKCOLUMN<Matrix>(&upperBounds_list), upperParameterBounds ) ); DBGA("Matrices assembled. Q size: " << Q.rows() << " by " << Q.cols()); /* //debug code. printout of all matrices FILE *fp = fopen("mcgrip_optimizer.txt","w"); fprintf(fp, "JTD_negI matrices:\n"); printList(JTD_negI_list, fp); fprintf(fp, "NegB matrices:\n"); printList(NegB_list, fp); fprintf(fp,"Q matrix:\n"); Q.print(fp); fprintf(fp,"GO matrices:\n"); printList(GO_list, fp); fprintf(fp,"Left hand eq. matrix:\n"); LeftHandEq.print(fp); fprintf(fp,"right hand eq. matrix:\n"); rightHandEq.print(fp); fprintf(fp,"FO matrices:\n"); printList(FO_list, fp); fprintf(fp,"Left hand ineq. matrix:\n"); LeftHandInEq.print(fp); fprintf(fp,"right hand ineq. matrix:\n"); rightHandInEq.print(fp); fprintf(fp,"Bounds:\n"); for(int i=0; i<lowerBounds.rows(); i++) { fprintf(fp,"%f <= x <= %f\n",lowerBounds.elem(i,0), upperBounds.elem(i,0)); } fclose(fp); */ //matrix of unknowns Matrix solution(Q.cols(), 1); DBGA("Calling solver"); double objVal; int result = factorizedQPSolver(Q, LeftHandEq, rightHandEq, LeftHandInEq, rightHandInEq, lowerBounds, upperBounds, solution, &objVal); DBGA("Solver complete. Result: " << result << ". Objective: " << objVal); //the parameters are the last 8 entries in the solution Matrix parameters(8,1); parameters.copySubBlock(0, 0, 8, 1, solution, solution.rows()-8, 0); DBGA("Parameters:\n" << parameters); DBGA("Building joint stiffness optimization matrices"); //optimize joint stiffnesses SparseMatrix QTh( Matrix::BLOCKROW<SparseMatrix>( Q, Matrix::BLOCKCOLUMN<SparseMatrix>(&Th_list) ) ); SparseMatrix LEqTh( Matrix::BLOCKROW<SparseMatrix>( LeftHandEq, Matrix::ZEROES<SparseMatrix>(LeftHandEq.rows(), 6) ) ); SparseMatrix LInEqTh( Matrix::BLOCKROW<SparseMatrix>( LeftHandInEq, Matrix::ZEROES<SparseMatrix>(LeftHandInEq.rows(), 6) ) ); //the bounds on the overall hand parameters including joints Matrix lowerParameterBoundsTh(14, 1); Matrix upperParameterBoundsTh(14, 1); //we use the previously optimized values to fix these //lowerParameterBoundsTh.copySubMatrix(0, 0, parameters); //upperParameterBoundsTh.copySubMatrix(0, 0, parameters); //optimize everything at the same time lowerParameterBoundsTh.copySubMatrix(0, 0, lowerParameterBounds); upperParameterBoundsTh.copySubMatrix(0, 0, upperParameterBounds); //for now, stiffnesses just positive //also, we can not build with 0 stifness so just put some small value in //it seems that returned numbers are in the 1.0 range //let's try with 1.0 min and 2.0 max, in practice we might be able to build that for (int i=0; i<6; i++) { lowerParameterBoundsTh.elem(8+i, 0) = 1.0; upperParameterBoundsTh.elem(8+i, 0) = 2.0; } //overall bounds for the system Matrix lowerBoundsTh( Matrix::BLOCKCOLUMN<Matrix>( Matrix::BLOCKCOLUMN<Matrix>(&lowerBounds_list), lowerParameterBoundsTh ) ); Matrix upperBoundsTh( Matrix::BLOCKCOLUMN<Matrix>( Matrix::BLOCKCOLUMN<Matrix>(&upperBounds_list), upperParameterBoundsTh ) ); Matrix solutionTh(QTh.cols(), 1); DBGA("Calling solver for joint stiffness"); result = factorizedQPSolver(QTh, LEqTh, rightHandEq, LInEqTh, rightHandInEq, lowerBoundsTh, upperBoundsTh, solutionTh, &objVal); DBGA("Solver complete. Result: " << result << ". Objective: " << objVal); //the parameters are the last 14 entries in the solution Matrix parametersTh(14,1); parametersTh.copySubBlock(0, 0, 14, 1, solutionTh, solutionTh.rows()-14, 0); DBGA("Parameters:\n" << parametersTh); }
/*! 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; }