Beispiel #1
0
	virtual void threadRelease() {

		/* Stop and close ports */
		cportL->interrupt();
		cportR->interrupt();
		susPort->interrupt();

		cportL->close();
		cportR->close();
		susPort->close();

		delete cportL;
		delete cportR;
		delete susPort;

		/* stop motor interfaces and close */
		gaze->stopControl();
		clientGazeCtrl.close();
		carm->stopControl();
		clientArmCart.close();

		robotTorso.close();
		robotHead.close();
		robotArm.close();

	}
 bool interruptModule()
 {
     imgLPortIn.interrupt();
     imgRPortIn.interrupt();
     igaze->stopControl();
     iarm->stopControl();
     return true;
 }
Beispiel #3
0
    virtual void threadRelease()
    {    
        // we require an immediate stop
        // before closing the client for safety reason
        igaze->stopControl();

        // it's a good rule to restore the controller
        // context as it was before opening the module
        igaze->restoreContext(startup_context_id);

        clientGaze.close();
        clientTorso.close();
    }
    virtual void threadRelease()
    {    
        // we require an immediate stop
        // before closing the client for safety reason
        // (anyway it's already done internally in the
        // destructor)
        igaze->stopControl();

        // it's a good rule to reinstate the tracking mode
        // as it was
        igaze->setTrackingMode(false);

        delete clientGaze;
 //       delete clientTorso; //pramod
    }
Beispiel #5
0
    bool close()
    {
        iarm->stopControl();
        iarm->restoreContext(startup_context);
        drvCart.close();

        ivel->stop(joints.size(),joints.getFirst());
        for (size_t i=0; i<modes.size(); i++)
            modes[i]=VOCAB_CM_POSITION;
        imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
        drvHand.close();

        if (simulator)
        {
            Bottle cmd,reply;
            cmd.addString("world");
            cmd.addString("del");
            cmd.addString("all");
            simPort.write(cmd,reply);

            simPort.close();
        }

        if (gaze)
        {
            igaze->stopControl();
            drvGaze.close();
        }

        igeo->stopFeedback();
        igeo->setTransformation(eye(4,4));
        drvGeomagic.close();
        forceFbPort.close();

        return true;
    }
    bool close()
    {
        handlerPort.close();
		objectsPort.close();

		igaze->stopControl();
		igaze->restoreContext(startupGazeContextID);
		igaze->deleteContext(startupGazeContextID);
		igaze->deleteContext(currentGazeContextID);

		if (clientGazeCtrl.isValid())
			clientGazeCtrl.close();

		icartLeft->stopControl();
		icartLeft->restoreContext(startupArmLeftContextID);
		icartLeft->deleteContext(startupArmLeftContextID);
		icartLeft->deleteContext(currentArmLeftContextID);

		if (clientArmLeft.isValid())
			clientArmLeft.close();

		icartRight->stopControl();
		icartRight->restoreContext(startupArmRightContextID);
		icartRight->deleteContext(startupArmRightContextID);
		icartRight->deleteContext(currentArmRightContextID);

		if (clientArmLeft.isValid())
			clientArmLeft.close();

		itorsoVelocity->stop();
		
		if (clientTorso.isValid())
		clientTorso.close();
		
        return true;
    }
Beispiel #7
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();

			}
		}
	}
Beispiel #8
0
    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;
        }
    }