Пример #1
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;
    }
Пример #2
0
void ThreeDModule::getEncoderPositions(double *headjnt_pos, double *torsojnt_pos, Stamp stamp) {	
	mutex.wait();
		
	if(stamp.getTime() == 0) {
		std::cout << "WARNING: No timestamp found in this bottle!! check your implementation!! " << std::endl;
		stamp.update();
	}
	
	// store the current (start) indices in the circular buffer
	int tIdx = torsoIdx;
	int hIdx = headIdx;
	
	// helpers
	int idxH = -1, idxT = -1;
	int hH, hT;
	double smallestDiffH = 10000;
	double smallestDiffT = 10000;	
	double tDiff;

	// the bottles to be copied out of the circ buffer
	Bottle head, torso;
	
	// going through the circular buffer
    idxH = 1;
    idxT = 1;
//	for(int k = 0; k < LIST_LENGTH; k++) {
//		hT = (LIST_LENGTH + tIdx - k) % LIST_LENGTH;	
//		hH = (LIST_LENGTH + hIdx - k) % LIST_LENGTH;
//		
//		tDiff = fabs(stamp.getTime() - torsoStamp[hT].getTime());
//		std::cout << stamp.getTime() << " idx: " << hT <<  " getTime: " << torsoStamp[hT].getTime() << ", diffT: "<< tDiff << std::endl;		
//		if( tDiff < smallestDiffT ) {
//			idxT = hT;				
//			smallestDiffT = tDiff;
//		}
//
//		tDiff = fabs(stamp.getTime() - torsoStamp[hH].getTime());
//		if( tDiff < smallestDiffH ) { 
//			idxH = hH;		
//			smallestDiffH = tDiff;
//		}
//	}
	
	if( idxH != -1 ) head = headState[idxH];	
	if( idxT != -1 ) torso = torsoState[idxT];
	std::cout <<  "idxT: " << idxT << "/" << idxT-tIdx <<  " val: " << torso.toString() <<  std::endl;			
	std::cout <<  "idxH: " << idxH << "/" << idxH-hIdx << " val: " << head.toString() <<  std::endl;				
	
	for(int i = 0; i < 6; i++) {
		headjnt_pos[i] = head.get(i).asDouble();
		if(i < 3) torsojnt_pos[i] = torso.get(i).asDouble();
	}
	
	mutex.post();
}
Пример #3
0
void comBalancingThread::run()
{    
    // if(!Opt_display){
    
   
        //updating Time Stamp
        static Stamp timeStamp;
        timeStamp.update();
        
        //***************************** Reading F/T measurements and encoders ****************

        #ifdef FOR_PLOTS_ONLY 
                Vector* F_ext_RLt = port_ft_foot_right->read(true);
                Vector* F_ext_LLt = port_ft_foot_left->read(true);

                if (F_ext_LL==0) F_ext_LL= new Vector(6,0.0);
                if (F_ext_RL==0) F_ext_RL = new Vector(6,0.0);

                (*F_ext_LL) = -1.0*((*F_ext_LLt) - (Offset_Lfoot));
                (*F_ext_RL) = -1.0*((*F_ext_RLt) - (Offset_Rfoot));

                //Transformation from ankle to f/t sensor
                Matrix foot_hn(4,4); foot_hn.zero();
                foot_hn(0,2)=1; foot_hn(0,3)=-7.75;
                foot_hn(1,1)=-1;
                foot_hn(2,0)=-1;
                foot_hn(3,3)=1;           

                Vector tmp1, tmp2;
                tmp1 = (*F_ext_RL).subVector(0,2); tmp1.push_back(0.0); tmp1 = foot_hn * tmp1;
                tmp2 = (*F_ext_RL).subVector(3,5); tmp2.push_back(0.0); tmp2 = foot_hn * tmp2;

                for (int i=0; i<3; i++) (*F_ext_RL)[i] = tmp1[i];
                for (int i=3; i<6; i++) (*F_ext_RL)[i] = tmp2[i-3];

                tmp1 = (*F_ext_LL).subVector(0,2); tmp1.push_back(0.0); tmp1 = foot_hn * tmp1;
                tmp2 = (*F_ext_LL).subVector(3,5); tmp2.push_back(0.0); tmp2 = foot_hn * tmp2;

                for (int i=0; i<3; i++) (*F_ext_LL)[i] = tmp1[i];
                for (int i=3; i<6; i++) (*F_ext_LL)[i] = tmp2[i-3];

                //fprintf(stderr, "F_EXT_RL from module: %s\n", (*F_ext_RL).toString().c_str());

        #else
                if(Opt_ankles_sens)
                {
                    F_ext_RL = EEWRightAnkle->read(true);
                    F_ext_LL = EEWLeftAnkle->read(true);

                }
                else{
                    F_ext_RL = EEWRightLeg->read(true);
                    F_ext_LL = EEWLeftLeg->read(true);
                }   
        #endif      


        Ienc_RL->getEncoders(encs_r.data());
        Ienc_LL->getEncoders(encs_l.data());


        // check if the robot is not in contact with the ground
        static bool on_ground = true;
        //printf("%f %f \n", (*F_ext_LL)[0], (*F_ext_RL)[0]);
        if ((*F_ext_LL)[0] > 50 &&
            (*F_ext_RL)[0] > 50  )
            {
              on_ground = true;
            }
        else
            {
              on_ground = false;
              for (int jj=0; jj<6; jj++) (*F_ext_LL)[jj] =  (*F_ext_RL)[jj] = 1e-20;
              (*F_ext_LL)[0] =  (*F_ext_RL)[0] = 1e+20;
            }
        //printf("%s\n", (*F_ext_LL).toString().c_str());
        //gif ((*F_ext_LL)[3])
        //***************************** Computing ZMP ****************************************

        computeZMP(&zmp_xy, F_ext_LL, F_ext_RL, encs_l, encs_r);
        
        #ifdef VERBOSE
        fprintf(stderr, "ZMP coordinates: %f %f       %d \n",zmp_xy[0],zmp_xy[1],(int)(torso));
        fprintf(stderr, "ZMP desired: %f %f\n",zmp_des[0], zmp_des[1]);
        #endif

        //*********************** SENDING ZMP COORDINATES TO GuiBalancer *********************
        // if (objPort->getOutputCount() > 0)
        // {
            objPort->prepare() = zmp_xy;
            objPort->setEnvelope(timeStamp);
            objPort->write();
        // }
        
        //********************** SENDING ZMP COORDS TO BE PLOT BY ICUBGUI *****************************
        if (objPort2->getOutputCount() > 0)
        {
            Matrix Trans_lastRot(4,4); Trans_lastRot.zero();
            Trans_lastRot.setSubmatrix(rot_f,0,0);
            Trans_lastRot(3,3) = 1;
            Trans_lastRot(1,3) = -separation(encs_r,encs_l)/2;
            Vector zmp_xy_trans(4); zmp_xy_trans.zero();
            zmp_xy_trans.setSubvector(0,zmp_xy);
            zmp_xy_trans(3)=1;
            zmp_xy_trans = Hright*(Right_Leg->getH())*SE3inv(Trans_lastRot)*zmp_xy_trans;
            objPort2->prepare() = zmp_xy_trans.subVector(0,1);
            objPort2->write();
        }

        //*********************** Reading ZMP derivative **********************************************
        iCub::ctrl::AWPolyElement el;
        el.data=zmp_xy;
        el.time=Time::now();
        zmp_xy_vel = zmp_xy_vel_estimator->estimate(el);

        //*********************** Obtaining Transformation Matrix from Root to WRF ********************
        Matrix RLeg_RotTrans(4,4); RLeg_RotTrans.zero();
        RLeg_RotTrans = Hright * Right_Leg->getH();

        Matrix lastRotTrans(4,4); 
        lastRotTrans.zero(); 
        lastRotTrans(0,2) = lastRotTrans(2,0) = lastRotTrans(3,3) = 1; 
        lastRotTrans(1,1) = -1;
        lastRotTrans(1,3) = -1*RLeg_RotTrans(1,3);

        Matrix H_r_f(4,4); H_r_f.zero();
        H_r_f = RLeg_RotTrans * lastRotTrans;
        Matrix H_f_r = SE3inv(H_r_f);
        // fprintf(stderr, "\n Right foot H_f_r Matrix: \n %s\n", H_f_r.toString().c_str());

        // Hip_from_foot->prepare() = H_f_r.getCol(3);
        // Hip_from_foot->write();

//********************** CONTACT WITH HUMAN DETECTION ******************************************
        /*
        skinContactList *skinContacts  = port_skin_contacts->read(false);
        static double start_time = 0;
        bool contact_detected=false;
         if(skinContacts)
        {
            for(skinContactList::iterator it=skinContacts->begin(); it!=skinContacts->end(); it++){
                if(it->getBodyPart() == RIGHT_ARM)
                    contact_detected=true;
            }
        }

        if(contact_detected) //if CONTACT happened.
         {
            start_time = yarp::os::Time::now();
            fprintf(stderr, "Switching to Torso Control Mode\n");

             torso=true;
            knees=false;

            contacto[0]=1;
        }
        else{
            double end_time = yarp::os::Time::now();
            if((end_time - start_time)>=5.0 && torso==true){
                fprintf(stderr, "Switching back to previous control strategy\n");
                torso=false;
                knees=true;
                contacto[0]=0;
            }
        }
            
        contact_sig->prepare() = contacto;
        contact_sig->write();
        */
//**********************     END CONTACT DETECTION    ***********************************************

        //********************** SUPPORT POLYGON DEFINITION ****************************
/*        double bound_fw =  0.10;
        double bound_bw = -0.05;
        double security_offset = 0.02;

            //zmp support polygon
        if((zmp_xy[0]>bound_fw) || (zmp_xy[0]<bound_bw)){
            if(zmp_xy[0]<bound_bw){
                zmp_des[0] = bound_bw + security_offset;
            }
            else{
                if(zmp_xy[0]>bound_fw){
                   zmp_des[0] = bound_fw - security_offset;
                }
            }  
        }*/
        //********************** CONTROL BLOCK 1 **************************************
        double delta_zmp [2];
        delta_zmp [0] = zmp_xy[0] - zmp_des[0];
        delta_zmp [1] = zmp_xy[1] - zmp_des[1];
        double delta_zmp_vel [2];
        delta_zmp_vel[0] = zmp_xy_vel[0] - 0.0;
        delta_zmp_vel[1] = zmp_xy_vel[1] - 0.0;
        double delta_com [2];
        double delta_theta;
        double delta_phi;
        delta_com [0] = + Kp_zmp_x * delta_zmp[0] - Kd_zmp_x * delta_zmp_vel[0];
        delta_com [1] = + Kp_zmp_y * delta_zmp[1] - Kd_zmp_y * delta_zmp_vel[1];
        delta_theta   = - Kp_tetha * delta_zmp[0] - Kd_tetha * delta_zmp_vel[0];
        delta_phi     = - Kp_phi   * delta_zmp[1] - Kd_phi   * delta_zmp_vel[1];
        
        double der_part =  Kd_zmp_x * delta_zmp_vel[0];
        #ifdef VERBOSE
        fprintf(stderr, "\n Kd*delta_zmp_vel[0] =  %f , %f \n", der_part, zmp_xy_vel[0]); 
        #endif

        //********************** CONTROL BLOCK 2 **************************************
        double COM_des[2];
        double phi_des;
        double theta_des;
        double COM_ref[2];
        double phi_ref;
        double theta_ref;
        //From initial standing position
        COM_des[0] = 0.0;
        COM_des[1] = 0.0;
        theta_des  = 0;
        phi_des   = 0;
        COM_ref[0] = delta_com[0] + COM_des[0];
        COM_ref[1] = delta_com[1] + COM_des[1];
        theta_ref  = delta_theta  + theta_des;
        phi_ref    = delta_phi    + phi_des;

        Vector COM_r(2); COM_r[0] = COM_ref[0]; COM_r[1] = COM_ref[1];
        COM_ref_port->prepare() = COM_r;
        COM_ref_port->setEnvelope(timeStamp);
        COM_ref_port->write();

        //********************** CONTROL BLOCK 3 **************************************
        double q_torso_theta        = 0.0;
        double q_torso_phi          = 0.0;
        double q_left_ankle_theta   = 0.0;
        double q_left_ankle_phi     = 0.0;
        double q_right_ankle_theta  = 0.0;
        double q_right_ankle_phi    = 0.0;

        double length_leg = 0.47;       //empirically taken this value from robot standing. COM z coord. from w.r.f.
        #ifdef VERBOSE
        fprintf(stderr, "COM_ref = %+6.6f ",COM_ref[0]);
        #endif
        q_right_ankle_theta = asin(-COM_ref[0]/length_leg)*CTRL_RAD2DEG;

        //q_right_ankle_phi   = -asin(-COM_ref[1]/length_leg)*CTRL_RAD2DEG;
        q_right_ankle_phi =  0.0;
        q_left_ankle_phi  = -2.0;
        
//        q_torso_theta = theta_ref;
        q_torso_theta = 15.0;

        //********************** SEND COMMANDS   **************************************
        // SATURATORS
            //torso forward/backward
            double limit = 20;
            if (q_torso_theta>limit)  q_torso_theta=limit;
            if (q_torso_theta<-limit) q_torso_theta=-limit;
            //torso right/left
            if (q_torso_phi>limit)  q_torso_phi=limit;
            if (q_torso_phi<-limit) q_torso_phi=-limit;

            //ankle backward/forward
            if (q_right_ankle_theta>limit)  q_right_ankle_theta=limit;
            if (q_right_ankle_theta<-limit) q_right_ankle_theta=-limit; 

            //ankle right/left
            if (q_right_ankle_phi>10)  q_right_ankle_phi=10;
            if (q_right_ankle_phi<-5) q_right_ankle_phi-5; 
            
        //copying movement        
        q_left_ankle_theta = q_right_ankle_theta;
        //q_left_ankle_phi = q_right_ankle_phi;  //WE NEED TO CALIBRATE AGAIN THIS JOINT BECAUSE -2 DEG FOR RIGHT LEG ARE ACTUALLY 0 FOR THE LEFT ONE.
        
        #ifdef VERBOSE
        fprintf(stderr, "q theta to be sent: %+6.6f ", q_right_ankle_theta);
        fprintf(stderr, "q phi   to be sent: %+6.6f ", q_right_ankle_phi);
        #endif 

        Vector ankle_ang(2); ankle_ang.zero();
        ankle_ang[0]=q_right_ankle_theta;
        ankle_angle->prepare()=ankle_ang;
        ankle_angle->setEnvelope(timeStamp);
        ankle_angle->write();

        #ifdef VERBOSE
        fprintf(stderr, "q torso to be sent: %+6.6f\n", q_torso_theta);
        #endif 

        if(!Opt_display)
        {
            if (on_ground)
            {
            Ipid_TO->setReference(2,q_torso_theta);
           // Ipid_TO->setReference(1,q_torso_phi);
            Ipid_RL->setReference(4,q_right_ankle_theta);
            Ipid_LL->setReference(4,q_left_ankle_theta);
            
            //open legs version
            Ipid_RL->setReference(5,q_right_ankle_phi);
            Ipid_LL->setReference(5,q_left_ankle_phi);
            
            //closed leg version
            //Ipid_RL->setReference(5,q_right_ankle_phi);
            //Ipid_RL->setReference(1,-q_right_ankle_phi);
            //Ipid_LL->setReference(5,-q_left_ankle_phi);
            //Ipid_LL->setReference(1,q_left_ankle_phi);
            }
        }

// ********************** OLD CONTROL ******************************************
        /*//TORSO CONTROL MODE
        if(torso && (*sample_count)>=2){

            if((zmp_xy[0]>0.05) || (zmp_xy[0]<-0.01)){

                if(zmp_xy[0]<0){
                    zmpXd = 0;
                }
                else{
                    if(zmp_xy[0]>0){
                    zmpXd = 0.03;
                    }
                }
                zmpd[0] = zmpXd;  
                switch_ctrl = true;
            }
                
            if(switch_ctrl){ 
                double error_zmp = zmp_xy[0]-zmpXd;
                command[2]=-Kp_T*(error_zmp) + Kd_T*((zmpVel)[0]);

                if(error_zmp<0.005){
                    switch_ctrl = false;
                    zmpd[0]=0;
                    command[2]=0;
                }
            }

            torso_ctrl_sig->prepare() = command;
            torso_ctrl_sig->write();

            desired_zmp->prepare() = zmpd;
            desired_zmp->write();
        
        }

        //KNEES CONTROL MODE
        if(knees && (*sample_count)>=2){
            if((zmp_xy[0]>0.05) || (zmp_xy[0]<-0.02)){

                if(zmp_xy[0]<0){
                    zmpXd = 0;
                }
                else{
                    if(zmp_xy[0]>0){
                    zmpXd = 0.03;
                    }
                }
                zmpd[0] = zmpXd; 
                switch_ctrl = true;
            }
                
            if(switch_ctrl){
                double error_zmp = zmp_xy[0]-zmpXd;
                command_RL[3]=-Kp_K*(error_zmp) + Kd_K*((zmpVel)[0]);
                command_LL[3]= command_RL[3];

                if(error_zmp<0.005){
                    switch_ctrl = false;
                    zmpd[0]=0;
                    command_RL[3]=0;
                    command_LL[3]=0;
                }
            }

            knee_ctrl_sig->prepare() = command_RL;
            knee_ctrl_sig->write();

            desired_zmp->prepare() = zmpd;
            desired_zmp->write();
        }
        */
// END OLD CONTROL
}
Пример #4
0
int main(int argc, char *argv[]) {
    // initialization

    // Open the network
    Network yarp;
    //BufferedPort<Sound> p;
    Port p;
    p.open("/sender");

    // Get a portaudio read device.
    Property conf;
    conf.put("device","portaudio");
    conf.put("read", "");
    // conf.put("samples", rate * rec_seconds);
    conf.put("samples", fixedNSample);
    conf.put("rate", rate);
    PolyDriver poly(conf);
    IAudioGrabberSound *get;

    // Make sure we can read sound
    poly.view(get);
    if (get==NULL) {
        printf("cannot open interface");
        return 1;
    }
    else{
        printf("correctly opened the interface rate: %d, number of samples: %d, number of channels %d \n",rate, rate*rec_seconds, 2);
    }

    //Grab and send
    Sound s;
    //Bottle b;
    //b.addString("hello");

    //unsigned char* dataSound;
    //short* dataAnalysis;
    //int v1, v2;
    //int i = 0, j = 0;
    //NetInt16 v;
    // i = sample amd j = channels;

    get->startRecording(); //this is optional, the first get->getsound() will do this anyway.
    Stamp ts;
    while (true)
    {
      double t1=yarp::os::Time::now();
      ts.update();  
      //s = p.prepare();           
      
      get->getSound(s);        
      
      //v1 = s.get(i,j);
      //v = (NetInt16) v1;
      //v2 = s.get(i+1,j+1); 
      //dataAnalysis = (short*) dataSound;        
      
      p.setEnvelope(ts);
      p.write(s);

      double t2=yarp::os::Time::now();
      printf("acquired %f seconds \n", t2-t1);
    }
    get->stopRecording();  //stops recording.

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

        }
    }