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; }