//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]; } }
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; }
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; }
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 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 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) ); }
/* 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 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); }
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 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; }
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; }
/*! 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; }
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 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 }