Пример #1
0
    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);
        }

        
        
        
    }
Пример #2
0
	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();

			}
		}
	}
Пример #3
0
 Vector retrieveTarget3D(const Vector &cogL, const Vector &cogR)
 {
     Vector pos;
     igaze->triangulate3DPoint(cogL, cogR, pos);
     return pos;
 }