Ejemplo n.º 1
0
    bool updateModule()
    {
        Vector *imuData=iPort.read();
        if (imuData==NULL)
            return false;

        Stamp stamp;
        iPort.getEnvelope(stamp);

        double t0=Time::now();
        Vector gyro=imuData->subVector(6,8);
        Vector gyro_filt=gyroFilt.filt(gyro);

        gyro-=gyroBias;
        gyro_filt-=gyroBias;

        Vector mag_filt=magFilt.filt(imuData->subVector(9,11));
        double magVel=norm(velEst.estimate(AWPolyElement(mag_filt,stamp.getTime())));

        adaptGyroBias=adaptGyroBias?(magVel<mag_vel_thres_up):(magVel<mag_vel_thres_down);
        gyroBias=biasInt.integrate(adaptGyroBias?gyro_filt:Vector(3,0.0));
        double dt=Time::now()-t0;

        if (oPort.getOutputCount()>0)
        {
            Vector &outData=oPort.prepare();
            outData=*imuData;
            outData.setSubvector(6,gyro);
            oPort.setEnvelope(stamp);
            oPort.write();
        }

        if (bPort.getOutputCount()>0)
        {
            bPort.prepare()=gyroBias;
            bPort.setEnvelope(stamp);
            bPort.write();
        }

        if (verbose)
        {
            yInfo("magVel   = %g => [%s]",magVel,adaptGyroBias?"adapt-gyroBias":"no-adaption");
            yInfo("gyro     = %s",gyro.toString(3,3).c_str());
            yInfo("gyroBias = %s",gyroBias.toString(3,3).c_str());
            yInfo("dt       = %.0f [us]",dt*1e6);
            yInfo("\n");
        }

        return true;
    }
Ejemplo n.º 2
0
    bool updateModule()
    {
        LockGuard lg(mutex);

        double dumpTime=Time::now();
        Bottle &bDump=dumpPort.prepare();
        bDump.clear();

        bDump.addString(actionTag);
        bDump.addString(objectTag);

        opc.checkout();        

        // agent body + position
        agentName = getPartnerName();
        if (Entity *e=opc.getEntity(agentName))
        {
            if (Agent *agent=dynamic_cast<Agent*>(e))
            {
                bDump.addList()=agent->m_body.asBottle();
                Bottle &bAgent=bDump.addList();
                bAgent.addString(agent->name());
                bAgent.addDouble(agent->m_ego_position[0]);
                bAgent.addDouble(agent->m_ego_position[1]);
                bAgent.addDouble(agent->m_ego_position[2]);
                bAgent.addInt(agent->m_present==1.0?1:0);
            }
        }

        // objects position
        list<Entity*> lEntity=opc.EntitiesCache();
        for (list<Entity*>::iterator itEnt=lEntity.begin(); itEnt!=lEntity.end(); itEnt++)
        {
            string entityName=(*itEnt)->name();
            string entityType=(*itEnt)->entity_type();
            if (entityType==EFAA_OPC_ENTITY_OBJECT)
            {
                if (Object *object=dynamic_cast<Object*>(*itEnt))
                {
                    Bottle &bObject=bDump.addList();
                    bObject.addString(object->name());
                    bObject.addDouble(object->m_ego_position[0]);
                    bObject.addDouble(object->m_ego_position[1]);
                    bObject.addDouble(object->m_ego_position[2]);
                    bObject.addInt(object->m_present==1.0?1:0);
                }
            }
        }

        bDump.addInt(gate);        

        dumpStamp.update(dumpTime);
        dumpPort.setEnvelope(dumpStamp);
        dumpPort.writeStrict();

        yInfo()<<bDump.toString();

        return true;
    }
    void sendContacts(BufferedPort<skinContactList> &outPort, const skinContactList &sCL)
    {
        ts.update();
        skinContactList &sCLout = outPort.prepare();
        sCLout.clear();

        sCLout = sCL;
        
        outPort.setEnvelope(ts);
        outPort.write();     
    }
Ejemplo n.º 4
0
	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");
		}
	}
Ejemplo n.º 5
0
    virtual void run()
    {    
        int  i_touching_left=0;
        int  i_touching_right=0;
        int  i_touching_diff=0;
        info.update();
        
        skinContactList *skinContacts  = port_skin_contacts->read(false);
        if(skinContacts)
        {
            for(skinContactList::iterator it=skinContacts->begin(); it!=skinContacts->end(); it++){
                if(it->getBodyPart() == LEFT_ARM)
                    i_touching_left += it->getActiveTaxels();
                else if(it->getBodyPart() == RIGHT_ARM)
                    i_touching_right += it->getActiveTaxels();
            }
        }
        i_touching_diff=i_touching_left-i_touching_right;

        if (abs(i_touching_diff)<5)
        {
            fprintf(stdout,"nothing!\n");
        }
        else
        if (i_touching_left>i_touching_right)
        {
            fprintf(stdout,"Touching left arm! \n");
            if (!left_arm_master) change_master();
        }
        else
        if (i_touching_right>i_touching_left)
        {
            fprintf(stdout,"Touching right arm! \n");
            if (left_arm_master) change_master();
        }

        if (left_arm_master)
        {
            robot->ienc[LEFT_ARM] ->getEncoders(encoders_master);
            robot->ienc[RIGHT_ARM]->getEncoders(encoders_slave);
            if (port_left_arm->getOutputCount()>0)
            {
                port_left_arm->prepare()= Vector(16,encoders_master);
                port_left_arm->setEnvelope(info);
                port_left_arm->write();
            }
            if (port_right_arm->getOutputCount()>0)
            {
                port_right_arm->prepare()= Vector(16,encoders_slave);
                port_right_arm->setEnvelope(info);
                port_right_arm->write();
            }            
            
        //    robot->ipos[RIGHT_ARM] ->positionMove(3,encoders_master[3]);
            for (int i=jjj; i<5; i++)
            {
                robot->ipid[RIGHT_ARM]->setReference(i,encoders_master[i]);
            }
        }
        else
        {
            robot->ienc[RIGHT_ARM]->getEncoders(encoders_master);
            robot->ienc[LEFT_ARM] ->getEncoders(encoders_slave);
            for (int i=jjj; i<5; i++)
            {
                robot->ipid[LEFT_ARM]->setReference(i,encoders_master[i]);
            }

        }
    }