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;
}
}
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;
}
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 YarpJointDev::getDOF ( yarp::sig::Vector& curDof ) {
curDof.clear();
for ( unsigned i = 0; i < _chain->GetNumberOfJoints(); ++i )
curDof.push_back ( 1 );
return true;
}
void iCubShoulderConstr::appendMatrixRow(yarp::sig::Matrix &dest,
const yarp::sig::Vector &row)
{
yarp::sig::Matrix tmp;
// if dest is already filled with something
if (dest.rows())
{
// exit if lengths do not match
if (row.length()!=dest.cols())
return;
tmp.resize(dest.rows()+1,dest.cols());
// copy the content of dest in temp
for (int i=0; i<dest.rows(); i++)
for (int j=0; j<dest.cols(); j++)
tmp(i,j)=dest(i,j);
// reassign dest
dest=tmp;
}
else
dest.resize(1,row.length());
// append the last row
for (int i=0; i<dest.cols(); i++)
dest(dest.rows()-1,i)=row[i];
}
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)));
}
}
}
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 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 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;
}
bool reactCtrlThread::controlArm(const yarp::sig::Vector &_vels)
{
VectorOf<int> jointsToSetA;
VectorOf<int> jointsToSetT;
if (!areJointsHealthyAndSet(jointsToSetA,"arm","velocity"))
{
yWarning("[reactCtrlThread]Stopping control because arm joints are not healthy!");
stopControlHelper();
return false;
}
if (useTorso)
{
if (!areJointsHealthyAndSet(jointsToSetT,"torso","velocity"))
{
yWarning("[reactCtrlThread]Stopping control because torso joints are not healthy!");
stopControlHelper();
return false;
}
}
if (!setCtrlModes(jointsToSetA,"arm","velocity"))
{
yError("[reactCtrlThread]I am not able to set the arm joints to velocity mode!");
return false;
}
if (useTorso)
{
if (!setCtrlModes(jointsToSetT,"torso","velocity"))
{
yError("[reactCtrlThread]I am not able to set the torso joints to velocity mode!");
return false;
}
}
printMessage(1,"Moving the robot with velocities: %s\n",_vels.toString(3,3).c_str());
if (useTorso)
{
Vector velsT(3,0.0);
velsT[0] = _vels[2]; //swapping pitch and yaw as per iKin vs. motor interface convention
velsT[1] = _vels[1];
velsT[2] = _vels[0]; //swapping pitch and yaw as per iKin vs. motor interface convention
ivelT->velocityMove(velsT.data());
ivelA->velocityMove(_vels.subVector(3,9).data()); //indexes 3 to 9 are the arm joints velocities
}
else
{
ivelA->velocityMove(_vels.data()); //if there is not torso, _vels has only the 7 arm joints
}
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;
}
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);
}
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;
}
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 toiDynTree(const yarp::sig::Vector& yarpVector, Direction& direction)
{
if( yarpVector.size() != 3 )
{
return false;
}
memcpy(direction.data(),yarpVector.data(),3*sizeof(double));
// normalize
direction.Normalize();
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();
}
}
}
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 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;
};
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 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));
}
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 toiDynTree(const yarp::sig::Vector& yarpVector, VectorDynSize& iDynTreeVector)
{
iDynTreeVector.resize(yarpVector.size());
memcpy(iDynTreeVector.data(),yarpVector.data(),yarpVector.size()*sizeof(double));
return true;
}
/* 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;
}
void iCubShoulderConstr::appendVectorValue(yarp::sig::Vector &dest, double val)
{
yarp::sig::Vector tmp(dest.length()+1);
for (size_t i=0; i<dest.length(); i++)
tmp[i]=dest[i];
dest=tmp;
dest[dest.length()-1]=val;
}