void home() { // these values should be loaded from a config file // for now we just put them in the code Vector x(3); x[0] = -0.3; x[1] = 0.4; x[2] = 0.2; iarm->goToPosition(x); // look down for the homing position look_down(); }
void armRest() { if (arm!="none") { Vector x(3); x[0]=REACH_REST_X; x[1]=REACH_REST_Y; x[2]=REACH_REST_Z; if (arm=="left") x[1]=-x[1]; iarm->goToPosition(x); armCmdState=0; } }
void armTrack() { if (arm!="none") { if (ARMISTRACKING(armCmdState)) { Vector fp; igaze->getFixationPoint(fp); if (fp[0]>REACH_X_MAX) fp[0]=REACH_X_MAX; fp[2]+=REACH_OFFS_Z; iarm->goToPosition(fp); fprintf(stdout,"Reaching for: (%.1f,%.1f,%.1f) m\n",fp[0],fp[1],fp[2]); } else armCmdState++; } }