示例#1
0
    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;
        }
    }
示例#2
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++;
     }
 }