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