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; }
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 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 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; }
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 toiDynTree(const yarp::sig::Vector& yarpVector, VectorDynSize& iDynTreeVector) { iDynTreeVector.resize(yarpVector.size()); memcpy(iDynTreeVector.data(),yarpVector.data(),yarpVector.size()*sizeof(double)); return true; }
void CB::YARPConfigurationVariables::getJointAccelerationsDirect(yarp::sig::Vector &v) { if(enc==NULL) { cout << "IPositionControl pointer is NULL!" << endl; } else { enc->getEncoderAccelerations(v.data()); } }
bool ImplementVirtualAnalogSensor::updateVirtualAnalogSensorMeasure(yarp::sig::Vector &measure) { yarp::sig::Vector measure_raw; castToMapper(helper)->voltageV2S(measure.data(), measure_raw.data()); bool ret = iVASRaw->updateVirtualAnalogSensorMeasureRaw(measure_raw); return ret; }
void CB::YARPConfigurationVariables::setJointVelocityDirect(yarp::sig::Vector v) { if(vel==NULL) { cout << "IVelocityControl pointer is NULL!" << endl; } else { vel->velocityMove(v.data()); } }
bool Vector::operator==(const yarp::sig::Vector &r) const { //check dimensions first size_t c=size(); if (c!=r.size()) return false; const double *tmp1=data(); const double *tmp2=r.data(); while(c--) { if (*tmp1++!=*tmp2++) return false; } return true; }
bool KDLtoYarp(const KDL::Vector & kdlVector,yarp::sig::Vector & yarpVector) { if( yarpVector.size() != 3 ) { yarpVector.resize(3); } memcpy(yarpVector.data(),kdlVector.data,3*sizeof(double)); return true; }
inline Eigen::Map<Eigen::VectorXd> toEigenVector(yarp::sig::Vector & vec) { return Eigen::Map<Eigen::VectorXd>(vec.data(), vec.size()); }
Eigen::Map<const Eigen::VectorXd> toEigen(const yarp::sig::Vector & vec) { return Eigen::Map<const Eigen::VectorXd>(vec.data(),vec.size()); }
void copyVector(const yarp::sig::Vector & src, double * dest) { memcpy(dest,src.data(),src.size()*sizeof(double)); }