bool toEigen(const yarp::sig::Vector & vec_yrp, Eigen::VectorXd & vec_eigen)
{
if( vec_yrp.size() != vec_eigen.size() ) { vec_eigen.resize(vec_yrp.size()); }
if( memcpy(vec_eigen.data(),vec_yrp.data(),sizeof(double)*vec_eigen.size()) != NULL ) {
return true;
} else {
return false;
}
}
void OpenSoT::tasks::velocity::Postural::setReference(const yarp::sig::Vector &x_desired,
const yarp::sig::Vector &xdot_desired)
{
assert(x_desired.size() == _x_size);
assert(xdot_desired.size() == _x_size);
_x_desired = x_desired;
_xdot_desired = xdot_desired;
this->update_b();
}
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;
}
//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;
}
void ObjectModelGrid::addContactPoint(const yarp::sig::Vector fingertipPosition){
assert(fingertipPosition.size() == 3);
cout << "Contact position: " << fingertipPosition.toString() << endl;
_contactPoints.push_back(fingertipPosition);
}
bool toiDynTree(const yarp::sig::Vector& yarpVector, iDynTree::Vector3& iDynTreeVector3)
{
if( yarpVector.size() != 3 )
{
return false;
}
memcpy(iDynTreeVector3.data(),yarpVector.data(),3*sizeof(double));
return true;
}
bool LinearMean::setHyperparameters(const yarp::sig::Vector newHyperPar)
{
if (newHyperPar.size()!=this->getNumberOfHyperparameters())
return false;
meanLinearHyperparam=newHyperPar.subVector(0, newHyperPar.size()-2);
meanOffset=newHyperPar(newHyperPar.size()-1);
return true;
}
bool isEqual(const yarp::sig::Vector& a, const yarp::sig::Vector& b, const double& tolerance)
{
if (a.size() != b.size()) return false;
for (size_t i = 0; i < a.size(); i++)
{
if (fabs(a[i] - b[i]) > tolerance)
{
return false;
}
}
return true;
}
bool yarpWholeBodyModelV1::convertGeneralizedTorques(yarp::sig::Vector idyntree_base_force, yarp::sig::Vector idyntree_torques, double * tau)
{
if( idyntree_base_force.size() != 6 && (int)idyntree_torques.size() != p_model->getNrOfDOFs() ) { return false; }
for(int j = 0; j < 6; j++ ) {
tau[j] = idyntree_base_force[j];
}
for(int wbi_joint_numeric_id=0; wbi_joint_numeric_id < this->dof; wbi_joint_numeric_id++ )
{
tau[wbi_joint_numeric_id+6] = idyntree_torques[wbiToiDynTreeJointId[wbi_joint_numeric_id]];
}
return true;
}
//---------------------------------------------------------
bool yarpToEigenVector(const yarp::sig::Vector &yarpVector, Eigen::VectorXd &eigenVector)
{
if(yarpVector.size() == 0)
{
cout<<"ERROR: input vector is empty (yarpToEigenVector)"<<endl;
return false;
}
//resize and fill eigen vector with yarp vector elements
eigenVector.resize(yarpVector.size());
for(unsigned int i = 0; i < yarpVector.size(); i++)
eigenVector(i) = yarpVector(i);
return true;
};
void MultiTaskLinearGPRLearner::setWeightsStandardDeviation(const yarp::sig::Vector& s) {
if( s.size() != this->getDomainCols() ) {
throw std::runtime_error("MultiTaskLinearGPRLearner: wrong dimension of noise std deviation");
}
inv_Sigma_w = zeros(this->getDomainCols(),this->getDomainCols());
for(unsigned i=0; i < s.size(); i++ ) {
inv_Sigma_w(i,i) = 1/(s[i]*s[i]);
}
this->weight_prior_indefinite = false;
this->reset();
}
bool RobotUtils::setReferenceSpeeds(const yarp::sig::Vector &maximum_velocity)
{
assert(maximum_velocity.size() == this->getNumberOfJoints());
if(!bodyIsInPositionMode()) {
std::cout << "Trying to set reference speeds for the whole coman "
<< "but the robot is not entirely in Position Mode";
return false;
}
yarp::sig::Vector velocity_torso,
velocity_right_arm,
velocity_left_arm,
velocity_right_leg,
velocity_left_leg,
velocity_head;
idynutils.fromIDynToRobot(maximum_velocity, velocity_torso, idynutils.torso);
idynutils.fromIDynToRobot(maximum_velocity, velocity_right_arm, idynutils.right_arm);
idynutils.fromIDynToRobot(maximum_velocity, velocity_left_arm, idynutils.left_arm);
idynutils.fromIDynToRobot(maximum_velocity, velocity_right_leg, idynutils.right_leg);
idynutils.fromIDynToRobot(maximum_velocity, velocity_left_leg, idynutils.left_leg);
if(head.isAvailable) idynutils.fromIDynToRobot(maximum_velocity, velocity_head, idynutils.head);
return torso.setReferenceSpeeds(velocity_torso) &&
right_arm.setReferenceSpeeds(velocity_right_arm) &&
left_arm.setReferenceSpeeds(velocity_left_arm) &&
right_leg.setReferenceSpeeds(velocity_right_leg) &&
left_leg.setReferenceSpeeds(velocity_left_leg) &&
(head.isAvailable || head.setReferenceSpeeds(velocity_head));
}
/**
* 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 MathHelper::yarpToEigenVector(const yarp::sig::Vector& yarpVector, Eigen::VectorXd& eigenVector)
{
if(yarpVector.size() == 0)
{
printf ("ERROR: input vector is empty.\n");
return false;
}
//resize and fill eigen vector with yarp vector elements
eigenVector.resize(yarpVector.size());
for(unsigned int i = 0; i < yarpVector.size(); i++)
{
eigenVector(i) = yarpVector(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;
}
bool toiDynTree(const yarp::sig::Vector& yarpVector, VectorDynSize& iDynTreeVector)
{
iDynTreeVector.resize(yarpVector.size());
memcpy(iDynTreeVector.data(),yarpVector.data(),yarpVector.size()*sizeof(double));
return true;
}
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;
}
void Postural::setReference(const yarp::sig::Vector& x_desired) {
assert(x_desired.size() == _x_size);
_x_desired = x_desired;
_xdot_desired.zero();
this->update_b();
}
Postural::Postural( const yarp::sig::Vector& x) :
Task("Postural", x.size()), _x(x),
_x_desired(x.size(),0.0), _xdot_desired(x.size(),0.0)
{
_W.resize(_x_size, _x_size);
_W.eye();
_A.resize(_x_size, _x_size);
_A.eye();
_hessianType = HST_IDENTITY;
/* first update. Setting desired pose equal to the actual pose */
this->setReference(x);
this->_update(x);
}
void MultiTaskLinearGPRLearner::setNoiseStandardDeviation(const yarp::sig::Vector& s) {
if( s.size() != this->getDomainRows() ) {
throw std::runtime_error("MultiTaskLinearGPRLearner: wrong dimension of noise std deviation");
}
inv_Sigma_n = zeros(this->getDomainRows(),this->getDomainRows());
for(unsigned i=0; i < s.size(); i++ ) {
inv_Sigma_n(i,i) = 1/(s[i]*s[i]);
}
this->no_output_error = false;
this->reset();
}
/* 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 YarptoiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::Wrench & iDynTreeWrench)
{
if( yarpVector.size() != 6 ) return false;
memcpy(iDynTreeWrench.getLinearVec3().data(),yarpVector.data(),3*sizeof(double));
memcpy(iDynTreeWrench.getAngularVec3().data(),yarpVector.data()+3,3*sizeof(double));
return true;
}
bool Vector_write(const std::string file_name, const yarp::sig::Vector & vec)
{
FILE * fp;
uint32_t len;
double elem;
len = vec.size();
fp = fopen(file_name.c_str(),"wb");
if( fp == NULL ) return false;
fwrite(&len,sizeof(uint32_t),1,fp);
for(size_t i=0;i<len;i++) {
elem = vec[i];
fwrite(&elem,sizeof(double),1,fp);
}
/*
if( gsl_vector_fwrite(fp,(gsl_vector *)vec.getGslVector()) == GSL_EFAILED ) {
fclose(fp);
return false;
}*/
fclose(fp);
return true;
}
void iCub::skinDynLib::vectorIntoBottle(const yarp::sig::Vector v, yarp::os::Bottle &b)
{
for (unsigned int i = 0; i < v.size(); i++)
{
b.addDouble(v[i]);
}
}
void CB::iCubConfigurationReference::setVals(yarp::sig::Vector ref) {
if(ref.size() != values.size()) {
std::cout << "Couldn't set reference values for " << deviceName.c_str() << "!!" << std::endl;
return;
} else {
values = ref;
}
}
void yarp::rtf::jointsPosMotion::setSpeed(yarp::sig::Vector &speedlist)
{
RTF_ASSERT_ERROR_IF_FALSE((speedlist.size() != mPriv->jointsList.size()), "Speed list has a different size of joint list");
mPriv->speed = speedlist;
for (size_t i = 0; i < mPriv->n_joints; i++) {
mPriv->ipos->setRefSpeed((int)mPriv->jointsList[i], mPriv->speed[i]);
}
}
bool isEqual(const yarp::sig::Vector& v1, const yarp::sig::Vector& v2, double precision)
{
if (v1.size() != v2.size())
{
return false;
}
for (size_t i = 0; i < v1.size(); i++)
{
double check = fabs(v1[i] - v2[i]);
if (check > precision)
{
return false;
}
}
return true;
}
void Recognizer::saveFilteredScores(const yarp::sig::Vector &scores, string &path)
{
ofstream outFile(path.c_str(), ios_base::app);
for (unsigned int i=0; i<scores.size(); i++)
{
outFile << scores[i] << " ";
}
outFile << endl;
outFile.close();
}
void yarp2idyntree(const yarp::sig::Vector & yarpVector,
iDynTree::VectorDynSize & idyntreeVector)
{
idyntreeVector.resize(yarpVector.size());
for(size_t row=0; row < idyntreeVector.size(); row++)
{
idyntreeVector(row) = yarpVector(row);
}
return;
}