void Leaf::computeBboxOO() { //compute the covariance matrix double covMat[3][3], v[3][3]; areaWeightedCovarianceMatrix(covMat); DBGP("Cov mat:"); DBGST(print(covMat)); //perform singular value decomposition Jacobi(covMat, v); DBGP("eigenvalues:"); DBGST(print(covMat)); DBGP("eigenVectors:"); DBGST(print(v)); int first = 0, second = 1, third = 2; if (covMat[1][1] > covMat[0][0]) { std::swap(first, second); } if (covMat[2][2] > covMat[first][first]) { std::swap(first, third); } if (covMat[third][third] > covMat[second][second]) { std::swap(second, third); } DBGP("Eigenvalues: " << covMat[first][first] << " " << covMat[second][second] << " " << covMat[third][third]); //set up rotation matrix vec3 xAxis(v[0][first], v[1][first], v[2][first]); vec3 yAxis(v[0][second], v[1][second], v[2][second]); vec3 zAxis = normalise(xAxis) * normalise(yAxis); yAxis = zAxis * normalise(xAxis); xAxis = yAxis * zAxis; mat3 R(xAxis, yAxis, zAxis); DBGP("Matrix: " << R); //compute bbox extents vec3 halfSize, center; fitBox(R, center, halfSize); //rotate box so that x axis always points in the direction of largest extent first = 0; if (halfSize.y() > halfSize.x()) first = 1; if (halfSize.z() > halfSize[first]) first = 2; transf rotate = transf::IDENTITY; if (first == 1) { // y has the largest extent, rotate around z rotate = rotate_transf(M_PI/2.0, vec3(0,0,1)); } else if (first == 2) { // z has the largest extent, rotate around y rotate = rotate_transf(M_PI/2.0, vec3(0,1,0)); } halfSize = halfSize * rotate; for (int i=0; i<3; i++) { if (halfSize[i] < 0) halfSize[i] = -halfSize[i]; } mat3 RR; rotate.rotation().ToRotationMatrix(RR); R = RR * R; mBbox.halfSize = halfSize; mBbox.setTran( transf(R, center ) ); }
int iterateDynamics(std::vector<Robot *> robotVec, std::vector<DynamicBody *> bodyVec, DynamicParameters *dp) { double h = dp->timeStep; bool useContactEps = dp->useContactEps; static double Jcg_tmp[9],Jcg_B[9],Jcg_N[9],Jcg_N_inv[9],R_N_B[9]; static double db0=0.0,tmp3[3]; static mat3 Rot; static int info; World *myWorld; KinematicChain *chain; int numBodies = bodyVec.size(),errCode = 0; int numRobots = robotVec.size(); int numJoints=0; int numDOF=0; int bn,cn,i,j; int Mrows,Dcols,Arows,Hrows,Hcols,Nurows,Nucols; int numDOFLimits=0; std::list<Contact *> contactList; std::list<Contact *> objContactList; std::list<Contact *>::iterator cp; // unsigned long dmark = dmalloc_mark(); double *ql = new double[7*numBodies]; double *qnew = new double[7*numBodies]; double *vl = new double[6*numBodies]; double *vlnew = new double[6*numBodies]; double *M = new double[(6*numBodies)*(6*numBodies)]; double *M_i = new double[(6*numBodies)*(6*numBodies)]; double *fext = new double[6*numBodies]; // LCP matrix double *A; // LCP vectors double *g,*lambda; double *predLambda = NULL; //used for debugging the prediction of LCP basis // main matrices for contact constraints double *H; // main matrices for joint constraints double *Nu; // main vector for contact constraints double *k; // main vectors for joint constraints double *eps; // intermediate matrices for contact constraints double *HtM_i,*v1; // intermediate matrices for contact constraints double *v2; // intermediate matrices for case of both joint and contact constraints double *NutM_i,*NutM_iNu,*INVNutM_iNu,*INVNutM_iNuNut; double *INVNutM_iNuNutM_i,*INVNutM_iNuNutM_iH; // intermediate vectors for case of both joint and contact constraints double *NutM_ikminuseps,*INVNutM_iNuNutM_ikminuseps; double *currq,*currM; Mrows = 6*numBodies; myWorld = bodyVec[0]->getWorld(); std::map<Body*, int> islandIndices; for (i=0;i<myWorld->getNumBodies();i++) { islandIndices.insert( std::pair<Body*, int>(myWorld->getBody(i), -1) ); } for (i=0;i<numBodies;i++) { islandIndices[ bodyVec[i] ] = i; } // count the joints and DOF, and the joint coupling constraints int numCouplingConstraints = 0; for (i=0;i<numRobots;i++) { numDOF += robotVec[i]->getNumDOF(); for (j=0;j<robotVec[i]->getNumChains();j++) { chain = robotVec[i]->getChain(j); numJoints += chain->getNumJoints(); } for (j=0;j<robotVec[i]->getNumDOF();j++) { numCouplingConstraints += robotVec[i]->getDOF(j)->getNumCouplingConstraints(); numDOFLimits += robotVec[i]->getDOF(j)->getNumLimitConstraints(); } } DBGP("Dynamics time step: " << h); DBGP("numJoints: " << numJoints); // count the total number of joints and contacts int numContacts = 0; int numTotalFrictionEdges = 0; int numDynJointConstraints=0; for (bn=0;bn<numBodies;bn++) { //count joints if (bodyVec[bn]->getDynJoint()) { int numCon = bodyVec[bn]->getDynJoint()->getNumConstraints(); numDynJointConstraints += numCon; DBGP(bodyVec[bn]->getName().latin1() << ": " << numCon << " constraints"); } //count contacts objContactList = bodyVec[bn]->getContacts(); for (cp=objContactList.begin();cp!=objContactList.end();cp++) { // check if the mate of this contact is already in the contact list if (std::find(contactList.begin(),contactList.end(),(*cp)->getMate()) == contactList.end()) { numContacts++; numTotalFrictionEdges += (*cp)->numFrictionEdges; contactList.push_back(*cp); } } } DBGP("Num contacts: " << numContacts); DBGP("Num friction edges: " << numTotalFrictionEdges); DBGP("Num dynjoint: " << numDynJointConstraints); // zero out matrices dcopy(Mrows*Mrows,&db0,0,M,1); dcopy(Mrows*Mrows,&db0,0,M_i,1); dcopy(Mrows,&db0,0,fext,1); //allocate the joint constraint matrices if (numJoints) { Nurows = Mrows; Nucols = numDynJointConstraints + numCouplingConstraints; DBGP("Nucols: " << Nucols); Nu = new double[Nurows * Nucols]; dcopy(Nurows*Nucols,&db0,0,Nu,1); eps = new double[Nucols]; dcopy(Nucols,&db0,0,eps,1); Arows = Mrows+Nucols; } // allocate the LCP matrix if (numContacts || numDOFLimits) { Dcols = numTotalFrictionEdges; DBGP("numContacts " << numContacts); DBGP("Dcols " << Dcols); DBGP("numDOFLimits " << numDOFLimits); Hrows = Mrows; Hcols = Dcols + 2*numContacts + numDOFLimits; H = new double[Hrows * Hcols]; dcopy(Hrows*Hcols,&db0,0,H,1); v1 = new double[Hrows * Hcols]; v2 = new double[Hrows]; dcopy(Hrows*Hcols,&db0,0,v1,1); dcopy(Hrows,&db0,0,v2,1); k = new double[Mrows]; //holds mass*previous velocity and external impulses Arows = Hcols; lambda = new double[Arows]; // the LCP solution } else { Dcols = 0; } // allocate the constraint matrix if (numJoints || numContacts) { A = new double[Arows*Arows]; g = new double[Arows]; dcopy(Arows*Arows,&db0,0,A,1); dcopy(Arows,&db0,0,g,1); } // compute mass matrix and external forces for (bn=0;bn<numBodies;bn++) { memcpy(vl+6*bn,bodyVec[bn]->getVelocity(),6*sizeof(double)); memcpy(vlnew+6*bn,bodyVec[bn]->getVelocity(),6*sizeof(double)); memcpy(ql+7*bn,bodyVec[bn]->getPos(),7*sizeof(double)); memcpy(qnew+7*bn,bodyVec[bn]->getPos(),7*sizeof(double)); currq = qnew + 7*bn; Quaternion tmpQuat(currq[3],currq[4],currq[5],currq[6]); tmpQuat.ToRotationMatrix(Rot); // The rotation matrix returned by ToRotationMatrix is expressed as // a graphics style rot matrix (new axes are in rows), the R_N_B matrix // is a robotics style rot matrix (new axes in columns) R_N_B[0] = Rot[0]; R_N_B[3] = Rot[1]; R_N_B[6] = Rot[2]; R_N_B[1] = Rot[3]; R_N_B[4] = Rot[4]; R_N_B[7] = Rot[5]; R_N_B[2] = Rot[6]; R_N_B[5] = Rot[7]; R_N_B[8] = Rot[8]; // Jcg_N = R_N_B * Jcg_B * R_N_B'; // where Jcg_B is inertia matrix in body coords // Jcg_N is inertia matrix in world coords ? memcpy(Jcg_B,bodyVec[bn]->getInertia(),9*sizeof(double)); //multiply by mass dscal(9, bodyVec[bn]->getMass(), Jcg_B, 1); dgemm("N","N",3,3,3,1.0,R_N_B,3,Jcg_B,3,0.0,Jcg_tmp,3); dgemm("N","T",3,3,3,1.0,Jcg_tmp,3,R_N_B,3,0.0,Jcg_N,3); if ((info = invertMatrix(3,Jcg_N,Jcg_N_inv))) { printf("In iterateDynamics, inertia matrix inversion failed (info is %d)\n",info); fprintf(stderr,"%f %f %f\n",Jcg_B[0], Jcg_B[1], Jcg_B[2]); fprintf(stderr,"%f %f %f\n",Jcg_B[3], Jcg_B[4], Jcg_B[5]); fprintf(stderr,"%f %f %f\n",Jcg_B[6], Jcg_B[7], Jcg_B[8]); fprintf(stderr,"Body is %s\n",bodyVec[bn]->getName().latin1()); } currM = M+((6*bn)*Mrows + bn*6); //point to the correct block of M currM[0] = bodyVec[bn]->getMass(); currM[6*numBodies+1] = bodyVec[bn]->getMass(); currM[12*numBodies+2] = bodyVec[bn]->getMass(); fillMatrixBlock(Jcg_N,3,3,3,5,5,currM,Mrows); currM = M_i+((6*bn)*Mrows + bn*6);//point to correct block of M_i currM[0] = 1.0/bodyVec[bn]->getMass(); currM[Mrows+1] = 1.0/bodyVec[bn]->getMass(); currM[2*Mrows+2] = 1.0/bodyVec[bn]->getMass(); fillMatrixBlock(Jcg_N_inv,3,3,3,5,5,currM,Mrows); // compute external wrench // fext = [ 0 0 -9810.0*mass -[ang_vel_N x (Jcg_N * ang_vel_N)] ] //based on this, it would appear that graspit force units are N*1.0e6 fext[6*bn+2] = -9810.0 * bodyVec[bn]->getMass() * dp->gravityMultiplier; // force of gravity // fext[6*bn+2] = 0; // NO force of gravity dgemv("N",3,3,1.0,Jcg_N,3,&vl[6*bn+3],1,0.0,tmp3,1); // inertial moments fext[6*bn+3] = - (vl[6*bn+4]*tmp3[2] - vl[6*bn+5]*tmp3[1]); fext[6*bn+4] = - (vl[6*bn+5]*tmp3[0] - vl[6*bn+3]*tmp3[2]); fext[6*bn+5] = - (vl[6*bn+3]*tmp3[1] - vl[6*bn+4]*tmp3[0]); double ForcesToBodyFrame[36]; transf invBody = bodyVec[bn]->getTran().inverse(); vec3 invBodyTransl = invBody.translation(); buildForceTransform(invBody,invBodyTransl,ForcesToBodyFrame); DBGP("fext initial: "); DBGST( disp_mat(stdout,&fext[6*bn],1,6,0) ); // add any other wrenches that have accumulated on the body daxpy(6,1.0,bodyVec[bn]->getExtWrenchAcc(),1,&fext[6*bn],1); DBGP("fext with accumulated wrench: "); DBGST( disp_mat(stdout,&fext[6*bn],1,6,0) ); if (numContacts||numDOFLimits) { // k = Mv_l + hfext currM = M+((6*bn)*Mrows + bn*6); //point to the correct block of M dgemv("N",6,6,1.0,currM,Mrows,vl+6*bn,1,0.0,k+6*bn,1); } } if (numJoints) { int ncn = 0; int hcn = 0; for (i=0;i<numBodies;i++) { if (bodyVec[i]->getDynJoint()) bodyVec[i]->getDynJoint()-> buildConstraints(Nu,eps,numBodies,islandIndices,ncn); } for (i=0;i<numRobots;i++) { robotVec[i]->buildDOFLimitConstraints(islandIndices,numBodies,H,g,hcn); robotVec[i]->buildDOFCouplingConstraints(islandIndices,numBodies,Nu,eps,ncn); } for (i=0;i<Nucols;i++) { eps[i] *= ERP/h; } for (i=0; i<hcn; i++) { g[i] *= ERP/h; } } // add contacts to the LCP if (!contactList.empty()) { DBGP("processing contacts"); double Ftform_N_C[36]; // A is square double *Wn = &H[numDOFLimits*Hrows]; double *D = &H[(numDOFLimits+numContacts)*Hrows]; double *E = &A[(numDOFLimits+numContacts+Dcols)*Arows + numDOFLimits+numContacts]; double *negET = &A[(numDOFLimits+numContacts)*Arows + numDOFLimits+numContacts+Dcols]; double *MU = &A[numDOFLimits*Arows + numDOFLimits+numContacts+Dcols]; double *contactEps = &g[numDOFLimits]; int frictionEdgesCount = 0; for (cp=contactList.begin(),cn=0; cp!=contactList.end(); cp++,cn++){ //DBGP("contact " << cn); transf cf = (*cp)->getContactFrame() * (*cp)->getBody1Tran(); transf cf2 = (*cp)->getMate()->getContactFrame() * (*cp)->getBody2Tran(); DBGP("CONTACT DISTANCE: " << (cf.translation() - cf2.translation()).len()); if (useContactEps) { contactEps[cn] = MIN(0.0,-ERP/h * (Contact::THRESHOLD/2.0 - (cf.translation() - cf2.translation()).len())); } DBGP(" EPS: " << contactEps[cn]); vec3 normal(cf.affine().element(2,0), cf.affine().element(2,1), cf.affine().element(2,2)); // find which body is this contact from for (bn=0;bn<numBodies;bn++) if ((*cp)->getBody1() == bodyVec[bn]) break; if (bn<numBodies) { //????? this doesn't seem correct vec3 radius = cf.translation() - ( bodyVec[bn]->getCoG() * (*cp)->getBody1Tran() - position::ORIGIN ); // radius = radius / 1000.0; // convert to meters vec3 RcrossN = radius * normal; DBGP("body1 normal: " << normal); DBGP("body1 radius: " << radius); Wn[cn*Hrows+6*bn] = normal.x(); Wn[cn*Hrows+6*bn+1] = normal.y(); Wn[cn*Hrows+6*bn+2] = normal.z(); Wn[cn*Hrows+6*bn+3] = RcrossN.x(); Wn[cn*Hrows+6*bn+4] = RcrossN.y(); Wn[cn*Hrows+6*bn+5] = RcrossN.z(); vec3 bodyOrigin = bodyVec[bn]->getCoG() * (*cp)->getBody1Tran() - position::ORIGIN; buildForceTransform(cf,bodyOrigin,Ftform_N_C); /* dgemm("N","N", 6,Contact::numFrictionEdges,6, 1.0,Ftform_N_C,6, Contact::frictionEdges,6, 0.0,&D[Contact::numFrictionEdges*cn*Hrows+6*bn],Hrows); */ dgemm("N","N", 6,(*cp)->numFrictionEdges,6, //m, n, k 1.0,Ftform_N_C,6, //alfa, A, lda (*cp)->frictionEdges,6, //B, ldb 0.0,&D[ frictionEdgesCount*Hrows+6*bn],Hrows); //beta, C, ldc } //find the other body for(bn=0;bn<numBodies;bn++) if ((*cp)->getBody2() == bodyVec[bn]) break; if (bn<numBodies) { //normal = vec3(cf2.affine().element(2,0), cf2.affine().element(2,1),cf2.affine().element(2,2)); normal = -normal; //vec3 radius = cf2.translation() - (bodyVec[bn]->getCoG() * (*cp)->getBody2Tran() - position::ORIGIN); vec3 radius = cf.translation() - (bodyVec[bn]->getCoG() * (*cp)->getBody2Tran() - position::ORIGIN); vec3 RcrossN = radius * normal; DBGP("body2 normal: " << normal); DBGP("body2 radius: " << radius); Wn[cn*Hrows+6*bn] = normal.x(); Wn[cn*Hrows+6*bn+1] = normal.y(); Wn[cn*Hrows+6*bn+2] = normal.z(); Wn[cn*Hrows+6*bn+3] = RcrossN.x(); Wn[cn*Hrows+6*bn+4] = RcrossN.y(); Wn[cn*Hrows+6*bn+5] = RcrossN.z(); vec3 bodyOrigin = bodyVec[bn]->getCoG()*(*cp)->getBody2Tran() - position::ORIGIN; buildForceTransform(cf,bodyOrigin,Ftform_N_C); //buildForceTransform(cf2,bodyOrigin,Ftform_N_C); /* dgemm("N","N",6,Contact::numFrictionEdges,6,-1.0,Ftform_N_C,6, Contact::frictionEdges,6, 0.0,&D[Contact::numFrictionEdges*cn*Hrows+6*bn],Hrows);*/ //original graspit had a -1.0 here in front of Ftform_N_C dgemm("N","N", 6,(*cp)->numFrictionEdges,6, -1.0,Ftform_N_C,6, (*cp)->frictionEdges,6, 0.0,&D[ frictionEdgesCount*Hrows+6*bn ],Hrows); } //for (i=cn*Contact::numFrictionEdges; i<(cn+1)*Contact::numFrictionEdges; i++) { for (i=frictionEdgesCount; i<frictionEdgesCount+(*cp)->numFrictionEdges; i++) { E[cn*Arows+i] = 1.0; negET[i*Arows+cn] = -1.0; } MU[cn*Arows + cn] = (*cp)->getCof(); frictionEdgesCount += (*cp)->numFrictionEdges; } } if (numContacts || numDOFLimits) daxpy(Mrows,h,fext,1,k,1); if (numJoints && (numContacts || numDOFLimits)) { // Cnu1 = INV(Nu'M_iNu)Nu'M_iH // Cnu2 = INV(Nu'M_iNu)(Nu'M_ik-eps) // v1 = -NuCnu1 // v2 = -NuCnu2 NutM_i = new double[Nucols*Mrows]; NutM_iNu = new double[Nucols*Nucols]; INVNutM_iNu = new double[Nucols*Nucols]; INVNutM_iNuNut = new double[Nucols*Nurows]; INVNutM_iNuNutM_i = new double[Nucols*Mrows]; INVNutM_iNuNutM_iH = new double[Nucols*Hcols]; NutM_ikminuseps = new double[Nucols]; INVNutM_iNuNutM_ikminuseps = new double[Nucols]; dgemm("T","N",Nucols,Mrows,Mrows,1.0,Nu,Nurows,M_i,Mrows, 0.0,NutM_i,Nucols); dgemm("N","N",Nucols,Nucols,Mrows,1.0,NutM_i,Nucols,Nu,Nurows, 0.0,NutM_iNu,Nucols); if ((info = invertMatrix(Nucols,NutM_iNu,INVNutM_iNu))) printf("In iterateDynamics, NutM_iNu matrix inversion failed (info is %d)\n",info); dgemm("N","T",Nucols,Nurows,Nucols,1.0,INVNutM_iNu,Nucols,Nu,Nurows, 0.0,INVNutM_iNuNut,Nucols); dgemm("N","N",Nucols,Mrows,Mrows,1.0,INVNutM_iNuNut,Nucols,M_i,Mrows, 0.0,INVNutM_iNuNutM_i,Nucols); dgemm("N","N",Nucols,Hcols,Mrows,1.0,INVNutM_iNuNutM_i,Nucols,H,Hrows, 0.0,INVNutM_iNuNutM_iH,Nucols); dgemm("N","N",Nurows,Hcols,Nucols,-1.0,Nu,Nurows,INVNutM_iNuNutM_iH,Nucols, 0.0,v1,Nurows); dgemv("N",Nucols,Mrows,1.0,NutM_i,Nucols,k,1,0.0,NutM_ikminuseps,1); daxpy(Nucols,-1.0,eps,1,NutM_ikminuseps,1); dgemv("N",Nucols,Nucols,1.0,INVNutM_iNu,Nucols,NutM_ikminuseps,1, 0.0,INVNutM_iNuNutM_ikminuseps,1); dgemv("N",Nurows,Nucols,-1.0,Nu,Nurows,INVNutM_iNuNutM_ikminuseps,1, 0.0,v2,1); } if (numContacts || numDOFLimits) { // in the simple case without joint constraints // A = H'M_iv1 + N // g = H'M_iv2 // where N is already stored in A // v1 is the first term of v_(l+1) and v2 is the second term // v_l+1 = M_i(v1 lambda + v2) = M_i(H lambda + k) // k is (Mv_l + hfext) //add H to v1 //add k to v2 DBGP("k:"); DBGST( disp_mat(stdout,k,1,Mrows,0) ); DBGP("first g:"); DBGST( disp_mat(stdout,g,1,Arows,0) ); daxpy(Mrows*Hcols,1.0,H,1,v1,1); daxpy(Mrows,1.0,k,1,v2,1); // build A and g HtM_i = new double[Hcols*Mrows]; dgemm("T","N",Hcols,Mrows,Hrows,1.0,H,Hrows,M_i,Mrows,0.0,HtM_i,Hcols); dgemm("N","N",Hcols,Hcols,Mrows,1.0,HtM_i,Hcols,v1,Mrows,1.0,A,Arows); // dgemv("N",Hcols,Mrows,1.0,HtM_i,Hcols,v2,1,0.0,g,1); dgemv("N",Hcols,Mrows,1.0,HtM_i,Hcols,v2,1,1.0,g,1); } int frictionEdgesCount; //debug information; can be removed if (numContacts || numDOFLimits) { bool lemkePredict = false; if (lemkePredict) { //try to use information from previous time steps to guess a good starting basis for Lemke's algorithm assembleLCPPrediction(lambda, Arows, numDOFLimits, &contactList); predLambda = new double[Arows]; // keep a copy of the prediction so we can check it later dcopy(Arows, lambda, 1, predLambda, 1); // fprintf(stderr,"Prediction: \n"); // printLCPBasis(predLambda, Arows, numDOFLimits, numContacts); } // double startTime; // startTime = getTime(); DBGP("g:"); DBGST( for (i=0;i<Arows;i++) printf("%le ",g[i]); );