void printStatus()
{
if (t-t1>=PRINT_STATUS_PER)
{
// we get the current fixation point in the
// operational space
Vector x;
igaze->getFixationPoint(x);
fprintf(stdout,"+++++++++\n");
fprintf(stdout,"fp [m] = (%s)\n",fp.toString(3,3).c_str());
fprintf(stdout,"x [m] = (%s)\n",x.toString(3,3).c_str());
fprintf(stdout,"norm(fp-x) [m] = %g\n",norm(fp-x));
fprintf(stdout,"---------\n\n");
t1=t;
}
}
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++;
}
}