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++;
}
}
