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]); );
