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(3,3).c_str());
// register the motion-done event, attaching the callback
// that will be executed as soon as the gaze is accomplished
igaze->registerEvent(*this);
// look at the chosen POI
igaze->lookAtAbsAngles(ang);
// switch state
state=STATE_WAIT;
}
if (state==STATE_STILL)
{
if (t-t4>=STILL_STATE_TIME)
{
fprintf(stdout,"done\n");
t1=t2=t3=t;
// switch state
state=STATE_TRACK;
}
}
}
void look_down()
{
// look at 0.5 meter in front of the robot
Vector pos(3);
pos[0] = -0.3;
pos[1] = 0.0;
pos[2] = 0.0;
igaze->lookAtFixationPoint(pos);
}
virtual void run() {
while (isStopping() != true) {
/* poll the click ports containers to see if we have left/right ready to go */
bfL.lock(); bfR.lock();
if (bfL.size() == 2 && bfR.size() == 2) {
printf("got a hit!\n");
/* if they are, raise the flag that action is beginning, save current joint configuration */
Bottle susmsg;
susmsg.addInt(1);
susPort->write(susmsg);
//get the current joint configuration for the torso, head, and arm
tang.clear(); tang.resize(3);
tEnc->getEncoders(tang.data());
hang.clear(); hang.resize(6);
hEnc->getEncoders(hang.data());
aang.clear(); aang.resize(16);
aEnc->getEncoders(aang.data());
/* get the xyz location of the gaze point */
Vector bvL(2); Vector bvR(2);
bvL[0] = bfL.get(0).asDouble(); bvL[1] = bfL.get(1).asDouble();
bvR[0] = bfR.get(0).asDouble(); bvR[1] = bfR.get(1).asDouble();
objPos.clear(); objPos.resize(3);
gaze->triangulate3DPoint(bvL,bvR,objPos);
/* servo the head to gaze at that point */
//gaze->lookAtStereoPixels(bvL,bvR);
gaze->lookAtFixationPoint(objPos);
gaze->waitMotionDone(1.0,10.0);
gaze->stopControl();
printf("object position estimated as: %f, %f, %f\n", objPos[0], objPos[1], objPos[2]);
printf("is this ok?\n");
string posResp = Network::readString().c_str();
if (posResp == "yes" || posResp == "y") {
/* move to hover the hand over the XY position of the target: [X, Y, Z=0.2], with palm upright */
objPos[2] = 0.1;
Vector axa(4);
axa.zero();
if (armInUse) {
axa[2] = 1.0; axa[3] = M_PI;
} else {
axa[1] = 1.0; axa[3] = M_PI;
}
carm->goToPoseSync(objPos,axa);
carm->waitMotionDone(1.0,10.0);
Time::delay(2.0);
//curl fingers and thumb slightly to hold object
Vector armCur(16);
aEnc->getEncoders(armCur.data());
armCur[8] = 3; armCur[10] = 25;
armCur[11] = 25; armCur[12] = 25;
armCur[13] = 25; armCur[14] = 25;
armCur[15] = 55;
aPos->positionMove(armCur.data());
Time::delay(2.0);
/* wait for terminal signal from user that object has been moved to the hand */
bool validTarg = false;
printf("object position reached, place in hand and enter target xy position\n");
while (!validTarg) {
string objResp = Network::readString().c_str();
/* ask the user to enter in an XY target location, or confirm use of previous one */
Bottle btarPos(objResp.c_str());
if (btarPos.size() < 2) {
//if user enters no target position, try to use last entered position
if (targetPos.length() != 3) {
printf("no previous target position available, please re-enter:\n");
} else {
validTarg = true;
}
} else {
targetPos.clear(); targetPos.resize(3);
targetPos[0] = btarPos.get(0).asDouble();
targetPos[1] = btarPos.get(1).asDouble();
targetPos[2] = 0.1;
validTarg = true;
}
}
/* move the arm to above the target location */
axa.zero();
if (armInUse) {
axa[2] = 1.0; axa[3] = M_PI;
} else {
axa[1] = 1.0; axa[3] = M_PI;
}
carm->goToPoseSync(targetPos,axa);
//carm->goToPosition(targetPos);
carm->waitMotionDone(1.0,10.0);
Time::delay(2.0);
/* wait for user signal that the object has been removed */
printf("object has been moved to target location. please remove object and hit enter\n");
string tarResp = Network::readString().c_str();
}
/* return to saved motor configuration, clear click buffers, lower flag signaling action done */
printf("gaze done, attempting reset\n");
tPos->positionMove(tang.data());
hPos->positionMove(hang.data());
aPos->positionMove(aang.data());
bfL.clear(); bfR.clear();
bfL.unlock(); bfR.unlock();
susmsg.clear();
susmsg.addInt(0);
susPort->write(susmsg);
}
else {
bfL.unlock(); bfR.unlock();
}
}
}
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");
}
}
void run()
{
Vector x,o;
action->getPose(x,o);
igaze->lookAtFixationPoint(x+offset);
}
void fixate(const Vector &x)
{
igaze->lookAtFixationPoint(x);
}
void reachingHandler(const bool b, const Vector &pos,
const Vector &rpy)
{
if (b)
{
if (s0==idle)
s0=triggered;
else if (s0==triggered)
{
if (++c0*getPeriod()>0.5)
{
pos0[0]=pos[0];
pos0[1]=pos[1];
pos0[2]=pos[2];
rpy0[0]=rpy[0];
rpy0[1]=rpy[1];
rpy0[2]=rpy[2];
iarm->getPose(x0,o0);
s0=running;
}
}
else
{
Vector xd(4,0.0);
xd[0]=pos[0]-pos0[0];
xd[1]=pos[1]-pos0[1];
xd[2]=pos[2]-pos0[2];
xd[3]=1.0;
Matrix H0=eye(4,4);
H0(0,3)=x0[0];
H0(1,3)=x0[1];
H0(2,3)=x0[2];
xd=H0*xd;
Matrix Rd;
if (onlyXYZ)
Rd=axis2dcm(o0);
else
{
Vector drpy(3);
drpy[0]=rpy[0]-rpy0[0];
drpy[1]=rpy[1]-rpy0[1];
drpy[2]=rpy[2]-rpy0[2];
Vector ax(4,0.0),ay(4,0.0),az(4,0.0);
ax[0]=1.0; ax[3]=drpy[2];
ay[1]=1.0; ay[3]=drpy[1]*((part=="left_arm")?1.0:-1.0);
az[2]=1.0; az[3]=drpy[0]*((part=="left_arm")?1.0:-1.0);
Rd=axis2dcm(o0)*axis2dcm(ax)*axis2dcm(ay)*axis2dcm(az);
}
Vector od=dcm2axis(Rd);
iarm->goToPose(xd,od);
if (gaze)
igaze->lookAtFixationPoint(xd);
yInfo("going to (%s) (%s)",
xd.toString(3,3).c_str(),od.toString(3,3).c_str());
if (simulator)
updateSim(xd);
}
}
else
{
if (s0==triggered)
onlyXYZ=!onlyXYZ;
if (c0!=0)
{
iarm->stopControl();
if (simulator)
{
Vector x,o;
iarm->getPose(x,o);
updateSim(x);
}
if (gaze)
igaze->stopControl();
}
s0=idle;
c0=0;
}
}
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;
}
}
}
bool respond(const Bottle& command, Bottle& reply)
{
if(command.size()==0)
{
reply.addString("No command received.");
return true;
}
switch(command.get(0).asVocab()){
case HOME:
if(command.size()>1)
switch(command.get(1).asVocab()){
case ARM:
int tempCxL,tempCxR;
icartLeft->storeContext(&tempCxL);
icartLeft->storeContext(&tempCxR);
icartLeft->restoreContext(currentArmLeftContextID);
icartLeft->restoreContext(currentArmRightContextID);
icartLeft->goToPositionSync(leftArmHomePosition);
icartRight->goToPositionSync(rightArmHomePosition);
icartRight->waitMotionDone(0.1,2);
icartLeft->waitMotionDone(0.1,2);
icartLeft->restoreContext(tempCxR);
icartLeft->restoreContext(tempCxL);
icartLeft->deleteContext(tempCxR);
icartLeft->deleteContext(tempCxL);
reply.addString("Arm home position reached.");
return true;
case TORSO:
exploreTorso(torsoHomePosition);
reply.addString("Torso home position reached.");
return true;
case GAZE:
int tempCx;
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
igaze->lookAtFixationPoint(gazeHomePosition);
igaze->waitMotionDone(0.1,2);
igaze->restoreContext(tempCx);
igaze->deleteContext(tempCx);
reply.addString("Gaze home position reached.");
return true;
default:
reply.addString("Wrong device for home position.");
return true;
}
else{
int tempCxL,tempCxR,tempCx;
icartLeft->storeContext(&tempCxL);
icartLeft->storeContext(&tempCxR);
icartLeft->restoreContext(currentArmLeftContextID);
icartLeft->restoreContext(currentArmRightContextID);
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
icartLeft->goToPositionSync(leftArmHomePosition);
icartRight->goToPositionSync(rightArmHomePosition);
igaze->lookAtFixationPoint(gazeHomePosition);
exploreTorso(torsoHomePosition);
igaze->waitMotionDone(0.1,2);
icartRight->waitMotionDone(0.1,2);
icartLeft->waitMotionDone(0.1,2);
icartLeft->restoreContext(tempCxR);
igaze->restoreContext(tempCx);
icartLeft->restoreContext(tempCxL);
icartLeft->deleteContext(tempCxR);
icartLeft->deleteContext(tempCxL);
igaze->deleteContext(tempCx);
reply.addString("Ok, moving to home position.");
return true;
}
case LOOK_AT:
if(command.size()==2){
Bottle bAsk,bReply, bGet;
bAsk.addVocab(Vocab::encode("ask"));
Bottle &bTempAsk=bAsk.addList().addList();
bTempAsk.addString("name");
bTempAsk.addString("==");
bTempAsk.addString(command.get(1).asString());
objectsPort.write(bAsk,bReply);
cout<<"first"<<endl;
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("id")==false ||
bReply.get(1).asList()->find("id").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
bGet.addVocab(Vocab::encode("get"));
Bottle &bTempGet=bGet.addList().addList();
bTempGet.addString("id");
bTempGet.addInt(bReply.get(1).asList()->find("id").asList()->get(0).asInt());
objectsPort.write(bGet,bReply);
cout<<"second"<<endl;
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("position_3d")==false ||
bReply.get(1).asList()->find("position_3d").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
cout<<"third"<<endl;
Vector gazePosition(3);
gazePosition[0] = bReply.get(1).asList()->find("position_3d").asList()->get(0).asDouble();
gazePosition[1] = bReply.get(1).asList()->find("position_3d").asList()->get(1).asDouble();
gazePosition[2] = bReply.get(1).asList()->find("position_3d").asList()->get(2).asDouble();
int tempCx;
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
igaze->lookAtFixationPoint(gazePosition);
igaze->waitMotionDone(0.2,3);
igaze->restoreContext(tempCx);
igaze->deleteContext(tempCx);
reply.addString("Gaze position reached.");
return true;
}
else if(command.size()==4){
Vector gazePosition(3);
int tempCx;
gazePosition[0] = command.get(1).asDouble();
gazePosition[1] = command.get(2).asDouble();
gazePosition[2] = command.get(3).asDouble();
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
igaze->lookAtFixationPoint(gazePosition);
igaze->waitMotionDone(0.2,3);
igaze->restoreContext(tempCx);
igaze->deleteContext(tempCx);
reply.addString("Gaze position reached.");
return true;
}
else{
reply.addString("Wrong number of parameters for lookAt.");
return true;
}
case GET:
if(command.size()==2){
Bottle bAsk,bReply, bGet;
bAsk.addVocab(Vocab::encode("ask"));
Bottle &bTempAsk=bAsk.addList().addList();
bTempAsk.addString("name");
bTempAsk.addString("==");
bTempAsk.addString(command.get(1).asString());
objectsPort.write(bAsk,bReply);
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("id")==false ||
bReply.get(1).asList()->find("id").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
bGet.addVocab(Vocab::encode("get"));
Bottle &bTempGet=bGet.addList().addList();
bTempGet.addString("id");
bTempGet.addInt(bReply.get(1).asList()->find("id").asList()->get(0).asInt());
objectsPort.write(bGet,bReply);
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("position_2d_left")==false ||
bReply.get(1).asList()->find("position_2d_left").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
Vector objPosition(4);
objPosition[0] = bReply.get(1).asList()->find("position_2d_left").asList()->get(0).asInt();
objPosition[1] = bReply.get(1).asList()->find("position_2d_left").asList()->get(1).asInt();
objPosition[2] = bReply.get(1).asList()->find("position_2d_left").asList()->get(2).asInt();
objPosition[3] = bReply.get(1).asList()->find("position_2d_left").asList()->get(3).asInt();
reply.addVocab(VOCAB3('a','c','k'));
Bottle &coord = reply.addList();
coord.addInt((int)(objPosition[0]+objPosition[2])/2);
coord.addInt((int)(objPosition[1]+objPosition[3])/2);
return true;
}
else{
reply.addString("Wrong number of parameters for get.");
return true;
}
case TRACK:
if(command.size()==3)
switch(command.get(1).asVocab()){
case ARM:
if (command.get(2).asString() == "on"){
int tempCxL,tempCxR;
//icartLeft->storeContext(&tempCxL);
//icartLeft->storeContext(&tempCxR);
//icartLeft->restoreContext(currentArmLeftContextID);
//icartLeft->restoreContext(currentArmRightContextID);
icartLeft->setTrackingMode(true);
icartRight->setTrackingMode(true);
icartRight->storeContext(¤tArmRightContextID);
icartLeft->storeContext(¤tArmLeftContextID);
//icartLeft->restoreContext(tempCxR);
// icartLeft->restoreContext(tempCxL);
// icartLeft->deleteContext(tempCxR);
// icartLeft->deleteContext(tempCxL);
reply.addString("Arm tracking mode enabled.");
}
else if (command.get(2).asString() == "off"){
int tempCxL,tempCxR;
//icartLeft->storeContext(&tempCxL);
//icartLeft->storeContext(&tempCxR);
//icartLeft->restoreContext(currentArmLeftContextID);
//icartLeft->restoreContext(currentArmRightContextID);
icartLeft->setTrackingMode(false);
icartRight->setTrackingMode(false);
icartRight->storeContext(¤tArmRightContextID);
icartLeft->storeContext(¤tArmLeftContextID);
//icartLeft->restoreContext(tempCxR);
// icartLeft->restoreContext(tempCxL);
// icartLeft->deleteContext(tempCxR);
// icartLeft->deleteContext(tempCxL);
reply.addString("Arm tracking mode disabled.");
}
else
reply.addString("Wrong parameter: on/off");
return true;
case GAZE:
if (command.get(2).asString() == "on"){
int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->setTrackingMode(true);
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Gaze tracking mode enabled.");
}
else if (command.get(2).asString() == "off"){
int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->setTrackingMode(false);
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Gaze tracking mode disabled.");
}
else
reply.addString("Wrong parameter for trac: on/off");
return true;
default:
reply.addString("Wrong device for tracking mode.");
return true;
}
else{
reply.addString("Missing parameters for track.");
return true;
}
case BLOCK:
if(command.size()>1)
switch(command.get(1).asVocab()){
case GAZE:
if(command.size()==3){
// int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->blockEyes(command.get(2).asDouble());
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Gaze blocking mode enabled.");
}
else{
//int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->blockEyes(DEFAULT_VERGENCE);
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Default vergence set.");
}
return true;
default:
reply.addString("Wrong device for blocking mode.");
return true;
}
else{
reply.addString("Missing parameters for block.");
return true;
}
case GOTO:
if(command.size()==4){
Vector torsoTarget(3);
torsoTarget[0] = command.get(1).asDouble();
torsoTarget[1] = command.get(2).asDouble();
torsoTarget[2] = command.get(3).asDouble();
exploreTorso(torsoTarget);
reply.addString("Torso position reached.");
}
else{
reply.addString("Missing parameters for goto.");
return true;
}
return true;
case RUN:
running = true;
reply.addString("Ready for exploration. next or stop?");
return true;
case NEXT:
if(running){
exploreTorso(waypoints.getRow(index));
index++;
if (index > 2){
running = false;
index = 0;
reply.addString("Waypoint reached. End of waypoints.");
}
else{
reply.addString("Waypoint reached. next or stop?");
}
}
else
reply.addString("Not running.");
return true;
case STOP:
index = 0;
running = false;
reply.addString("Run stopped. Index reset.");
return true;
default:
RFModule::respond(command,reply);
return true;
}
return true;
}
