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 SensorsMeasurements::toVector(VectorDynSize & measurementVector) const
{
unsigned int itr;
LinAcceleration thisLinAcc;
AngVelocity thisAngVel;
Wrench thisWrench;
unsigned int numFT = this->pimpl->SixAxisFTSensorsMeasurements.size();
unsigned int numAcc = this->pimpl->AccelerometerMeasurements.size();
unsigned int numGyro = this->pimpl->GyroscopeMeasurements.size();
bool ok = true;
measurementVector.resize(6*numFT + 3*numAcc + 3*numGyro);
for(itr = 0; itr<numFT; itr++)
{
thisWrench = this->pimpl->SixAxisFTSensorsMeasurements.at(itr);
ok && measurementVector.setVal(6*itr, thisWrench.getVal(0));
ok && measurementVector.setVal(6*itr+1,thisWrench.getVal(1));
ok && measurementVector.setVal(6*itr+2,thisWrench.getVal(2));
ok && measurementVector.setVal(6*itr+3,thisWrench.getVal(3));
ok && measurementVector.setVal(6*itr+4,thisWrench.getVal(4));
ok && measurementVector.setVal(6*itr+5,thisWrench.getVal(5));
}
for(itr = 0; itr<numAcc; itr++)
{
thisLinAcc = this->pimpl->AccelerometerMeasurements.at(itr);
ok && measurementVector.setVal(6*numFT + 3*itr,thisLinAcc.getVal(0));
ok && measurementVector.setVal(6*numFT + 3*itr+1,thisLinAcc.getVal(1));
ok && measurementVector.setVal(6*numFT + 3*itr+2,thisLinAcc.getVal(2));
}
for(itr = 0; itr<numGyro; itr++)
{
thisAngVel = this->pimpl->GyroscopeMeasurements.at(itr);
ok && measurementVector.setVal(6*numFT + 3*numAcc + 3*itr,thisAngVel.getVal(0));
ok && measurementVector.setVal(6*numFT + 3*numAcc + 3*itr+1,thisAngVel.getVal(1));
ok && measurementVector.setVal(6*numFT + 3*numAcc + 3*itr+2,thisAngVel.getVal(2));
}
return ok;
}
