Beispiel #1
0
    void onRead(T &obj) override
    {
        if (++cnt==dwnsample)
        {
            if (firstIncomingData)
            {
                yInfo() << "Incoming data detected";
                firstIncomingData=false;
            }

            DumpItem item;
            Stamp info;

            BufferedPort<T>::getEnvelope(info);
            item.seqNumber=info.getCount();

            if (txTime || (info.isValid() && !rxTime))
                item.timeStamp.setTxStamp(info.getTime());

            if (rxTime || !info.isValid())
                item.timeStamp.setRxStamp(Time::now());

            item.obj=factory(obj);

            buf.lock();
            buf.push_back(item);
            buf.unlock();

            cnt=0;
        }
    }
void velImpControlThread::run()
{
	double t_start = yarp::os::Time::now();


	if (getIterations()>100)
	{
		fprintf(stderr, "Thread ran %d times, est period %lf[ms], used %lf[ms]\n",
				getIterations(),
				getEstPeriod(),
				getEstUsed());
		resetStat();
	}
	_mutex.wait();

	////getting new commands from the fast command port
	//this command receives also feedforward velocities
	yarp::os::Bottle *bot = command_port.read(false);
	if(bot!=NULL)
	{
        nb_void_loops = 0;
		//fprintf(stderr, "\n Receiving command: \t");
		int size = bot->size()/2;
		for(int i=0;i<size;i++)
		{	
			int ind = bot->get(2*i).asInt();
			if(ind < VELOCITY_INDEX_OFFSET) 
			{//this is a position command
				targets(ind) = bot->get(2*i+1).asDouble();
			} 
			else 
			{//this is a velocity command
				ffVelocities(ind - VELOCITY_INDEX_OFFSET) = bot->get(2*i+1).asDouble();
			}
			//fprintf(stderr, "for joint *%d, received %f, \t", ind, targets(ind));
		}
		first_command++;
	} else {
        nb_void_loops++;
        if(nb_void_loops > 5) {
            ffVelocities = 0.0;
        }
    }

#if SWITCH    
    switchImp(ffVelocities[0]); //change stiffness according to shoulder/hips velocities
	compute_stiffness(requestedStiff, requestedDamp, currStiff, currDamp);
	//printf("0: %+3.5f %+3.5f %+3.5f %+3.5f *** ",requestedStiff[0], requestedDamp[0], currStiff[0], currDamp[0]);
	//printf("1: %+3.5f %+3.5f %+3.5f %+3.5f *** ",requestedStiff[1], requestedDamp[1], currStiff[1], currDamp[1]);
	//printf("2: %+3.5f %+3.5f %+3.5f %+3.5f *** ",requestedStiff[2], requestedDamp[2], currStiff[2], currDamp[2]);
	//printf("\n");
	if (impedance_enabled==true)
	{
		for(int i=0; i< nJoints; i++) iimp->setImpedance(i,  currStiff[i], currDamp[i]);
	}
#endif
	Bottle stiffness_output;
	Bottle damping_output;
	Bottle velocity_output;
    for(int i=0; i< nJoints; i++) 
	{
		stiffness_output.addDouble(currStiff[i]);
		damping_output.addDouble(currDamp[i]);
		velocity_output.addDouble(command[i]);
	}
	Stamp stmp;
	stmp = itime->getLastInputStamp();
	if (stmp.isValid())
	{
		stiffness_port.setEnvelope(stmp);
		damping_port.setEnvelope(stmp);
		velocity_port.setEnvelope(stmp);
	}
	else
	{
		stmp=Stamp(-1,0.0);
		stiffness_port.setEnvelope(stmp);
		damping_port.setEnvelope(stmp);
		velocity_port.setEnvelope(stmp);
	}
    velocity_port.write(velocity_output);
	stiffness_port.write(stiffness_output);
	damping_port.write(damping_output);

	//getting commands from the slow port
	yarp::sig::Vector *vec = command_port2.read(false);
	if (vec!=0)
	{
		targets=*vec;
		first_command++;
	}

	static int count=0;
	count++;

    // normale by randaz
	ienc->getEncoders(encoders.data());
    
    // versione che prende direttam la refernce del pid
    for(int i=0; i<nJoints; i++)
    {
    		if(impContr[i]==1)
    		{
    			ipid->getReference(i, encoders_ref.data()+i);
                printf("%d :  %f vs %f \n",i,encoders(i),encoders_ref(i));
    		}
    }

	//ienc->getEncoderSpeeds(encoders_speed.data());
/*	fprintf(stderr, "printing to file \n");
//#if 0
	 for(int i=0;i<nJoints;i++)
	 {
		printf("%f ",encoders(i));
		//fprintf(currentSpeedFile,"%f ",encoders(i));
	 }
	//fprintf(currentSpeedFile,"%f\n",t_start-time_watch);
	 printf("\n");
	 for(int i=0;i<nJoints;i++)
	 {
		printf("%f ",encoders_ref(i));
	 }
	 printf("\n");

	 for(int i=0;i<nJoints;i++)
	 {
		printf("%d :  %f vs %f \n",i,encoders(i),encoders_ref(i));
	 }
	 printf("\n");*/

//#endif

	Kd=0.0;

	for(int k=0;k<nJoints;k++)
	{
		double current_err = targets(k)-encoders_ref(k);
		error_d(k) = (current_err - error(k))/((double)control_rate)*1000.0;
		error(k) = current_err;

		//we calculate the command Adding the ffVelocities
		command(k) = Kp(k)*error(k) + Kd(k)*error_d(k) + ffVelocities(k);
	}
	

	//    std::cout << command.toString() << std::endl;

	limitSpeed(command);

	if (suspended)
		command=0;

#if 0
	for(int i=0;i<nJoints;i++)
		fprintf(targetSpeedFile,"%f ",command(i));
	fprintf(targetSpeedFile,"%f\n",t_start-time_watch);
#endif

	if(first_command) {
		int trials = 0;
		while(!ivel->velocityMove(command.data())){
			trials++;
			fprintf(stderr,"velcontrol ERROR>> velocity move sent false\n");
			if(trials>10) {
				fprintf(stderr, "velcontrol ERROR>> tried 10 times to velocityMove, halting...\n");
				this->halt();
				break;
			}
		}
	}
	_mutex.post();

}
Beispiel #3
0
//  Generate new values 
void DataPlot::timerEvent(QTimerEvent *)
{
    for (int k = 0; k < numberOfInputPorts; k++)
        {
            int timeout = 0;
            int maxTimeout = 2;
            Bottle *b = NULL;
            
            while(b==NULL && timeout < maxTimeout) 
                {
                    b = inputPorts[k].read(false);
                    Time::delay(0.005);
                    timeout++;
                }
            if (timeout==maxTimeout)
                {
                    if(VERBOSE) fprintf(stderr, "MESSAGE: Couldn't receive data. Going to put a zero! \n");
                    for(int i = 0; i < numberOfPlots[k]; i++)
                        {
                            d_y[k][i][PLOT_SIZE - 1] = 0;
                            for ( int j = 0; j < PLOT_SIZE - 1; j++ )
                                {
                                    d_y[k][i][j] = d_y[k][i][j+1];
                                }
                        }
                }
            else
                {
                    for(int i = 0; i < numberOfPlots[k]; i++)
                        {

                            Stamp stmp;
                            inputPorts[k].getEnvelope(stmp);
#ifdef ENABLE_REALTIME
                            if (stmp.isValid()) 
                                d_x_real_time[PLOT_SIZE - 1] = (stmp.getTime() - initialTime);                            
                            
                            for ( int j = 0; j < PLOT_SIZE - 1; j++ )
                                {
                                    if(realTime)
                                        d_x_real_time[j] =  d_x_real_time[j+1];
                                }
#endif
                            if (b==NULL)
                                d_y[k][i][PLOT_SIZE - 1] = 0;
                            else
                                if (b->size()-1 >= index[k][i])
                                    {
                                        d_y[k][i][PLOT_SIZE - 1] = b->get(index[k][i]).asDouble();
                                        //if(VERBOSE) fprintf(stderr, "MESSAGE: Getting from port %d the index %d\n", k, index[k][i]);
                                    }
                            // y moves from left to right:
                            // Shift y array right and assign new value to y[0].
                            
                            for ( int j = 0; j < PLOT_SIZE - 1; j++ )
                                {
                                    d_y[k][i][j] = d_y[k][i][j+1];
                                }
                            
                            // update the display
                            //setAxisScale(QwtPlot::yLeft, min, max);
#ifdef ENABLE_REALTIME                            
                            if(numberAcquiredData==PLOT_SIZE && realTime)
                                {
                                    if(VERBOSE) fprintf(stderr, "MESSAGE: switching to real time\n");
                                    
                                    QwtPlotCurve *timeBasedCurve = new QwtPlotCurve("Data");
                                    timeBasedCurve->attach(this);
                                    timeBasedCurve->setRawData(d_x_real_time, d_y[k][i], PLOT_SIZE);
                                    timeBasedCurve->setPen(coloredPens[k%NUMBER_OF_LIN][i%NUMBER_OF_COL]);
                                    nonTimeBasedCurve[k][i].attach(NULL);
                                    
                                    //Set title
                                    char cTitle[256];
                                    sprintf(cTitle, "Data(%d)", index[k][i]);
                                    QwtText curveTitle(cTitle, QwtText::PlainText);
                                    curveTitle.setFont(plotFont);
                                    timeBasedCurve->setTitle(curveTitle); 
                                }
#endif
                        }
                }
        }
    if (acquire)
        replot();

    numberAcquiredData++;
    //if(VERBOSE) fprintf(stderr, "Number of acquired data is %d\n", numberAcquiredData);
    //QSize plotSize= this->sizeHint();
    //if(VERBOSE) fprintf(stderr, "Hint is: hInt=%d, vInt=%d\n", plotSize.height(), plotSize.width()); 

	static double before = Time::now();	
	double now = Time::now();
	static double meanEffectiveTime = 0;

	double currentEffectiveTime = (now - before)*1000.0;
	if (numberAcquiredData >= 2)
		meanEffectiveTime = (meanEffectiveTime*(numberAcquiredData-2) + currentEffectiveTime)/(numberAcquiredData-1);
	//if(VERBOSE) fprintf(stderr, "Iteration %d: Current time is %f and mean is %f\n", numberAcquiredData, currentEffectiveTime, meanEffectiveTime);	
	if (currentEffectiveTime*0.5 > d_interval)
	{
		if(VERBOSE) fprintf(stderr, "Real Timer is %f but I was supposed to run at %d ms. Mean is: %f \n", currentEffectiveTime, d_interval, meanEffectiveTime);	
	    if(VERBOSE) fprintf(stderr, "You should slow down to %d ms \n", (int) (meanEffectiveTime * 2));	
        //setTimerInterval((int) (meanEffectiveTime * 2));
	}

	before = now;

}
Beispiel #4
0
void DataPlot::initSignalDimensions()
{
    if(VERBOSE) fprintf(stderr, "MESSAGE: Will now initialize the signal dimensions\n");
    //getting info on the connected port
    nonTimeBasedCurve = new QwtPlotCurve*[numberOfInputPorts];
    for (int k = 0; k < numberOfInputPorts; k++)
        {
            Bottle *b = NULL;
            int timeout = 0;
            const int maxTimeout = 1000;
            while(b==NULL && timeout < maxTimeout) 
                {
                    b = inputPorts[k].read(false);
                    Time::delay(0.005);
                    timeout++;
                }
            if (timeout==maxTimeout)
                {
                    if(VERBOSE) fprintf(stderr, "MESSAGE: Couldn't receive data. Going to put a zero! \n");
                    realTime = false;
                    //  Initialize data
                    for (int j = 0; j< numberOfPlots[k]; j++)
                        {
                            for (int i = 0; i < PLOT_SIZE; i++)
                                {
                                    d_x[i] = i;     // time axis
                                    //if(VERBOSE) fprintf(stderr, "MESSAGE: (%d, %d)\n", j, i);
                                    d_y[k][j][i] = 0;
                                }
                        }
                }
            else
                {
                    if(VERBOSE) fprintf(stderr, "MESSAGE: Will now try real time!\n");
                    inputVectorSize = b->size();
                    Stamp stmp;
                    inputPorts[k].getEnvelope(stmp);
                    if (stmp.isValid())
                        {     
                            if(VERBOSE) fprintf(stderr, "MESSAGE: will use real time!\n");
                            realTime = true;
                            initialTime = stmp.getTime();
                        }
                
                    //  Initialize data
                    for (int j = 0; j< numberOfPlots[k]; j++)
                        {
                            if (b->size()-1 < index[k][j])
                                if(VERBOSE) fprintf(stderr, "WARNING: will plot some zeros since the accessed index exceed the input vector dimensions!\n");
                            for (int i = 0; i < PLOT_SIZE; i++)
                                {
                                    d_x[i] = i;     // time axis
#ifdef ENABLE_REALTIME
                                    if (realTime)
                                        d_x_real_time[i] = i;
#endif
                                    //if(VERBOSE) fprintf(stderr, "MESSAGE: (%d, %d)\n", j, i);
                                    d_y[k][j][i] = 0;
                                }
                        }
                }

            //if(VERBOSE) fprintf(stderr, "MESSAGE: initializing plot datas!\n");
            // Assign a title
            insertLegend(new QwtLegend(), QwtPlot::BottomLegend);

            nonTimeBasedCurve[k] = new QwtPlotCurve[numberOfPlots[k]];
            for(int i=0; i < numberOfPlots[k]; i++)
                {
                    //Set title
                    char cTitle[256];
                    sprintf(cTitle, "Data(%d)", index[k][i]);
                    QwtText curveTitle(cTitle, QwtText::PlainText);
                    curveTitle.setFont(plotFont);
                    nonTimeBasedCurve[k][i].setTitle(curveTitle);

                    //if(VERBOSE) fprintf(stderr, "MESSAGE: Will now initialize the plot %d\n", index[i]);
                    // Insert new curves
                    nonTimeBasedCurve[k][i].attach(this);

                    // Set curve styles
                    nonTimeBasedCurve[k][i].setPen(coloredPens[k%NUMBER_OF_LIN][i%NUMBER_OF_COL]);

                    // Attach (don't copy) data. Both curves use the same x array.
                    nonTimeBasedCurve[k][i].setRawData(d_x, d_y[k][i], PLOT_SIZE);
                }

            // Axis 
            QwtText axisTitle("Time/seconds");
            axisTitle.setFont(plotFont);
            setAxisTitle(QwtPlot::xBottom, axisTitle);
            setAxisScale(QwtPlot::xBottom, 0, 100);
            setAxisFont(QwtPlot::xBottom, plotFont);

            setAxisTitle(QwtPlot::yLeft, "Values");
            //setAxisScale(QwtPlot::yLeft, -1.5, 1.5);
            setAxisAutoScale(QwtPlot::yLeft);
            setAxisAutoScale(QwtPlot::xBottom);
            setAxisFont(QwtPlot::yLeft, plotFont);

            setTimerInterval(50.0); 
        }
    //if(VERBOSE) fprintf(stderr, "MESSAGE: plot intialized!\n");
}