bool match(HashGrid::Ptr grid, const Vector3F& v) {
VectorI candidates = grid->get_items_near_point(v);
if (candidates.size() != 1) {
std::cout << "<" << v.transpose() << "> : " << candidates.transpose()
<< std::endl;
}
return candidates.size() > 0;
}
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);
}