void storeInterestingPoint()
{
if (t-t3>=STORE_POI_PER)
{
Vector ang;
// we store the current azimuth, elevation
// and vergence wrt the absolute reference frame
// The absolute reference frame for the azimuth/elevation couple
// is head-centered with the robot in rest configuration
// (i.e. torso and head angles zeroed).
igaze->getAngles(ang);
fprintf(stdout,"Storing POI #%d ... %s [deg]\n",
poiList.size(),ang.toString().c_str());
poiList.push_back(ang);
t3=t;
}
}
// the motion-done callback
virtual void gazeEventCallback()
{
Vector ang;
igaze->getAngles(ang);
fprintf(stdout,"Actual gaze configuration: (%s) [deg]\n",
ang.toString(3,3).c_str());
fprintf(stdout,"Moving the torso; check if the gaze is compensated ...\n");
// move the torso yaw
double val;
ienc->getEncoder(0,&val);
ipos->positionMove(0,val>0.0?-30.0:30.0);
t4=t;
// detach the callback
igaze->unregisterEvent(*this);
// switch state
state=STATE_STILL;
}
virtual void run(){
tmp = command;
(*command)[0] = -60*(gsl_rng_uniform(r));
(*command)[1] = 100*(gsl_rng_uniform(r));
(*command)[2] = -35 + 95*(gsl_rng_uniform(r));
(*command)[3] = 10 + 90*(gsl_rng_uniform(r));
printf("%.1lf %.1lf %.1lf %.1lf\n", (*command)[0], (*command)[1], (*command)[2], (*command)[3]);
//above 0 doesn't seem to be safe for joint 0
if ((*command)[0] > 0 || (*command)[0] < -60){
(*command)[0] = (*tmp)[0];
}
if ((*command)[1] > 100 || (*command)[1] < -0){
(*command)[1] = (*tmp)[1];
}
if ((*command)[2] > 60 || (*command)[2] < -35){
(*command)[2] = (*tmp)[2];
}
if ((*command)[3] > 100 || (*command)[3] < 10){
(*command)[3] = (*tmp)[3];
}
//use fwd kin to find end effector position
Bottle plan, pred;
for (int i = 0; i < nj; i++){
plan.add((*command)[i]);
}
armPlan->write(plan);
armPred->read(pred);
Vector commandCart(3);
for (int i = 0; i < 3; i++){
commandCart[i] = pred.get(i).asDouble();
}
printf("Cartesian safety check\n");
double rad = sqrt(commandCart[0]*commandCart[0]+commandCart[1]*commandCart[1]);
// safety radius back to 30 cm
if (rad > 0.3){
pos->positionMove(command->data());
bool done = false;
while(!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
printf("Moved arm to new location\n");
Vector &armJ = armLocJ->prepare();
Vector encoders(nj);
enc->getEncoders(encoders.data());
armJ = encoders;
Vector noisyArm(3);
for(int i = 0; i < 3; i++){
//noisyArm[i] = commandCart[i] + 0.01*(2*gsl_rng_uniform(r)-1);
//sanity check
noisyArm[i] = commandCart[i] + 0.005*(2*gsl_rng_uniform(r)-1);
}
//insert here:
//read off peak saliences
//fixate there
//calculate cartesian value, compare to real cart. value of arm
printf("Looking at arm\n");
igaze->lookAtFixationPoint(noisyArm);
done = false;
while(!done){
igaze->checkMotionDone(&done);
Time::delay(0.5);
}
//igaze->waitMotionDone(0.1,30);
printf("Saw arm\n");
Vector &headAng = headLoc->prepare();
igaze->getAngles(headAng);
Bottle tStamp;
tStamp.clear();
tStamp.add(Time::now());
headLoc->setEnvelope(tStamp);
headLoc->write();
armLocJ->write();
headLoc->unprepare();
armLocJ->unprepare();
}
else{
printf("Self collision detected!\n");
}
}
virtual void run()
{
t=Time::now();
generateTarget();
if (state==STATE_TRACK)
{
// look at the target (streaming)
igaze->lookAtFixationPoint(fp);
// some verbosity
printStatus();
// we collect from time to time
// some interesting points (POI)
// where to look at soon afterwards
storeInterestingPoint();
if (t-t2>=SWITCH_STATE_PER)
{
// switch state
state=STATE_RECALL;
}
}
if (state==STATE_RECALL)
{
// pick up the first POI
// and clear the list
Vector ang=poiList.front();
poiList.clear();
fprintf(stdout,"Retrieving POI #0 ... %s [deg]\n",
ang.toString().c_str());
// look at the chosen POI
igaze->lookAtAbsAngles(ang);
// switch state
state=STATE_WAIT;
}
if (state==STATE_WAIT)
{
bool done=false;
igaze->checkMotionDone(&done);
if (done)
{
Vector ang;
igaze->getAngles(ang);
fprintf(stdout,"Actual gaze configuration: %s [deg]\n",
ang.toString().c_str());
/* pramod modified starts
fprintf(stdout,"Moving the torso; see if the gaze is compensated ... ");
// move the torso yaw
double val;
ienc->getEncoder(0,&val);
ipos->positionMove(0,val>0.0?-30.0:30.0);
pramod modified ends */
t4=t;
// switch state
state=STATE_STILL;
}
}
if (state==STATE_STILL)
{
if (t-t4>=STILL_STATE_TIME)
{
fprintf(stdout,"done\n");
t1=t2=t3=t;
// switch state
state=STATE_TRACK;
}
}
}