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();
}
// 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;
}
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");
}