BOOL ComputeNewStrip(LPMBLARESTATE State){
#define VSSUB(X, Y, Z) Z.x=Y.x-X.x; Z.y=Y.y-X.y; Z.z=Y.z-X.z
VSSUB(State->LastPoints[0], State->LastPoints[1], CV1);
VSSUB(State->LastPoints[1], CV, CV2);
if (State->NoTors){
CV3.x=CameraMat._13;
CV3.y=CameraMat._23;
CV3.z=CameraMat._33;
}
else{
CROSS_PRODUCT(CV1, CV2, CV3);
}
if ((MODULO3D((&CV3))<MIN_SCALE*(MODULO3D(&CV1)*MODULO3D(&CV2)))) {
;
}
else{
CROSS_PRODUCT(CV3, CV2, (State->CV1));
NORMALIZE3D(&(State->CV1));
}
State->Vertices[State->VertexDrawn].x=State->CV1.x*State->StripWidth;
State->Vertices[State->VertexDrawn].y=State->CV1.y*State->StripWidth;
State->Vertices[State->VertexDrawn].z=State->CV1.z*State->StripWidth;
State->Vertices[State->VertexDrawn].color=State->Color;
State->Vertices[State->VertexDrawn+1].x=
-State->Vertices[State->VertexDrawn].x;
State->Vertices[State->VertexDrawn+1].y=
-State->Vertices[State->VertexDrawn].y;
State->Vertices[State->VertexDrawn+1].z=
-State->Vertices[State->VertexDrawn].z;
State->Vertices[State->VertexDrawn+1].color=State->Color;
if (State->VertexDrawn<=2){
memcpy((char *)State->Vertices, ((char *)State->Vertices)+2*sizeof(D3DLVERTEX),
2*sizeof(D3DLVERTEX));
State->Vertices[0].x+=State->LastPoints[0].x;
State->Vertices[0].y+=State->LastPoints[0].y;
State->Vertices[0].z+=State->LastPoints[0].z;
State->Vertices[1].x+=State->LastPoints[0].x;
State->Vertices[1].y+=State->LastPoints[0].y;
State->Vertices[1].z+=State->LastPoints[0].z;
}
State->Vertices[State->VertexDrawn].x+=CV.x;
State->Vertices[State->VertexDrawn].y+=CV.y;
State->Vertices[State->VertexDrawn].z+=CV.z;
State->Vertices[State->VertexDrawn+1].x+=CV.x;
State->Vertices[State->VertexDrawn+1].y+=CV.y;
State->Vertices[State->VertexDrawn+1].z+=CV.z;
return TRUE;
}
/*
* La procedure "plane_norme" calcule le vecteur norme orthogonal au plan
* defini par les 3 points.
* Entree :
* np Le vecteur norme orthogonal au plan.
* ap, bp, cp Points formant un repere du plan.
*/
void
plane_norme (Vector *np, Point3f *ap, Point3f *bp, Point3f *cp)
{
Vector u, v;
DIF_COORD3(u,*bp,*ap); /* base orthonorme (ap, u, v) */
DIF_COORD3(v,*cp,*ap);
norm_vector (&u);
norm_vector (&v);
CROSS_PRODUCT (*np,u,v);
}
/*
* La procedure "rotate_vector" transforme le vecteur
* par la rotation de sens trigonometrique d'angle et d'axe donnes.
* Entree :
* vp Vecteur a transformer.
* a Angle de rotation en degres.
* axis Vecteur directeur de l'axe de rotation.
*/
void
rotate_vector (Vector *vp, float a, Vector *axis)
{
Vector n, u, v, cross;
float f;
a *= (float)M_PI / (float)180.0; /* passage en radians */
n = *axis; /* norme le vecteur directeur */
norm_vector (&n);
/*
* Avant rotation, vp vaut :
* u + v
* Apres rotation, vp vaut :
* u + cos(a) * v + sin(a) * (n^vp)
* = u + cos(a) * v + sin(a) * (n^v)
* avec u = (vp.n) * n, v = vp-u;
* ou "u" est la projection de "vp" sur l'axe "axis",
* et "v" est la composante de "vp" perpendiculaire a "axis".
*/
f = DOT_PRODUCT(*vp, n);
u = n;
MUL_COORD3(u,f,f,f); /* (vp.n) * n */
DIF_COORD3(v,*vp,u); /* calcule "v" */
f = (float) cos ((double) a);
MUL_COORD3(v,f,f,f); /* v * cos(a) */
CROSS_PRODUCT(cross,n,*vp);
f = (float) sin ((double) a);
MUL_COORD3(cross,f,f,f); /* (n^v) * sin(a) */
SET_COORD3(*vp,
u.x + v.x + cross.x,
u.y + v.y + cross.y,
u.z + v.z + cross.z);
}
//! 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);
}
void HumanoidController::HumanoidControl()
{
// Perform Optimization
{
UpdateConstraintMatrix();
int maxPriorityLevels = OptimizationSchedule.maxCoeff();
const int numTasks = OptimizationSchedule.size();
if (maxPriorityLevels > 0)
{
for (int level=1; level<=maxPriorityLevels; level++)
{
taskConstrActive.setZero(numTasks);
taskOptimActive.setZero(numTasks);
bool runOpt = false;
for (int i=0; i<numTasks;i ++)
{
if (OptimizationSchedule(i) == level)
{
taskOptimActive(i) = 1;
runOpt = true;
}
else if ((OptimizationSchedule(i) < level) && (OptimizationSchedule(i) > -1))
{
taskConstrActive(i) = 1;
}
}
if (runOpt)
{
UpdateObjective();
UpdateHPTConstraintBounds();
//cout << "Optimizing level " << level << endl;
Optimize();
for (int i=0; i<numTasks;i ++)
{
if (OptimizationSchedule(i) == level)
{
TaskBias(i) += TaskError(i);
//cout << "Optimization Level " << level << " task error " << i << " = " << TaskError(i) << endl;
}
}
}
}
}
/// Compute optimal quantities
// desired change of centroidal momentum
hDotOpt = CentMomMat*qdd + cmBias;
// Desired ZMP info
zmpWrenchOpt.setZero();
Vector6F icsForce, localForce;
Float * nICS = icsForce.data(); // ICS
Float * fICS = nICS+3;
Float * nLoc = localForce.data(); // Local
Float * fLoc = nLoc+3;
for (int k1 = 0; k1 < NS; k1++)
{
LinkInfoStruct * listruct = artic->m_link_list[SupportIndices[k1]];
CartesianVector tmp;
localForce = SupportXforms[k1].transpose()*fs.segment(6*k1,6);
// Apply Spatial Force Transform Efficiently
// Rotate Quantities
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,nLoc,nICS);
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,fLoc,fICS);
// Add the r cross f
CROSS_PRODUCT(listruct->link_val2.p_ICS,fICS,tmp);
ADD_CARTESIAN_VECTOR(nICS,tmp);
zmpWrenchOpt+=icsForce;
}
transformToZMP(zmpWrenchOpt,zmpPosOpt);
// Form Joint Input and simulate
VectorXF jointInput = VectorXF::Zero(STATE_SIZE);
int k = 7;
// Skip over floating base (i=1 initially)
for (int i=1; i<artic->m_link_list.size(); i++)
{
LinkInfoStruct * linki = artic->m_link_list[i];
if (linki->dof)
{
//cout << "Link " << i << " dof = " << linki->dof << endl;
//cout << "qd = " << qdDm.segment(k,linki->dof).transpose() << endl;
jointInput.segment(k,linki->dof) = tau.segment(linki->index_ext-6,linki->dof);
k+=linki->link->getNumDOFs();
}
}
//cout << "Tau = " << tau.transpose() << endl;
//cout << "Joint input = " << jointInput.transpose() << endl;
//exit(-1);
//jointInput.segment(7,NJ) = tau;
artic->setJointInput(jointInput.data());
/// verification
ComputeActualQdd(qddA);
}
#ifdef CONTROL_DEBUG
// Debug Code
{
}
#endif
}
void HumanoidController::HumanoidControl(ControlInfo & ci) {
int taskRow = 0;
Float discountFactor = 1;
dmTimespec tv1, tv2, tv3, tv4;
//Update Graphics Variables
{
ComPos[0] = pCom(0);
ComPos[1] = pCom(1);
ComPos[2] = pCom(2);
ComDes[0] = pComDes(0);
ComDes[1] = pComDes(1);
ComDes[2] = pComDes(2);
}
// Perform Optimization
{
dmGetSysTime(&tv2);
UpdateConstraintMatrix();
int maxPriorityLevels = OptimizationSchedule.maxCoeff();
const int numTasks = OptimizationSchedule.size();
if (maxPriorityLevels > 0) {
for (int level=1; level<=maxPriorityLevels; level++) {
taskConstrActive.setZero(numTasks);
taskOptimActive.setZero(numTasks);
bool runOpt = false;
for (int i=0; i<numTasks;i ++) {
if (OptimizationSchedule(i) == level) {
taskOptimActive(i) = 1;
runOpt = true;
}
else if ((OptimizationSchedule(i) < level) && (OptimizationSchedule(i) > -1)) {
taskConstrActive(i) = 1;
}
}
if (runOpt) {
UpdateObjective();
UpdateHPTConstraintBounds();
dmGetSysTime(&tv3);
//cout << "Optimizing level " << level << endl;
Optimize();
for (int i=0; i<numTasks;i ++) {
if (OptimizationSchedule(i) == level) {
TaskBias(i) += TaskError(i);
//cout << "Optimization Level " << level << " task error " << i << " = " << TaskError(i) << endl;
}
}
}
}
}
else {
UpdateObjective();
UpdateHPTConstraintBounds();
dmGetSysTime(&tv3);
Optimize();
}
//exit(-1);
// Extract Results
hDotOpt = CentMomMat*qdd + cmBias;
// Form Joint Input and simulate
VectorXF jointInput = VectorXF::Zero(STATE_SIZE);
// Extact Desired ZMP info
zmpWrenchOpt.setZero();
Vector6F icsForce, localForce;
Float * nICS = icsForce.data(), * nLoc = localForce.data();
Float * fICS = nICS+3, * fLoc = nLoc+3;
for (int k1 = 0; k1 < NS; k1++) {
LinkInfoStruct * listruct = artic->m_link_list[SupportIndices[k1]];
CartesianVector tmp;
localForce = SupportXforms[k1].transpose()*fs.segment(6*k1,6);
// Apply Spatial Force Transform Efficiently
// Rotate Quantities
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,nLoc,nICS);
APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,fLoc,fICS);
// Add the r cross f
CROSS_PRODUCT(listruct->link_val2.p_ICS,fICS,tmp);
ADD_CARTESIAN_VECTOR(nICS,tmp);
zmpWrenchOpt+=icsForce;
}
transformToZMP(zmpWrenchOpt,zmpPosOpt);
int k = 7;
// Skip over floating base (i=1 initially)
for (int i=1; i<artic->m_link_list.size(); i++) {
LinkInfoStruct * linki = artic->m_link_list[i];
if (linki->dof) {
//cout << "Link " << i << " dof = " << linki->dof << endl;
//cout << "qd = " << qdDm.segment(k,linki->dof).transpose() << endl;
jointInput.segment(k,linki->dof) = tau.segment(linki->index_ext-6,linki->dof);
k+=linki->link->getNumDOFs();
}
}
//cout << "Tau = " << tau.transpose() << endl;
//cout << "Joint input = " << jointInput.transpose() << endl;
//exit(-1);
//jointInput.segment(7,NJ) = tau;
artic->setJointInput(jointInput.data());
ComputeActualQdd(qddA);
dmGetSysTime(&tv4);
}
//Populate Control Info Struct
{
ci.calcTime = timeDiff(tv1,tv2);
ci.setupTime = timeDiff(tv2,tv3);
ci.optimTime = timeDiff(tv3,tv4);
ci.totalTime = timeDiff(tv1,tv4);
ci.iter = iter;
}
#ifdef CONTROL_DEBUG
// Debug Code
{
cout << "q " << q.transpose() << endl;
cout << "qd " << qdDm.transpose() << endl;
cout << "qd2 " << qd.transpose() << endl;
cout << "Task Bias " << TaskBias << endl;
//cout << "H = " << endl << artic->H << endl;
cout << "CandG = " << endl << artic->CandG.transpose() << endl;
if (simThread->sim_time > 0) {
cout << setprecision(5);
MSKboundkeye key;
double bl,bu;
for (int i=0; i<numCon; i++) {
MSK_getbound(task, MSK_ACC_VAR, i, &key, &bl, &bu);
cout << "i = " << i;
switch (key) {
case MSK_BK_FR:
cout << " Free " << endl;
break;
case MSK_BK_LO:
cout << " Lower Bound " << endl;
break;
case MSK_BK_UP:
cout << " Upper Bound " << endl;
break;
case MSK_BK_FX:
cout << " Fixed " << endl;
break;
case MSK_BK_RA:
cout << " Ranged " << endl;
break;
default:
cout << " Not sure(" << key << ")" << endl;
break;
}
cout << bl << " to " << bu << endl;
}
cout << "x(57) = " << xx(57) << endl;
cout << "tau = " << tau.transpose() << endl;
cout << "qdd = " << qdd.transpose() << endl;
cout << "fs = " << fs.transpose() << endl;
//cout << "lambda = " << lambda.transpose() << endl;
VectorXF a = TaskJacobian * qdd;
//cout << "a" << endl;
VectorXF e = TaskJacobian * qdd - TaskBias;
cout << "e = " << e.transpose() << endl;
MatrixXF H = artic->H;
VectorXF CandG = artic->CandG;
MatrixXF S = MatrixXF::Zero(NJ,NJ+6);
S.block(0,6,NJ,NJ) = MatrixXF::Identity(NJ,NJ);
VectorXF generalizedContactForce = VectorXF::Zero(NJ+6);
for (int i=0; i<NS; i++) {
generalizedContactForce += SupportJacobians[i].transpose()*fs.segment(6*i,6);
}
VectorXF qdd2 = H.inverse()*(S.transpose() * tau + generalizedContactForce- CandG);
//cout << "qdd2 = " << qdd2.transpose() << endl << endl << endl;
//cout << "CandG " << CandG.transpose() << endl;
//cout << "Gen Contact Force " << generalizedContactForce.transpose() << endl;
cout << "hdot " << (CentMomMat*qdd + cmBias).transpose() << endl;
cout << "cmBias " << cmBias.transpose() << endl;
cout << "qd " << qd.transpose() << endl;
//VectorXF qdd3 = H.inverse()*(S.transpose() * tau - CandG);
//FullPivHouseholderQR<MatrixXF> fact(H);
cout <<"fNet \t" << (fs.segment(3,3) + fs.segment(9,3)).transpose() << endl;
Vector3F g;
g << 0,0,-9.81;
cout <<"hdot - mg\t" << (CentMomMat*qdd + cmBias).segment(3,3).transpose() - totalMass * g.transpose()<< endl;
exit(-1);
}
// Old Debug Code
{
VectorXF generalizedContactForce = VectorXF::Zero(NJ+6);
Matrix6F X;
MatrixXF Jac;
X.setIdentity();
for (int i=0; i<NS; i++) {
int linkIndex = SupportIndices[i];
artic->computeJacobian(linkIndex,X,Jac);
dmRigidBody * link = (dmRigidBody *) artic->getLink(linkIndex);
for (int j=0; j< link->getNumForces(); j++) {
dmForce * f = link->getForce(j);
Vector6F fContact;
f->computeForce(artic->m_link_list[linkIndex]->link_val2,fContact.data());
generalizedContactForce += Jac.transpose()*fContact;
}
}
cout << "J' f = " << generalizedContactForce.transpose() << endl;
VectorXF qdd3 = H.inverse()*(S.transpose() * tau - CandG + generalizedContactForce);
cout << "qdd3 = " << qdd3.transpose() << endl;
VectorXF state = VectorXF::Zero(2*(NJ+7));
state.segment(0,NJ+7) = q;
state.segment(NJ+7,NJ+7) = qdDm;
VectorXF stateDot = VectorXF::Zero(2*(NJ+7));
//Process qdds
artic->dynamics(state.data(),stateDot.data());
//
VectorXF qdds = VectorXF::Zero(NJ+6);
qdds.segment(0,6) = stateDot.segment(NJ+7,6);
//cout << "w x v " << cr3(qd.segment(0,3))*qd.segment(3,3) << endl;
qdds.segment(3,3) -= cr3(qdDm.segment(0,3))*qdDm.segment(3,3);
qdds.segment(6,NJ) = stateDot.segment(NJ+7*2,NJ);
Matrix3F ics_R_fb;
copyRtoMat(artic->m_link_list[0]->link_val2.R_ICS,ics_R_fb);
qdds.head(3) = ics_R_fb.transpose() * qdds.head(3);
qdds.segment(3,3) = ics_R_fb.transpose() * qdds.segment(3,3);
cout << "qdds = " << qdds.transpose() << endl;
//cout << "CandG " << endl << CandG << endl;
//cout << setprecision(6);
//cout << "I_0^C = " << endl << artic->H.block(0,0,6,6) << endl;
//exit(-1);
}
}
#endif
/*{
MatrixXF H = artic->H;
VectorXF CandG = artic->CandG;
MatrixXF S = MatrixXF::Zero(NJ,NJ+6);
S.block(0,6,NJ,NJ) = MatrixXF::Identity(NJ,NJ);
VectorXF generalizedContactForce = VectorXF::Zero(NJ+6);
for (int i=0; i<NS; i++) {
generalizedContactForce += SupportJacobians[i].transpose()*fs.segment(6*i,6);
}
qdd = H.inverse()*(S.transpose() * tau + generalizedContactForce- CandG);
cout << setprecision(8);
cout << "fs " << endl << fs << endl;
cout << "qdd " << endl << qdd << endl;
}
exit(-1);*/
static int numTimes = 0;
numTimes++;
//Float dummy;
//cin >> dummy;
//if (numTimes == 2) {
// exit(-1);
//}
//exit(-1);
}
