// BEGIN KAWIGIEDIT TESTING
// Generated by KawigiEdit 2.1.4 (beta) modified by pivanof
bool KawigiEdit_RunTest(int testNum, string p0, int p1, int p2, bool hasAnswer, int p3) {
cout << "Test " << testNum << ": [" << "\"" << p0 << "\"" << "," << p1 << "," << p2;
cout << "]" << endl;
Stamp *obj;
int answer;
obj = new Stamp();
clock_t startTime = clock();
answer = obj->getMinimumCost(p0, p1, p2);
clock_t endTime = clock();
delete obj;
bool res;
res = true;
cout << "Time: " << double(endTime - startTime) / CLOCKS_PER_SEC << " seconds" << endl;
if (hasAnswer) {
cout << "Desired answer:" << endl;
cout << "\t" << p3 << endl;
}
cout << "Your answer:" << endl;
cout << "\t" << answer << endl;
if (hasAnswer) {
res = answer == p3;
}
if (!res) {
cout << "DOESN'T MATCH!!!!" << endl;
} else if (double(endTime - startTime) / CLOCKS_PER_SEC >= 2) {
cout << "FAIL the timeout" << endl;
res = false;
} else if (hasAnswer) {
cout << "Match :-)" << endl;
} else {
cout << "OK, but is it right?" << endl;
}
cout << "" << endl;
return res;
}
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 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();
}
void InputPortProcessor::onRead(yarp::sig::Vector &v)
{
now=Time::now();
mutex.wait();
if (count>0)
{
double tmpDT=now-prev;
deltaT+=tmpDT;
if (tmpDT>deltaTMax)
deltaTMax=tmpDT;
if (tmpDT<deltaTMin)
deltaTMin=tmpDT;
//compare network time
if (tmpDT*1000<ANALOG_TIMEOUT)
{
state=IAnalogSensor::AS_OK;
}
else
{
state=IAnalogSensor::AS_TIMEOUT;
}
}
prev=now;
count++;
lastVector=v;
Stamp newStamp;
getEnvelope(newStamp);
//initialialization (first received data)
if (lastStamp.isValid()==false)
{
lastStamp = newStamp;
}
//now compare timestamps
if ((1000*(newStamp.getTime()-lastStamp.getTime()))<ANALOG_TIMEOUT)
{
state=IAnalogSensor::AS_OK;
}
else
{
state=IAnalogSensor::AS_TIMEOUT;
}
lastStamp = newStamp;
mutex.post();
}
void Rangefinder2DInputPortProcessor::onRead(yarp::os::Bottle &b)
{
now=SystemClock::nowSystem();
mutex.wait();
if (count>0)
{
double tmpDT=now-prev;
deltaT+=tmpDT;
if (tmpDT>deltaTMax)
deltaTMax=tmpDT;
if (tmpDT<deltaTMin)
deltaTMin=tmpDT;
//compare network time
if (tmpDT*1000<LASER_TIMEOUT)
{
state = b.get(1).asInt();
}
else
{
state = IRangefinder2D::DEVICE_TIMEOUT;
}
}
prev=now;
count++;
lastBottle=b;
Stamp newStamp;
getEnvelope(newStamp);
//initialialization (first received data)
if (lastStamp.isValid()==false)
{
lastStamp = newStamp;
}
//now compare timestamps
if ((1000*(newStamp.getTime()-lastStamp.getTime()))<LASER_TIMEOUT)
{
state = b.get(1).asInt();
}
else
{
state = IRangefinder2D::DEVICE_TIMEOUT;
}
lastStamp = newStamp;
mutex.post();
}
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;
}
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;
}
~Statistic()
{
const Debug::Stream Dbg("Core::RawScaner::Statistic");
const Stamp spent = Timer.Elapsed();
Dbg("Total processed: %1%", TotalData);
Dbg("Time spent: %1%", Time::Duration<Stamp::ValueType, Stamp>(1, spent).ToString());
const uint64_t useful = ArchivedData + ModulesData;
Dbg("Useful detected: %1% (%2% archived + %3% modules)", useful, ArchivedData, ModulesData);
Dbg("Coverage: %1%%%", useful * 100 / TotalData);
Dbg("Speed: %1% b/s", spent.Get() ? (TotalData * Stamp::PER_SECOND / spent.Get()) : TotalData);
StatisticBuilder<7> builder;
builder.Add(MakeStatLine(), 0);
StatItem total;
total.Name = "Total";
for (DetectMap::const_iterator it = Detection.begin(), lim = Detection.end(); it != lim; ++it)
{
builder.Add(MakeStatLine(it->second), 1 + it->second.Index);
total += it->second;
}
builder.Add(MakeStatLine(total), 1 + Detection.size());
Dbg(builder.Get().c_str());
}
//--------------------------------------------------------------
void testApp::setup()
{
ofSetFrameRate(60);
ofSetVerticalSync(true);
/// ram setup
// ------------------
ramInitialize(10000, true);
/// scenes setup
// ------------------
ramSceneManager& sceneManager = ramSceneManager::instance();
sceneManager.addScene( movingCam.getPtr() );
sceneManager.addScene( drawLines.getPtr() );
sceneManager.addScene( bigbox.getPtr() );
sceneManager.addScene( future.getPtr() );
sceneManager.addScene( donuts.getPtr() );
sceneManager.addScene( stamp.getPtr() );
sceneManager.addScene( expansion.getPtr() );
// ignore win32
#ifndef TARGET_WIN32
sceneManager.addScene( particles.getPtr() );
#endif
sceneManager.addScene( abacus.getPtr() );
sceneManager.addScene( soundcube.getPtr() );
sceneManager.addScene( upsideDown.getPtr() );
sceneManager.addScene( kepler.getPtr() );
sceneManager.addScene( hastyChase.getPtr() );
sceneManager.addScene( colorGrid.getPtr() );
sceneManager.addScene( threePoints.getPtr() );
sceneManager.addScene( fourPoints.getPtr() );
sceneManager.addScene( chain.getPtr() );
sceneManager.addScene( monster.getPtr() );
sceneManager.addScene( laban.getPtr() );
sceneManager.addScene( notation.getPtr() );
}
void SpriteSheet::ExportStampTiles(const PlatformConfig& config, const Stamp& referenceStamp, std::stringstream& stream, const std::string& actorName) const
{
#if defined _DEBUG
ion::debug::Assert(referenceStamp.CheckTilesBatched(), "SpriteSheet::ExportStampTiles() - Tiles not in sequential order");
#endif
//Get all unique tiles
std::vector<TileId> tileIndices;
int numUniqueTiles = referenceStamp.GetSortedUniqueTileBatch(tileIndices);
//Offset by first index
int firstIndex = tileIndices[0];
int frameIdx = 0;
for(std::vector<SpriteSheetFrame>::const_iterator it = m_frames.begin(), end = m_frames.end(); it != end; ++it, ++frameIdx)
{
std::stringstream label;
label << "actor_" << actorName << "_sheet_" << m_name << "_frame_" << frameIdx;
stream << label.str() << ":" << std::endl;
//Export in sorted unique order
for(int i = 0; i < tileIndices.size(); i++)
{
//Find index of first use of this tile id in reference stamp
int tileIndex = referenceStamp.GetTileIndex(tileIndices[i]);
//Transpose to sprite (column major) order
ion::Vector2i pos(tileIndex / referenceStamp.GetWidth(), tileIndex % referenceStamp.GetWidth());
int rowMajorIndex = (pos.y * referenceStamp.GetHeight()) + pos.x;
//Export tile of animation frame
(*it)[rowMajorIndex].Export(config, stream);
stream << std::endl;
}
}
}
bool EdisonSegmModule::updateModule()
{
ImageOf<PixelRgb> *yrpImgIn;
static int cycles = 0;
yrpImgIn = _imgPort.read();
if (yrpImgIn == NULL) // this is the case if module is requested to quit while waiting for image
return true;
bool use_private_stamp;
Stamp s;
if(!_imgPort.getEnvelope(s))
{
cout << "No stamp found in input image. Will use private stamp" << endl;
use_private_stamp = true;
}
else
{
cout << "Received image #" << s.getCount() << " generated at time " << s.getTime() << endl;
use_private_stamp = false;
}
if(cycles == 0)
_timestart = yarp::os::Time::now();
cycles++;
IplImage *iplimg = (IplImage*)yrpImgIn->getIplImage();
//computing the ROI to crop the image
/*struct _IplROI roi;
roi.coi = 0; // all channels are selected
roi.height = height_;
roi.width = width_;
roi.xOffset = ( orig_width_ - width_ ) / 2;
roi.yOffset = ( orig_height_ - height_ ) / 2;*/
//copying roi data to buffer
/*iplimg->roi = &roi;
cvCopy( iplimg, inputImage.getIplImage());*/
//Rescale image if required
if( (width_ != orig_width_) || (height_ != orig_height_ ) )
cvResize(iplimg, inputImage.getIplImage(), CV_INTER_NN);
else
cvCopy( iplimg, inputImage.getIplImage());
double edgetime = yarp::os::Time::now();
//compute gradient and confidence maps
BgEdgeDetect edgeDetector(gradWindRad);
BgImage bgImage;
bgImage.SetImage(inputImage_, width_, height_, true);
edgeDetector.ComputeEdgeInfo(&bgImage, confMap_, gradMap_);
//compute the weigth map
for(int i = 0; i < width_*height_; i++) {
if(gradMap_[i] > 0.02) {
weightMap_[i] = mixture*gradMap_[i] + (1 - mixture)*confMap_[i];
} else {
weightMap_[i] = 0;
}
}
///////////////////////////// This block can be parallelized
cout << "Edge computation Time (ms): " << (yarp::os::Time::now() - edgetime)*1000.0 << endl;
msImageProcessor iProc;
if( dim_ == 3 )
iProc.DefineImage(inputImage_, COLOR, height_, width_);
else
{
cvCvtColor(inputImage.getIplImage(), inputHsv.getIplImage(), CV_RGB2HSV);
cvSplit(inputHsv.getIplImage(), inputHue.getIplImage(), 0, 0, 0);
iProc.DefineImage(inputHue_, GRAYSCALE, height_, width_);
}
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
iProc.SetWeightMap(weightMap_, threshold);
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
double filtertime = yarp::os::Time::now();
iProc.Filter(sigmaS, sigmaR, speedup);
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
cout << "Mean Shift Filter Computation Time (ms): " << (yarp::os::Time::now() - filtertime)*1000.0 << endl;
//obtain the filtered image
iProc.GetResults(filtImage_);
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
//fuse regions
double fusetime = yarp::os::Time::now();
iProc.FuseRegions(sigmaR, minRegion);
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
cout << "Region Fusion Computation Time (ms): " << (yarp::os::Time::now() - fusetime)*1000.0 << endl;
//obtain the segmented image
iProc.GetResults(segmImage_);
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
//define the boundaries - do not need this
/*
RegionList *regionList = iProc.GetBoundaries();
int *regionIndeces = regionList->GetRegionIndeces(0);
int numRegions = regionList->GetNumRegions();
numBoundaries_ = 0;
for(int i = 0; i < numRegions; i++) {
numBoundaries_ += regionList->GetRegionCount(i);
}
if(boundaries_) delete [] boundaries_;
boundaries_ = new int [numBoundaries_];
for(int i = 0; i < numBoundaries_; i++) {
boundaries_[i] = regionIndeces[i];
}*/
int regionCount; // how many regions have been found
int *labels = NULL; //pointer for the labels (should this be released in the end?)
float *modes; //pointer for the Luv values (should this be released in the end?)
int *modePointCounts; //the area of each region (should this be released in the end?)
regionCount = iProc.GetRegions(&labels, &modes, &modePointCounts);
int *labelp = (int*)labelImage.getRawImage();
for(int i = 0; i < width_*height_; i++)
labelp[i] = labels[i];
IplImage *labelint = (IplImage*)labelImage.getIplImage();
IplImage *labelchar = (IplImage*)labelView.getIplImage();
cvConvert(labelint, labelchar);
//prepare timestamps
if(use_private_stamp)
{
_stamp.update();
_labelPort.setEnvelope(_stamp);
_labelViewPort.setEnvelope(_stamp);
_viewPort.setEnvelope(_stamp);
_filtPort.setEnvelope(_stamp);
_rawPort.setEnvelope(_stamp);
}
else
{
_labelPort.setEnvelope(s);
_labelViewPort.setEnvelope(s);
_viewPort.setEnvelope(s);
_filtPort.setEnvelope(s);
_rawPort.setEnvelope(s);
}
ImageOf<PixelInt> &yrpImgLabel = _labelPort.prepare();
//Rescale image if required
if( (width_ != orig_width_) || (height_ != orig_height_ ) )
{
yrpImgLabel.resize(orig_width_, orig_height_);
cvResize(labelImage.getIplImage(), yrpImgLabel.getIplImage(), CV_INTER_NN);
}
else
yrpImgLabel = labelImage;
_labelPort.write();
ImageOf<PixelMono> &yrpImgDebug = _labelViewPort.prepare();
//Rescale image if required
if( (width_ != orig_width_) || (height_ != orig_height_ ) )
{
yrpImgDebug.resize(orig_width_, orig_height_);
cvResize(labelView.getIplImage(), yrpImgDebug.getIplImage(), CV_INTER_NN);
}
else
yrpImgDebug = labelView;
_labelViewPort.write();
ImageOf<PixelRgb> &yrpFiltOut = _filtPort.prepare();
//Rescale image if required
if( (width_ != orig_width_) || (height_ != orig_height_ ) )
{
yrpFiltOut.resize(orig_width_, orig_height_);
cvResize(filtImage.getIplImage(), yrpFiltOut.getIplImage(), CV_INTER_NN);
}
else
yrpFiltOut = filtImage;
_filtPort.write();
ImageOf<PixelRgb> &yrpImgView = _viewPort.prepare();
//Rescale image if required
if( (width_ != orig_width_) || (height_ != orig_height_ ) )
{
yrpImgView.resize(orig_width_, orig_height_);
cvResize(segmImage.getIplImage(), yrpImgView.getIplImage(), CV_INTER_NN);
}
else
yrpImgView = segmImage;
_viewPort.write();
ImageOf<PixelRgb> &yrpImgOut = _rawPort.prepare();
yrpImgOut = *yrpImgIn;
_rawPort.write();
//report the frame rate
if(cycles % 100 == 0)
{
double cps = ((double)cycles)/(yarp::os::Time::now() - _timestart);
printf("fps: %02.2f\n", cps);
}
return true;
}
void stereoCalibThread::stereoCalibRun()
{
imageL=new ImageOf<PixelRgb>;
imageR=new ImageOf<PixelRgb>;
Stamp TSLeft;
Stamp TSRight;
bool initL=false;
bool initR=false;
int count=1;
Size boardSize, imageSize;
boardSize.width=this->boardWidth;
boardSize.height=this->boardHeight;
while (!isStopping()) {
ImageOf<PixelRgb> *tmpL = imagePortInLeft.read(false);
ImageOf<PixelRgb> *tmpR = imagePortInRight.read(false);
if(tmpL!=NULL)
{
*imageL=*tmpL;
imagePortInLeft.getEnvelope(TSLeft);
initL=true;
}
if(tmpR!=NULL)
{
*imageR=*tmpR;
imagePortInRight.getEnvelope(TSRight);
initR=true;
}
if(initL && initR && checkTS(TSLeft.getTime(),TSRight.getTime())){
bool foundL=false;
bool foundR=false;
mutex->wait();
if(startCalibration>0) {
string pathImg=imageDir;
preparePath(pathImg.c_str(), pathL,pathR,count);
string iml(pathL);
string imr(pathR);
imgL= (IplImage*) imageL->getIplImage();
imgR= (IplImage*) imageR->getIplImage();
Mat Left(imgL);
Mat Right(imgR);
std::vector<Point2f> pointbufL;
std::vector<Point2f> pointbufR;
if(boardType == "CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else {
foundL = findChessboardCorners( Left, boardSize, pointbufL, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
foundR = findChessboardCorners( Right, boardSize, pointbufR, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
}
if(foundL && foundR) {
cvCvtColor(imgL,imgL,CV_RGB2BGR);
cvCvtColor(imgR,imgR, CV_RGB2BGR);
saveStereoImage(pathImg.c_str(),imgL,imgR,count);
imageListR.push_back(imr);
imageListL.push_back(iml);
imageListLR.push_back(iml);
imageListLR.push_back(imr);
Mat cL(pointbufL);
Mat cR(pointbufR);
drawChessboardCorners(Left, boardSize, cL, foundL);
drawChessboardCorners(Right, boardSize, cR, foundR);
count++;
}
if(count>numOfPairs) {
fprintf(stdout," Running Left Camera Calibration... \n");
monoCalibration(imageListL,this->boardWidth,this->boardHeight,this->Kleft,this->DistL);
fprintf(stdout," Running Right Camera Calibration... \n");
monoCalibration(imageListR,this->boardWidth,this->boardHeight,this->Kright,this->DistR);
stereoCalibration(imageListLR, this->boardWidth,this->boardHeight,this->squareSize);
fprintf(stdout," Saving Calibration Results... \n");
updateIntrinsics(imgL->width,imgL->height,Kright.at<double>(0,0),Kright.at<double>(1,1),Kright.at<double>(0,2),Kright.at<double>(1,2),DistR.at<double>(0,0),DistR.at<double>(0,1),DistR.at<double>(0,2),DistR.at<double>(0,3),"CAMERA_CALIBRATION_RIGHT");
updateIntrinsics(imgL->width,imgL->height,Kleft.at<double>(0,0),Kleft.at<double>(1,1),Kleft.at<double>(0,2),Kleft.at<double>(1,2),DistL.at<double>(0,0),DistL.at<double>(0,1),DistL.at<double>(0,2),DistL.at<double>(0,3),"CAMERA_CALIBRATION_LEFT");
Mat Rot=Mat::eye(3,3,CV_64FC1);
Mat Tr=Mat::zeros(3,1,CV_64FC1);
updateExtrinsics(this->R,this->T,"STEREO_DISPARITY");
fprintf(stdout, "Calibration Results Saved in %s \n", camCalibFile.c_str());
startCalibration=0;
count=1;
imageListR.clear();
imageListL.clear();
imageListLR.clear();
}
}
mutex->post();
ImageOf<PixelRgb>& outimL=outPortLeft.prepare();
outimL=*imageL;
outPortLeft.write();
ImageOf<PixelRgb>& outimR=outPortRight.prepare();
outimR=*imageR;
outPortRight.write();
if(foundL && foundR && startCalibration==1)
Time::delay(2.0);
initL=initR=false;
cout.flush();
}
}
delete imageL;
delete imageR;
}
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");
}
// 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;
}
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]);
}
}
}
int main(int argc, char *argv[]) {
//Declair yarp
Network yarp;
if (!yarp.checkNetwork())
{
printf("YARP server not available!\n");
return -1;
}
//Creating a Resiving yarp port
BufferedPort<Sound> bufferPort;
bufferPort.open("/receiver");
if (!Network::exists("/receiver")) {
printf("receiver not exists \n");
return -1;
}
//Connecting the resiving audio port with the sender on the pc104
Network::connect("/sender", "/receiver");
if (!Network::isConnected("/sender", "/receiver")) {
printf("no connection \n");
return -1;
}
double empty[samplingBuffer *4] = {0}; //plus 2 for time and sample stamps
//Setup for memory mapping
FILE *fid;
fid = fopen("/tmp/AudioMemMap.tmp", "w");
fwrite(empty, sizeof(double), samplingBuffer * 4, fid);
fclose(fid);
int mappedFileID;
mappedFileID = open("/tmp/AudioMemMap.tmp", O_RDWR);
double *mappedAudioData;
mappedAudioData = (double *)mmap(0, memoryMapSize_bytes, PROT_WRITE, MAP_SHARED , mappedFileID, 0);
//Main loop that maps the sound to the memory mapped region delaired above
Sound* s;
Stamp ts;
while (true) {
//This is a blocking read
s = bufferPort.read(true);
bufferPort.getEnvelope(ts);
printf("count:%d time:%f \n", ts.getCount(), ts.getTime());
int e0 = ts.getCount();
double e1 = ts.getTime();
int row = 0;
for (int col = 0 ; col < samplingBuffer; col++) {
NetInt16 temp_c = (NetInt16) s->get(col, 0);
NetInt16 temp_d = (NetInt16) s->get(col, 1);
mappedAudioData[row] = (double) temp_c / normDivid ;
mappedAudioData[row + 1] = (double) temp_d / normDivid;
mappedAudioData[row + 2] = (double) (e0 * samplingBuffer) + col;
mappedAudioData[row + 3] = (double) (e1 + col * samplePeriod);
row += 4;
}
}
return 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
}
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();
}
bool GBSegmModule::updateModule()
{
ImageOf<PixelRgb> *yrpImgIn;
static int cycles = 0;
yrpImgIn = _imgPort.read();
if (yrpImgIn == NULL) // this is the case if module is requested to quit while waiting for image
return true;
bool use_private_stamp;
Stamp s;
if(!_imgPort.getEnvelope(s))
{
cout << "No stamp found in input image. Will use private stamp" << endl;
use_private_stamp = true;
}
else
{
cout << "Received image #" << s.getCount() << " generated at time " << s.getTime() << endl;
use_private_stamp = false;
}
if(cycles == 0)
_timestart = yarp::os::Time::now();
cycles++;
//IplImage *iplimg = (IplImage*)yrpImgIn->getIplImage();
cout << "converting image of size " << yrpImgIn->width() << yrpImgIn->height() <<" to size" << input->width() << input->height() << endl;
YarpImageToRGBImage(input, yrpImgIn);
cout << "converted" << endl;
segMutex.wait();
if(seg)
delete seg;
seg=segment_image(input, sigma, k, min_size, &num_components);
segMutex.post();
cout << "processed" << endl;
//prepare timestamps
if(use_private_stamp)
{
_stamp.update();
_viewPort.setEnvelope(_stamp);
}
else
{
_viewPort.setEnvelope(s);
}
ImageOf<PixelRgb> &yrpImgView = _viewPort.prepare();
//Rescale image if required
yrpImgView.resize(seg->width(), seg->height());
RGBImageToYarpImage(seg, &yrpImgView);
_viewPort.write();
//report the frame rate
if(cycles % 100 == 0)
{
double cps = ((double)cycles)/(yarp::os::Time::now() - _timestart);
printf("fps: %02.2f\n", cps);
}
return true;
}
//--------------------------------------------------------
// Used to apply deformations to the given heightmap
void
Deform::displace_heightmap(TexData texdata, vector2 clickPos, vector2 clickOffset, vector4 SIRM, bool isCoarse, string stampName, GLuint copySrcTex)
{
GLuint backupTex;
int currentViewport[4];
int dim;
int copyX, copyY;
int copyW, copyH;
float metre_scale;
matrix2 stampRot;
Stamp *stamp;
GLuint shaderID;
// Setup transforms
SIRM.x = 0.5f * SIRM.x; // scale in metres
stampRot = rotate_tr2(SIRM.z); // rotation
// Stamp controls
if (stampName.compare("") == 0)
stamp = GetStampMan()->GetCurrentStamp();
else
stamp = GetStampMan()->GetStamp(stampName);
shaderID = stamp->GetShaderID();
// Setup variables dependent on whether it is Coarse or High detail deformation
if (isCoarse)
{
dim = m_coarseDim;
metre_scale = m_metre_to_tex;
SIRM.x = SIRM.x * metre_scale;
// Scale because the coarsemap uses float textures which aren't normalized to [0, 1]
SIRM.y = SIRM.y * VERT_SCALE;
clickPos = (clickPos * metre_scale) + vector2(0.5f) + clickOffset;
backupTex = m_coarseBackup;
}
else
{
dim = m_highDim;
metre_scale = m_metre_to_detail_tex;
SIRM.x = SIRM.x * metre_scale;
clickPos = (clickPos * metre_scale) + clickOffset;
backupTex = m_highBackup;
}
// Calculate bounds of the render area as texel coordinates where X,Y in [0, dim-1]
copyW = (int)ceil(2.9f * SIRM.x * dim) ;
copyH = (int)ceil(2.9f * SIRM.x * dim);
copyX = max(0, (int)(dim * clickPos.x) - copyW / 2);
copyY = max(0, (int)(dim * clickPos.y) - copyH / 2);
// Make sure it's not out of bounds
copyW = copyX + copyW > dim-1 ? dim - copyX : copyW;
copyH = copyY + copyH > dim-1 ? dim - copyY : copyH;
////// PHASE 1: Copy To Backup
//////////////////////////////
// For HD textures, we need to copy the heightmap into a double buffer for it to read from
// For Coarsemap, we only need to do a copy the first time as it does not share the texture
if (!isCoarse || !m_initialised) {
// Setup the Read framebuffer
glBindFramebuffer(GL_READ_FRAMEBUFFER, m_fbo_heightmap);
glReadBuffer(GL_COLOR_ATTACHMENT0);
// Set up textures and attachments for the copy
if (copySrcTex != 0){
// If we're writing to an HD for the first time, we need to fill it with the Zero tex
glFramebufferTexture2D(GL_READ_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D,
copySrcTex, 0);
glBindTexture(GL_TEXTURE_2D, texdata.heightmap);
// Use the Zero texture to read the current state from
backupTex = copySrcTex;
}
else {
// Otherwise copy the current state of the render region into the backup
glFramebufferTexture2D(GL_READ_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D,
texdata.heightmap, 0);
glBindTexture(GL_TEXTURE_2D, backupTex);
}
// Perform copy
if (isCoarse || copySrcTex != 0)
glCopyTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, 0, 0, dim, dim);
else
//glCopyTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, 0, 0, dim, dim);
glCopyTexSubImage2D(GL_TEXTURE_2D, 0, copyX, copyY, copyX, copyY, copyW, copyH);
// Unbind FBO, texture and regenerate mipmap
glBindFramebuffer(GL_READ_FRAMEBUFFER, 0);
glGenerateMipmap(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, 0);
// set that has been initialised
m_initialised |= isCoarse;
}
////// PHASE 2: Render to Texture
/////////////////////////////////
// First we set up the Framebuffer and it's viewport and bind our target attachment
glGetIntegerv(GL_VIEWPORT, currentViewport);
glViewport(0, 0, dim, dim);
glBindFramebuffer(GL_DRAW_FRAMEBUFFER, m_fbo_heightmap);
glFramebufferTexture2D(GL_DRAW_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D,
texdata.heightmap, 0);
glDrawBuffer(GL_COLOR_ATTACHMENT0);
// Bind the source texture
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, backupTex);
//Bind the stamp texture if it uses one
if (stamp->IsTexStamp())
{
glActiveTexture(GL_TEXTURE1);
stamp->BindTexture();
}
// Bind the shader and set the uniform values
glUseProgram(shaderID);
glUniform2fv(glGetUniformLocation(shaderID, "clickPos"), 1, clickPos.v);
glUniform2f (glGetUniformLocation(shaderID, "stamp_scale"), SIRM.x, SIRM.x);
glUniformMatrix2fv(glGetUniformLocation(shaderID, "stamp_rotation"), 1, GL_FALSE, stampRot.m);
if (stamp->IsTexStamp())
{
// Controls for mirroring the texture
vector2 mirror(0.0f, -1.0f);
if (SIRM.w != 0)
mirror = vector2(1.0f);
glUniform2fv(glGetUniformLocation(shaderID, "stamp_mirror"), 1, mirror.v);
}
glUniform1f(glGetUniformLocation(shaderID, "intensity"), SIRM.y);
// Call a predefined function if need be
if (stamp->initShader)
stamp->initShader(stamp, clickPos, SIRM.x, SIRM.y);
// Bind the Vertex Array Object containing the Render Quad and its texture coordinates
glBindVertexArray(m_vao);
// RENDER to (DEFORM) the heightmap
glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
// Reset viewport and framebuffer as it was
glViewport(currentViewport[0], currentViewport[1], currentViewport[2], currentViewport[3]);
glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0);
////// PHASE 3: Regenerate Mipmap and Copy Backup
/////////////////////////////////////////////////
glBindTexture(GL_TEXTURE_2D, texdata.heightmap);
glGenerateMipmap(GL_TEXTURE_2D);
// If it's the Coarsemap, we must copy these changes into its double buffer (backup texture)
if (isCoarse)
{
// Setup textures to copy heightmap changes to the other map
glBindFramebuffer(GL_READ_FRAMEBUFFER, m_fbo_heightmap);
glFramebufferTexture2D(GL_READ_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D,
texdata.heightmap, 0);
glReadBuffer(GL_COLOR_ATTACHMENT0);
glBindTexture(GL_TEXTURE_2D, m_coarseBackup);
// Copy the changed subimage
glCopyTexSubImage2D(GL_TEXTURE_2D, 0, copyX, copyY, copyX, copyY, copyW, copyH);
glBindFramebuffer(GL_READ_FRAMEBUFFER, 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;
}