//! Only call this function after you run ObtainArticulationData()
//! Author: PMW
void HumanoidController::ComputeComInfo(Matrix6XF & Cmm, Vector6F & bias, Vector3F & pCom, Float & m)
{
LinkInfoStruct * fb = artic->m_link_list[0]; // floating base
dmLink * fbLink = fb->link;
CartesianTensor fb_R_ics;
//pCom initally contains the position of the torso in the i.c.s
fbLink->getPose(fb_R_ics,pCom.data());
Matrix3F ics_R_fb;
ics_R_fb << fb_R_ics[0][0] , fb_R_ics[1][0] , fb_R_ics[2][0],
fb_R_ics[0][1] , fb_R_ics[1][1] , fb_R_ics[2][1],
fb_R_ics[0][2] , fb_R_ics[1][2] , fb_R_ics[2][2];
Vector3F pFBRelCoM;
m = fb->I_C.m;
pFBRelCoM(0) = -fb->I_C.h[0]/m; // fb frame and com frame have the same orientation
pFBRelCoM(1) = -fb->I_C.h[1]/m;
pFBRelCoM(2) = -fb->I_C.h[2]/m;
//Matrix6F IC0;
//CrbToMat(fb->I_C, IC0);
//cout << "FB IC0 in CoM Func " << endl << IC0 << endl;
// pCom = pBase + pComRelBase
pCom -= ics_R_fb*pFBRelCoM;
int N = artic->H.cols();
Cmm.resize(6,N);
Matrix6F XT = Matrix6F::Zero();
XT.block(0,0,3,3) = ics_R_fb;
XT.block(0,3,3,3) = ics_R_fb*cr3(pFBRelCoM);
XT.block(3,3,3,3) = ics_R_fb; // XT is force transformation from FB to COM
Cmm = XT * artic->H.topRows(6); //expressed in ICS coordinate
IC0 = XT*artic->H.block(0,0,6,6)*XT.transpose();
IBarC0 = IC0.block(0,0,3,3);
// These quantities (f,a) for the floating base are the result of
// inverse dynamics with qdd=0. As a result, fb->link_val2.f
// contains the first 6 rows of CandG!
bias = XT * (fb->link_val2.f-artic->H.block(0,0,6,6)*fb->link_val2.a); // bias = \dot{A}_G \dot{q}
}
//! Populate information about ground reaction forces
//! Author: PMW
void HumanoidController::ComputeGrfInfo(GRFInfo & grf)
{
Vector6F fZMP = Vector6F::Zero();
Vector6F icsForce;
SpatialVector localForce;
Float * nICS = icsForce.data();
Float * fICS =nICS+3;
Float * nLoc =localForce;
Float * fLoc =nLoc + 3;
CartesianVector tmp;
grf.localContacts = NS;
grf.pCoPs.resize(NS);
grf.fCoPs.resize(NS);
grf.nCoPs.resize(NS);
grf.footWrenches.resize(NS);
grf.footJacs.resize(NS);
for (int k1 = 0; k1 < NS; k1++)
{
dmRigidBody * body = (dmRigidBody *) artic->getLink(SupportIndices[k1]);
LinkInfoStruct * listruct = artic->m_link_list[SupportIndices[k1]];
artic->computeJacobian(SupportIndices[k1], Matrix6F::Identity(), grf.footJacs[k1]); // get foot jacobian
for (int k2 = 0; k2 < body->getNumForces(); k2++)
{
body->getForce(k2)->computeForce(listruct->link_val2,localForce); // get contact force in local coordinate
grf.footWrenches[k1] << localForce[0],localForce[1],localForce[2],
localForce[3],localForce[4],localForce[5];
//Float * p = localForce;
//cout << "Local Force " << tsc->SupportIndices[k1];
//cout << " = " << localForce[0] << " , " << localForce[1] << " , " << localForce[2] << " , ";
//cout << localForce[3] << " , " << localForce[4] << " , " << localForce[5] << endl;
// Apply Spatial Force Transform Efficiently
// Rotate Quantities
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,nLoc,nICS); // nICS = R_ICS * nLoc
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,fLoc,fICS); // fICS = R_ICS * fLoc
// Add the r cross f
CROSS_PRODUCT(listruct->link_val2.p_ICS,fICS,tmp);
ADD_CARTESIAN_VECTOR(nICS,tmp); // nICS = pICS X fICS + nICS
//cout << "ICS Force " << icsForce.transpose() << endl;
fZMP+=icsForce; // intermediate values
{
Matrix3F RtoICS;
copyRtoMat(listruct->link_val2.R_ICS, RtoICS);
grfInfo.fCoPs[k1] = RtoICS * grf.footWrenches[k1].tail(3);
// from ankle to support origin
Vector6F supportFrameWrench = SupportXforms[k1].inverse().transpose()*grf.footWrenches[k1];
Vector3F supportFrameCoP;
// NOTE: SupportXforms[i] is the 6D transform from ankle frame to support frame
transformToZMP(supportFrameWrench,supportFrameCoP); // get foot COP in support frame
// note "supportFrameWrench" is modified, it becomes "foot ZMP wrench"
grfInfo.nCoPs[k1] = supportFrameWrench(2);
Vector3F pRel;
((dmContactModel *) body->getForce(k2))->getContactPoint(0,pRel.data());
pRel(1) = 0; pRel(2) = 0; // only preserve x component /// !!!
Vector3F footFrameCoP = pRel + SupportXforms[k1].block(0,0,3,3).transpose()*supportFrameCoP;// express the foot COP in foot frame
grfInfo.pCoPs[k1] = RtoICS*footFrameCoP;
grfInfo.pCoPs[k1](0) += listruct->link_val2.p_ICS[0];
grfInfo.pCoPs[k1](1) += listruct->link_val2.p_ICS[1];
grfInfo.pCoPs[k1](2) += listruct->link_val2.p_ICS[2];// foot COP in ICS
}
}
}
transformToZMP(fZMP,grf.pZMP);
grf.fZMP = fZMP.tail(3);
grf.nZMP = fZMP(2);
}
//HumanoidController::HumanoidController(dmArticulation * robot) : TaskSpaceControllerA(robot)
HumanoidController::HumanoidController(dmArticulation * robot) : TaskSpaceControllerConic(robot)
{
// Create Linearized Friction Cone Basis
// in support frame (z-up)
FrictionBasis = MatrixXF::Zero(6,NF);
for (int j=0; j<NF; j++)
{
double angle = (j * 2 * M_PI) / NF;
FrictionBasis(3,j) = MU*cos(angle);
FrictionBasis(4,j) = MU*sin(angle);
FrictionBasis(5,j) = 1;
}
// Initialize Support Jacobians
SupportJacobians.resize(NS);
for (int i=0; i<NS; i++)
{
SupportJacobians[i] = MatrixXF::Zero(6,1);
}
// Indicate support bodies
SupportIndices.resize(NS); // number of support
SupportIndices[0] = 11; // support body 0
SupportIndices[1] = 20; // support body 1
contactState.resize(NS);
slidingState.resize(NS);
// Construct transform from ankle frame to support frame
Matrix3F RSup; // Sup - support
// RSup << 0,0,1,0,1,0,-1,0,0;
RSup << 0,0,1, 0,-1,0, 1,0,0;
XformVector Xforms(NS);
for (int k=0; k<NS; k++)
{
Xforms[k].resize(6,6);
dmRigidBody * link = (dmRigidBody*) artic->getLink(SupportIndices[k]);
dmContactModel * dmContactLattice = (dmContactModel *) link->getForce(0);
Vector3F pRel;
dmContactLattice->getContactPoint(0,pRel.data());
pRel(1) = 0; pRel(2) = 0;
Xforms[k].block(0,0,3,3) = RSup;
Xforms[k].block(0,3,3,3) = Matrix3F::Zero();
Xforms[k].block(3,0,3,3) = -RSup*cr3(pRel);
Xforms[k].block(3,3,3,3) = RSup;
}
SupportXforms = Xforms;
// Construct force transform from each contact point to support origin
PointForceXforms.resize(NS);
for (int i=0; i<NS; i++)
{
PointForceXforms[i].resize(NJ);
//cout << "Getting forces for link " << SupportIndices[i] << endl;
dmRigidBody * linki = (dmRigidBody*) artic->m_link_list[SupportIndices[i]]->link;
dmContactModel * dmContactLattice = (dmContactModel *) linki->getForce(0);
//cout << "Assigning Temporary Matricies" << endl;
Matrix3F RSup = SupportXforms[i].block(0,0,3,3);
Matrix3F tmpMat = SupportXforms[i].block(3,0,3,3)*RSup.transpose();
Vector3F piRelSup;
crossExtract(tmpMat,piRelSup);
//cout << "pirelsup " << endl << piRelSup << endl;
for (int j=0; j<NP; j++)
{
Vector3F pRel, tmp;
//Tmp is now the contact point location relative to the body coordinate
dmContactLattice->getContactPoint(j,tmp.data());
// Point of contact (relative to support origin) in support coordinates
pRel = RSup*tmp + piRelSup;
PointForceXforms[i][j].setIdentity(6,6);
PointForceXforms[i][j].block(0,3,3,3) = cr3(pRel);
}
}
// Initialize variables
q.resize(STATE_SIZE);
qdDm.resize(STATE_SIZE); // joint rate DM
qd.resize(RATE_SIZE); // joint rate
grfInfo.localContacts = 0;
pFoot.resize(NS);
vFoot.resize(NS);
aFoot.resize(NS);
RFoot.resize(NS); // 3x3
pDesFoot.resize(NS);
vDesFoot.resize(NS);
RDesFoot.resize(NS);
aDesFoot.resize(NS);
kpFoot.resize(NS);
kdFoot.resize(NS);
for (int i=0; i<NS; i++)
{
pFoot[i].resize(3);
vFoot[i].resize(6);
aFoot[i].resize(6);
pDesFoot[i].resize(3);
vDesFoot[i].resize(6);
aDesFoot[i].resize(6);
aDesFoot[i].setZero();
vDesFoot[i].setZero();
pDesFoot[i].setZero();
RDesFoot[i].setZero(); // 3x3
kpFoot[i] = 0;
kdFoot[i] = 0;
}
aComDes.resize(3);
aComDes.setZero();
vComDes.resize(3);
vComDes.setZero();
pComDes.resize(3);
pComDes.setZero();
kComDes.resize(3); // angular momentum around com
kComDes.setZero();
int numLinks = artic->getNumLinks();
RDesJoint.resize(numLinks);
posDesJoint.resize(numLinks);
rateDesJoint.resize(numLinks);
accDesJoint.resize(numLinks);
kpJoint.resize(numLinks);
kdJoint.resize(numLinks);
artic->getTrueNumDOFs(); // fill bodyi->index_ext, bodyi->dof
for (int i=0; i<numLinks; i++)
{
LinkInfoStruct * bodyi = artic->m_link_list[i];
int dof = bodyi->dof;
if (dof != 0)
{
if (dof == 6)
{
posDesJoint[i].setZero(3); // linear position of FB
}
else if (dof ==3)
{
// Do nothing, since only uses RDes
}
else if (dof == 1)
{
posDesJoint[i].setZero(1);
}
rateDesJoint[i].setZero(dof);
accDesJoint[i].setZero(dof);
}
}
}
HumanoidController::HumanoidController(dmArticulation * robot) : TaskSpaceControllerA(robot) {
q.resize(STATE_SIZE);
qdDm.resize(STATE_SIZE);
qd.resize(RATE_SIZE);
SupportIndices.resize(NS);
SupportIndices[0] = 5;
SupportIndices[1] = 9;
contactState.resize(NS);
slidingState.resize(NS);
Matrix3F RSup;
RSup << 0,0,1,0,1,0,-1,0,0;
XformVector Xforms(NS);
for (int k=0; k<NS; k++) {
Xforms[k].resize(6,6);
dmRigidBody * link = (dmRigidBody*) artic->getLink(SupportIndices[k]);
dmContactModel * dmContactLattice = (dmContactModel *) link->getForce(0);
Vector3F pRel;
dmContactLattice->getContactPoint(0,pRel.data());
pRel(1) = 0; pRel(2) = 0;
Xforms[k].block(0,0,3,3) = RSup;
Xforms[k].block(0,3,3,3) = Matrix3F::Zero();
Xforms[k].block(3,0,3,3) = -RSup*cr3(pRel);
Xforms[k].block(3,3,3,3) = RSup;
}
SupportXforms = Xforms;
// Construct Point Force Transform
PointForceXforms.resize(NS);
for (int i=0; i<NS; i++) {
PointForceXforms[i].resize(NJ);
//cout << "Getting forces for link " << SupportIndices[i] << endl;
dmRigidBody * linki = (dmRigidBody*) artic->m_link_list[SupportIndices[i]]->link;
dmContactModel * dmContactLattice = (dmContactModel *) linki->getForce(0);
//cout << "Assigning Temporary Matricies" << endl;
Matrix3F RSup = SupportXforms[i].block(0,0,3,3);
Matrix3F tmpMat = SupportXforms[i].block(3,0,3,3)*RSup.transpose();
Vector3F piRelSup;
crossExtract(tmpMat,piRelSup);
//cout << "pirelsup " << endl << piRelSup << endl;
for (int j=0; j<NP; j++) {
Vector3F pRel, tmp;
//Tmp is now the contact point location relative to the body coordinate
dmContactLattice->getContactPoint(j,tmp.data());
// Point of contact (relative to support origin) in support coordinates
pRel = RSup*tmp + piRelSup;
PointForceXforms[i][j].setIdentity(6,6);
PointForceXforms[i][j].block(0,3,3,3) = cr3(pRel);
}
}
grfInfo.localContacts = 0;
pFoot.resize(NS);
pDesFoot.resize(NS);
vFoot.resize(NS);
vDesFoot.resize(NS);
RFoot.resize(NS);
RDesFoot.resize(NS);
aFoot.resize(NS);
aDesFoot.resize(NS);
kpFoot.resize(NS);
kdFoot.resize(NS);
for (int i=0; i<NS; i++) {
pFoot[i].resize(3);
pDesFoot[i].resize(3);
vFoot[i].resize(6);
vDesFoot[i].resize(6);
aFoot[i].resize(6);
aDesFoot[i].resize(6);
aDesFoot[i].setZero();
vDesFoot[i].setZero();
pDesFoot[i].setZero();
RDesFoot[i].setZero();
kpFoot[i] = 0;
kdFoot[i] = 0;
}
//ComTrajectory.setSize(3);
//aDesFoot.resize(NS);
//pDesFoot.resize(NS);
//vDesFoot.resize(NS);
//RDesFoot.resize(NS);
aComDes.resize(3);
aComDes.setZero();
vComDes.resize(3);
vComDes.setZero();
pComDes.resize(3);
pComDes.setZero();
kComDes.resize(3);
kComDes.setZero();
}