Esempio n. 1
0
void InputSettingsWindow::assign() {
  QTreeWidgetItem *item = list->currentItem();
  if(!item) return;
  signed index = item->data(0, Qt::UserRole).toInt();
  if(index == -1) return assignGroup();
  setActiveInput(inputTable[index]);
}
void HumanoidDataLogger::logData() {
	
	dataMutex.Lock();
	newRecord();
	assignItem(TIME, simThread->sim_time);
	assignItem(STATE_CODE, state);
	assignItem(CONTROL_TIME, controlTime);
	
	assignGroup(JOINT_ANGLES, q);
	assignGroup(JOINT_RATES, qd);
	assignGroup(JOINT_TORQUES, tau);
	Vector6F force;
	
	force.head(3) = grfInfo.fCoPs[0];
	force.segment(3,2) = grfInfo.pCoPs[0].head(2);
	force(5)     = grfInfo.nCoPs[0];
	assignGroup(RIGHT_FOOT_WRENCH, force);
	assignItem(RCONT_STATE, contactState[0]);
	assignItem(RSLIDE_STATE, slidingState[0]);
	
	force.head(3) = grfInfo.fCoPs[1];
	force.segment(3,2) = grfInfo.pCoPs[1].head(2);
	force(5)     = grfInfo.nCoPs[1];
	assignGroup(LEFT_FOOT_WRENCH, force);
	assignItem(LCONT_STATE, contactState[1]);
	assignItem(LSLIDE_STATE, slidingState[1]);
	
	force.head(3) = grfInfo.fZMP;
	force.segment(3,2) = grfInfo.pZMP.head(2);
	force(5)     = grfInfo.nZMP;
	assignGroup(ZMP_WRENCH, force);
	
	assignGroup(ZMP_WRENCH_OPT, zmpWrenchOpt);
	assignGroup(ZMP_POS_OPT, zmpPosOpt);
	
	assignGroup(COM_POSITION, pCom);
	assignGroup(COM_POSITION_DES, pComDes);
	assignGroup(COM_VELOCITY, vCom);
	assignGroup(COM_VELOCITY_DES, vComDes);
	
	assignGroup(CENTROIDAL_MOMENTUM, centMom);
	assignGroup(HDES, hDes);
	
	assignGroup(HDOT_DES, hDotDes);
	assignGroup(HDOT_OPT, hDotOpt);
	
	assignGroup(QDD_OPT, qdd);
	assignGroup(QDD_ACT, qddA);
	
	
	assignGroup(RIGHT_FOOT_POS, pFoot[0]);
	assignGroup(RIGHT_FOOT_POS_DES, pDesFoot[0]);
	
	assignGroup(RIGHT_FOOT_VEL, vFoot[0]);
	assignGroup(RIGHT_FOOT_VEL_DES, vDesFoot[0]);
	
	assignGroup(RIGHT_FOOT_ACC, aFoot[0]);
	assignGroup(RIGHT_FOOT_ACC_DES, aDesFoot[0]);
	
	assignGroup(LEFT_FOOT_POS, pFoot[1]);
	assignGroup(LEFT_FOOT_POS_DES, pDesFoot[1]);
	
	assignGroup(LEFT_FOOT_VEL, vFoot[1]);
	assignGroup(LEFT_FOOT_VEL_DES, vDesFoot[1]);
	
	assignGroup(LEFT_FOOT_ACC, aFoot[1]);
	assignGroup(LEFT_FOOT_ACC_DES, aDesFoot[1]);
	
	assignGroup(RWRENCH_OPT, fs.head(6));
	assignGroup(LWRENCH_OPT, fs.tail(6));
	
	assignMatrixGroup(JRF, grfInfo.footJacs[0]);
	assignMatrixGroup(JLF, grfInfo.footJacs[1]);
	
	assignGroup(RIGHT_FOOT_SPATIAL_WRENCH, grfInfo.footWrenches[0]);
	assignGroup(LEFT_FOOT_SPATIAL_WRENCH, grfInfo.footWrenches[1]);
	
	assignGroup(CANDG, artic->CandG);
	assignMatrixGroup(HMAT,artic->H);
	dataMutex.Unlock();
}