Example #1
0
 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);
}