//ANALOG SENSOR
int GazeboYarpJointSensorsDriver::read(yarp::sig::Vector &out)
{
///< \todo TODO in my opinion the reader should care of passing a vector of the proper dimension to the driver, but apparently this is not the case
/*
if( (int)jointsensors_data.size() != jointsensors_nr_of_channels ||
(int)out.size() != jointsensors_nr_of_channels ) {
return AS_ERROR;
}
*/
if( (int)jointsensors_data.size() != jointsensors_nr_of_channels ) {
return AS_ERROR;
}
if( (int)out.size() != jointsensors_nr_of_channels ) {
std::cout << " GazeboYarpJointSensorsDriver:read() warning : resizing input vector, this can probably be avoided" << std::endl;
out.resize(jointsensors_nr_of_channels);
}
data_mutex.wait();
out = jointsensors_data;
data_mutex.post();
return AS_OK;
}
bool iDynTreetoYarp(const iDynTree::Wrench & iDynTreeWrench,yarp::sig::Vector & yarpVector)
{
if( yarpVector.size() != 6 ) { yarpVector.resize(6); }
memcpy(yarpVector.data(),iDynTreeWrench.getLinearVec3().data(),3*sizeof(double));
memcpy(yarpVector.data()+3,iDynTreeWrench.getAngularVec3().data(),3*sizeof(double));
return true;
}
void eulerAngles(const std::vector<yarp::sig::Vector> &edges1,const std::vector<yarp::sig::Vector> &edges2,const std::vector<yarp::sig::Vector> &crossProduct,yarp::sig::Vector &alpha,yarp::sig::Vector &beta,yarp::sig::Vector &gamma)
{
double singamma;
alpha.resize(crossProduct.size(),0.0);
beta.resize(crossProduct.size(),0.0);
gamma.resize(crossProduct.size(),0.0);
for (int i=0; i<crossProduct.size(); i++)
{
double value=crossProduct.at(i)[0];
beta[i]=asin(std::min(abs(value),1.0)*Helpers::sign(value));
if (value==1.0)
alpha[i]=asin(Helpers::sign(edges2.at(i)[2])*std::min(1.0,abs(edges2.at(i)[2])));
else
{
double tmp=crossProduct.at(i)[2]/cos(beta[i]);
alpha[i]=acos(Helpers::sign(tmp)*std::min(1.0,abs(tmp)));
if (Helpers::sign(crossProduct.at(i)[1])!=Helpers::sign(-sin(alpha[i])*cos(beta[i])))
alpha[i]=-alpha[i];
singamma=-edges2.at(i)[0]/cos(beta[i]);
if (edges1.at(i)[0]>=0)
gamma[i]=asin(Helpers::sign(singamma)*std::min(1.0,abs(singamma)));
else
gamma[i]=-M_PI-asin(Helpers::sign(singamma)*std::min(1.0,abs(singamma)));
}
}
}
void yarp_IMU_interface::sense(yarp::sig::Vector &orientation,
yarp::sig::Vector &linearAcceleration,
yarp::sig::Vector &angularVelocity)
{
this->sense();
orientation.resize(3);
orientation = _output.subVector(0,2);
linearAcceleration.resize(3);
linearAcceleration = _output.subVector(3,5);
angularVelocity.resize(3);
angularVelocity = _output.subVector(6,8);
}
void run()
{
Bottle *input = port.read();
if (input!=NULL)
{
if (first_run)
{
for (int i=0; i<input->size(); i++)
{
previous.resize(input->size());
current.resize(input->size());
diff.resize(input->size());
for (int i=0; i<input->size(); i++) current[i]=previous[i]=input->get(i).asDouble();
}
first_run=false;
}
bool print = false;
for (int i=0; i<input->size(); i++)
{
previous[i]=current[i];
current[i]=input->get(i).asDouble();
diff[i]=current[i]-previous[i];
tolerance = 10/100;
double percent = fabs(diff[i]/current[i]);
if (percent > tolerance)
{
fprintf(stdout,"ch: %d percent +6.6%f\n", i , percent);
print = true;
}
}
if (print == true)
{
for (int i=0; i<input->size(); i++)
{
fprintf(stdout,"+6.6%f ",diff[i]);
}
}
fprintf (stdout,"\n");
}
/*
static double time_old_wd=Time::now();
double time_wd=Time::now();
fprintf(stdout,"time%+3.3f ", time_wd-time_old_wd);
cout << " " << output.toString().c_str() << endl;
time_old_wd=time_wd;
*/
}
bool icubFinger::readEncoders(yarp::sig::Vector &encoderValues){
bool ret = false;
encoderValues.resize(3);
encoderValues.zero();
int pendingReads = _fingerEncoders->getPendingReads();
Bottle *handEnc = NULL;
for(int i = 0; i <= pendingReads; i++){
handEnc = _fingerEncoders->read();
}
if(handEnc != NULL)
{
encoderValues[0] = handEnc->get(_proximalEncoderIndex).asDouble();
encoderValues[1] = handEnc->get(_middleEncoderIndex).asDouble();
encoderValues[2] = handEnc->get(_distalEncoderIndex).asDouble();
ret = true;
adjustMinMax(encoderValues[0], _minProximal, _maxProximal);
adjustMinMax(encoderValues[1], _minMiddle, _maxMiddle);
adjustMinMax(encoderValues[2], _minDistal, _maxDistal);
}
//cout << "Encoders: " << encoderValues.toString() << endl;
return ret;
}
/**
* Some helper function for converting between Yarp,KDL and Eigen
* matrix types.
*
*/
bool toYarp(const KDL::Wrench & ft, yarp::sig::Vector & ft_yrp)
{
if( ft_yrp.size() != 6 ) { ft_yrp.resize(6); }
for(int i=0; i < 6; i++ ) {
ft_yrp[i] = ft[i];
}
}
/* Both ds and result should be of the same dimension */
bool TactileData::diff(TactileData &ds, yarp::sig::Vector &result, bool ifAbs)
{
double *data = this->data();
int size = this->size();
//check if dimensions of two DataSample match (though it will be always ;) )
if(ds.size() != size)
{
cout << "Dimensions do not match: " << ds.size() << " " << size << endl;
return false;
}
if(result.size() != size)
{
cout << "WARNING. Dimension of result vector is different. Reset it" << endl;
result.resize(size);
}
if(ifAbs)
{
for(int i = 0; i < size; i++)
{
result[i] = std::fabs(data[i] - ds[i]);
//result.push_back(this->data[i] - ds[i]);
}
}
else
{
for(int i = 0; i < size; i++)
{
result[i] = data[i] - ds[i];
//result.push_back(this->data[i] - ds[i]);
}
}
return true;
}
bool toYarp(const Vector3& iDynTreeVector3, yarp::sig::Vector& yarpVector)
{
if( yarpVector.size() != 3 )
{
yarpVector.resize(3);
}
memcpy(yarpVector.data(),iDynTreeVector3.data(),3*sizeof(double));
return true;
}
bool toYarp(const iDynTree::Position& iDynTreePosition, yarp::sig::Vector& yarpVector)
{
if( yarpVector.size() != 3 )
{
yarpVector.resize(3);
}
memcpy(yarpVector.data(),iDynTreePosition.data(),3*sizeof(double));
return true;
}
void MiddleFinger::getTactileDataComp(yarp::sig::Vector &tactileData){
tactileData.resize(12);
tactileData.zero();
Bottle *tactileDataPort = _tactileDataComp_in->read();
int startIndex = 12;
if(tactileDataPort != NULL){
for(int i = startIndex; i < startIndex + 12; ++i){
tactileData[i - startIndex] = tactileDataPort->get(i).asDouble();
}
}
}
bool IndexFinger::getPositionHandFrame(yarp::sig::Vector &position, yarp::sig::Vector &fingerEncoders){
bool ret = true;
Vector joints;
int nEncs;
position.clear();
position.resize(3); //x,y, z position
ret = ret && _armEncoder->getAxes(&nEncs);
Vector encs(nEncs);
if(! (ret = ret && _armEncoder->getEncoders(encs.data())))
{
cerr << _dbgtag << "Failed to read arm encoder data" << endl;
}
//cout << encs.toString() << endl;
ret = ret && _iCubFinger->getChainJoints(encs, joints);
if(ret == false){
cout << "failed to get chain joints" << endl;
return false;
}
// Replace the joins with the encoder readings
joints[0] = 20; // The index fingertip calibration procedure used this value.
joints[1] = 90 * (1 - (fingerEncoders[0] - _minProximal) / (_maxProximal - _minProximal) );
joints[2] = 90 * (1 - (fingerEncoders[1] - _minMiddle) / (_maxMiddle - _minMiddle) );
joints[3] = 90 * (1 - (fingerEncoders[2] - _minDistal) / (_maxDistal - _minDistal) );
//cout << joints.size() << endl;
//Convert the joints to radians.
for (int j = 0; j < joints.size(); j++)
joints[j] *= DEG2RAD;
yarp::sig::Matrix tipFrame = _iCubFinger->getH(joints);
position = tipFrame.getCol(3); // Tip's position in the hand coordinate
return ret;
}
bool KinectDriverOpenNI::get3DPoint(int u, int v, yarp::sig::Vector &point3D)
{
const XnDepthPixel* pDepthMap = depthGenerator.GetDepthMap();
XnPoint3D p2D, p3D;
int newU=u;
int newV=v;
//request arrives with respect to the 320x240 image, but the depth by default
// is 640x480 (we resize it before send it to the server)
if (device==KINECT_TAGS_DEVICE_KINECT)
{
newU=u*2;
newV=v*2;
}
p2D.X = newU;
p2D.Y = newV;
p2D.Z = pDepthMap[newV*this->def_depth_width+newU];
depthGenerator.ConvertProjectiveToRealWorld(1, &p2D, &p3D);
//We provide the 3D point in meters
point3D.resize(3,0.0);
point3D[0]=p3D.X/1000;
point3D[1]=p3D.Y/1000;
point3D[2]=p3D.Z/1000;
return true;
}
bool ObjectModelGrid::getNextSamplingPos(yarp::sig::Vector& nextSamplingPoint){
bool ret = false;
nextSamplingPoint.resize(3);
// Fix the x coordinate, move along the y
// increment x when y is at or beyon max
if(_nextSamplingPoint[1] < _yMax){
_nextSamplingPoint[1] += _stepSize;
// cap at max
if(_nextSamplingPoint[1] > _yMax){
_nextSamplingPoint[1] = _yMax;
}
ret = true;
}
else if(_nextSamplingPoint[0] > _xMin){
_nextSamplingPoint[0] -= _stepSize;
if(_nextSamplingPoint[0] < _xMin){
_nextSamplingPoint[0] = _xMin;
}
_nextSamplingPoint[1] = _yMin;
ret = true;
}
else{
ret = false;
}
nextSamplingPoint = _nextSamplingPoint;
return ret;
}
void icubFinger::getAngels(yarp::sig::Vector &angles, yarp::sig::Vector fingerEncoders){
angles.resize(3);
angles[0] = 90 * (1 - (fingerEncoders[0] - _minProximal) / (_maxProximal - _minProximal) );
angles[1] = 90 * (1 - (fingerEncoders[1] - _minMiddle) / (_maxMiddle - _minMiddle) );
angles[2] = 90 * (1 - (fingerEncoders[2] - _minDistal) / (_maxDistal - _minDistal) );
}
bool yarp::dev::FrameTransformClient::transformPose(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_pose, yarp::sig::Vector &transformed_pose)
{
if (input_pose.size() != 6)
{
yError() << "sorry.. only 6 dimensional vector (3 axes + roll pith and yaw) allowed, dear friend of mine..";
return false;
}
if (transformed_pose.size() != 6)
{
yWarning("FrameTransformClient::transformPose() performance warning: size transformed_pose should be 6, resizing");
transformed_pose.resize(6, 0.0);
}
yarp::sig::Matrix m(4, 4);
if (!getTransform(target_frame_id, source_frame_id, m))
{
yError() << "no transform found between source '" << target_frame_id << "' and target '" << source_frame_id << "'";
return false;
}
FrameTransform t;
t.transFromVec(input_pose[0], input_pose[1], input_pose[2]);
t.rotFromRPY(input_pose[3], input_pose[4], input_pose[5]);
t.fromMatrix(m * t.toMatrix());
transformed_pose[0] = t.translation.tX;
transformed_pose[1] = t.translation.tY;
transformed_pose[2] = t.translation.tZ;
yarp::sig::Vector rot;
rot = t.getRPYRot();
transformed_pose[3] = rot[0];
transformed_pose[4] = rot[1];
transformed_pose[5] = rot[2];
return true;
}
bool Vector_read(const std::string file_name, yarp::sig::Vector & vec)
{
FILE * fp;
uint32_t len;
double elem;
fp = fopen(file_name.c_str(),"rb");
if( fp == NULL ) return false;
fread(&len,sizeof(uint32_t),1,fp);
vec.resize(len);
for(size_t i=0;i<len;i++) {
fread(&elem,sizeof(double),1,fp);
vec[i]=elem;
}
/*
if( gsl_vector_fread(fp,(gsl_vector*)vec.getGslVector()) == GSL_EFAILED ) {
fclose(fp);
return false;
}*/
fclose(fp);
return true;
}
void DictionaryLearning::f_bow(std::vector<yarp::sig::Vector> & features, yarp::sig::Vector & code)
{
fprintf(stdout, "Starting bow coding \n");
double time=Time::now();
code.resize(dictionarySize);
code=0.0;
int n_features=features.size();
for (int feat_idx=0; feat_idx<n_features; feat_idx++)
{
float minNorm=(float)1e20;
int winnerAtom;
//Vector &A=features[feat_idx];
double *A2 = features[feat_idx].data();
//double time_atom=Time::now();
for(int atom_idx=0; atom_idx<dictionarySize; atom_idx++)
{
//Vector &B=dictionaryBOW[atom_idx];
double *B2 = dictionaryBOW[atom_idx].data();
//double time_norm=Time::now();
double norm=0.0;
for (int i=0; i<featuresize; i++)
{
//double d=A[i]-B[i];
double d=A2[i]-B2[i];
norm+=d*d;
if (norm > minNorm)
goto ciao;
}
if(norm<minNorm)
{
minNorm=norm;
winnerAtom=atom_idx;
}
ciao:{}
//time_norm=Time::now()-time_norm;
//fprintf(stdout, "norm %f \n",time_norm);
}
//time_atom=Time::now()-time_atom;
//fprintf(stdout, "%f \n",time_atom);
code[winnerAtom]++;
}
code=code/yarp::math::norm(code);
time=Time::now()-time;
fprintf(stdout, "%f \n",time);
}
void idyntreeMat2yarp(const iDynTreeVectorType & idyntreeVector,
yarp::sig::Vector & yarpVector)
{
yarpVector.resize(idyntreeVector.size());
for(size_t row=0; row < idyntreeVector.size(); row++)
{
yarpVector(row) = idyntreeVector(row);
}
}
bool embObjFTsensor::getTemperatureSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
{
GET_privData(mPriv).mutex.wait();
out.resize(1);
out[0] = GET_privData(mPriv).lastTemperature/10.0; //I need to convert from tenths of degree centigrade to degree centigrade
timestamp = GET_privData(mPriv).timestampTemperature;
GET_privData(mPriv).mutex.post();
return true;
}
bool toYarp(const Eigen::VectorXd & vec_eigen, yarp::sig::Vector & vec_yrp)
{
if( vec_yrp.size() != vec_eigen.size() ) { vec_yrp.resize(vec_eigen.size()); }
if( memcpy(vec_yrp.data(),vec_eigen.data(),sizeof(double)*vec_eigen.size()) != NULL ) {
return true;
} else {
return false;
}
}
void OrientationThread::getAngles(yarp::sig::Vector &angles, int nAngles)
{
angles.resize(nAngles);
double factor=360.0/nAngles;
double tmp=0.0;
for (int i=0; i<nAngles; i++)
{
angles[i]=tmp;
tmp+=factor;
}
}
bool Rangefinder2DInputPortProcessor::getData(yarp::sig::Vector &ranges)
{
mutex.wait();
if (lastBottle.size()==0) { mutex.post(); return false; }
unsigned int size = lastBottle.get(0).asList()->size();
ranges.resize(size);
for (unsigned int i = 0; i < size; i++)
ranges[i] = lastBottle.get(0).asList()->get(i).asDouble();
mutex.post();
return true;
}
/*! Read a vector from the sensor.
* @param out a vector containing the sensor's last readings.
* @return AS_OK or return code. AS_TIMEOUT if the sensor timed-out.
**/
int comanFTsensor::read(yarp::sig::Vector &out)
{
// This method gives data to the analogServer
mutex.wait();
status = AS_OK;
// TODO
int sensor_idx = 0;
out.resize(numberOfBoards * _channels);
for(int idx = 0; idx < numberOfBoards; idx++)
{
FtBoard *ftSensor = NULL;
sensor_idx = FTmap[idx];
ftSensor = _fts[sensor_idx]; // sensor_ids = boardId
if( NULL == ftSensor)
{
//yError() << "Trying to read data from a non-existing FT sensor " << sensor_idx << ", check your config file";
for(int tmp_idx = 0; tmp_idx < _channels; tmp_idx++)
out[idx*_channels + tmp_idx] = 0;
// Debug purpose, this way the output will be the boardId, to check if it is
// trying to read from the right board
out[idx*_channels + 0] = (double) sensor_idx;
out[idx*_channels + 1] = (double) sensor_idx;
out[idx*_channels + 2] = (double) sensor_idx;
out[idx*_channels + 3] = (double) sensor_idx;
out[idx*_channels + 4] = (double) sensor_idx;
out[idx*_channels + 5] = (double) sensor_idx;
continue;
}
ts_bc_data_t bc_data;
ft_bc_data_t &data = bc_data.raw_bc_data.ft_bc_data;
ftSensor->get_bc_data(bc_data);
// ci sono 6 valori da leggere per ogni FT, per ora!!
out[idx*_channels + 0] = (double)data.fx / scaleFactor[idx];
out[idx*_channels + 1] = (double)data.fy / scaleFactor[idx];
out[idx*_channels + 2] = (double)data.fz / scaleFactor[idx];
out[idx*_channels + 3] = (double)data.tx / scaleFactor[idx];
out[idx*_channels + 4] = (double)data.ty / scaleFactor[idx];
out[idx*_channels + 5] = (double)data.tz / scaleFactor[idx];
}
mutex.post();
return status;
}
/*! Read a vector from the sensor.
* @param out a vector containing the sensor's last readings.
* @return AS_OK or return code. AS_TIMEOUT if the sensor timed-out.
**/
int embObjAnalogSensor::read(yarp::sig::Vector &out)
{
// This method gives data to the analogServer
mutex.wait();
if (!data)
{
mutex.post();
return false;
}
// errors are not handled for now... it'll always be OK!!
if (status!=IAnalogSensor::AS_OK)
{
switch (status)
{
case IAnalogSensor::AS_OVF:
{
counterSat++;
}
break;
case IAnalogSensor::AS_ERROR:
{
counterError++;
}
break;
case IAnalogSensor::AS_TIMEOUT:
{
counterTimeout++;
}
break;
default:
{
counterError++;
}
}
mutex.post();
return status;
}
out.resize(data->size());
for(int k=0;k<data->size();k++)
{
out[k]=(*data)[k];
}
mutex.post();
return status;
}
int GazeboYarpContactLoadCellArrayDriver::read(yarp::sig::Vector& out)
{
yarp::os::LockGuard guard(m_dataMutex);
if (!m_dataAvailable)
{
return AS_TIMEOUT;
}
out.resize(m_contactNormalForces.size());
out = m_contactNormalForces;
return AS_OK;
}
/*! Read a vector from the sensor.
* @param out a vector containing the sensor's last readings.
* @return AS_OK or return code. AS_TIMEOUT if the sensor timed-out.
**/
int embObjInertials::read(yarp::sig::Vector &out)
{
// This method gives analogdata to the analogServer
if(false == opened)
{
return false;
}
mutex.wait();
// errors are not handled for now... it'll always be OK!!
if (status != IAnalogSensor::AS_OK)
{
switch (status)
{
case IAnalogSensor::AS_OVF:
{
counterSat++;
} break;
case IAnalogSensor::AS_ERROR:
{
counterError++;
} break;
case IAnalogSensor::AS_TIMEOUT:
{
counterTimeout++;
} break;
default:
{
counterError++;
} break;
}
mutex.post();
return status;
}
out.resize(analogdata.size());
for(int k=0; k<analogdata.size(); k++)
{
out[k] = analogdata[k];
}
mutex.post();
return status;
}
/*! Read a vector from the sensor.
* @param out a vector containing the sensor's last readings.
* @return AS_OK or return code. AS_TIMEOUT if the sensor timed-out.
**/
int RobotranYarpForceTorqueDriver::read(yarp::sig::Vector &out)
{
// This method gives data to the analogServer
mutex.wait();
status = AS_OK;
//data=MBSdataStruct.Qq[];
out.resize(data.size());
out = data;
mutex.post();
return status;
}
//---------------------------------------------------------
bool eigenToYarpVector(const Eigen::VectorXd &eigenVector, yarp::sig::Vector &yarpVector)
{
if(eigenVector.size() == 0)
{
cout<<"ERROR: input vector is empty (eigenToYarpVector)"<<endl;
return false;
}
//resize and fill eigen vector with yarp vector elements
yarpVector.resize(eigenVector.size());
for(unsigned int i=0; i <eigenVector.size(); i++)
yarpVector(i) = eigenVector(i);
return true;
};
bool MiddleFinger::getPositionHandFrame(yarp::sig::Vector &position, yarp::sig::Vector &fingerEncoders){
bool ret = true;
Vector joints;
int nEncs;
position.clear();
position.resize(3); //x,y, z position
ret = ret && _armEncoder->getAxes(&nEncs);
Vector encs(nEncs);
if(! (ret = ret && _armEncoder->getEncoders(encs.data())))
{
cerr << _dbgtag << "Failed to read arm encoder data" << endl;
}
//cout << encs.toString() << endl;
//cout << fingerEncoders.toString() << endl;
ret = ret && _iCubFinger->getChainJoints(encs, joints);
if(ret == false){
cout << "failed to get chain joints" << endl;
return false;
}
//std::cout << "From iCubFinger: " << joints.toString() << endl;
//This is where it is different from the index finger
joints[0] = 90 * (1 - (fingerEncoders[0] - _minProximal) / (_maxProximal - _minProximal) );
joints[1] = 90 * (1 - (fingerEncoders[1] - _minMiddle) / (_maxMiddle - _minMiddle) );
joints[2] = 90 * (1 - (fingerEncoders[2] - _minDistal) / (_maxDistal - _minDistal) );
//cout << "From mycalculat: " << joints.toString() << endl;
//cout << joints.size() << endl;
//Convert the joints to radians.
for (int j = 0; j < joints.size(); j++)
joints[j] *= M_PI/180;
yarp::sig::Matrix tipFrame = _iCubFinger->getH(joints);
position = tipFrame.getCol(3); // Tip's position in the hand coordinate
}
