void generateTarget() { // translational target part: a circular trajectory // in the yz plane centered in [-0.3,-0.1,0.1] with radius=0.1 m // and frequency 0.1 Hz // xd[0]=-0.3; // xd[1]=-0.1+0.2*cos(2.0*M_PI*0.1*(t-t0)); // xd[2]=+0.1+0.2*sin(2.0*M_PI*0.1*(t-t0)); // we keep the orientation of the left arm constant: // we want the middle finger to point forward (end-effector x-axis) // with the palm turned down (end-effector y-axis points leftward); // to achieve that it is enough to rotate the root frame of pi around z-axis od[0]=0.0; od[1]=0.0; od[2]=1.0; od[3]=M_PI; //adding code for taget location through vision //incoming image from both eyes Bottle *target = incomingPort.read(); Vector r(2), l(2); r[0] = target->get(0).asDouble(); r[1] = target->get(1).asDouble(); l[0] = target->get(3).asDouble(); l[1] = target->get(4).asDouble(); if(r[0]==0.0 && r[1]==0.0 && l[0]==0.0 && l[1]==0.0){ xd[0]=-0.3; xd[1]=-.1; xd[2]=.1; } else{ igaze->lookAtStereoPixels(l,r); igaze->triangulate3DPoint(l,r,xd); } }
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(); } } }
Vector retrieveTarget3D(const Vector &cogL, const Vector &cogR) { Vector pos; igaze->triangulate3DPoint(cogL, cogR, pos); return pos; }