bool SensorsMeasurements::setNrOfSensors(const SensorType& sensor_type, unsigned int nrOfSensors)
{
Wrench zeroWrench;
LinAcceleration zeroLinAcc;
AngVelocity zeroAngVel;
unsigned int returnVal = 0;
switch (sensor_type)
{
case SIX_AXIS_FORCE_TORQUE :
zeroWrench.zero();
this->pimpl->SixAxisFTSensorsMeasurements.resize(nrOfSensors,zeroWrench);
break;
case ACCELEROMETER :
zeroLinAcc.zero();
this->pimpl->AccelerometerMeasurements.resize(nrOfSensors,zeroLinAcc);
break;
case GYROSCOPE :
zeroAngVel.zero();
this->pimpl->GyroscopeMeasurements.resize(nrOfSensors,zeroAngVel);
default :
returnVal = 0;
}
return(returnVal);
return true;
}
void checkWrenchTransformation(const Transform & trans, const Wrench & w)
{
Wrench wTransformed = trans*w;
Matrix6x6 adjWrench = trans.asAdjointTransformWrench();
Vector6 wTranslatedCheck;
Vector6 wp = w.asVector();
toEigen(wTranslatedCheck) = toEigen(adjWrench)*toEigen(wp);
ASSERT_EQUAL_VECTOR(wTranslatedCheck,wTransformed.asVector());
}
bool SensorsMeasurements::resize(const SensorsList &sensorsList)
{
Wrench zeroWrench;zeroWrench.zero();
LinAcceleration zeroLinAcc;zeroLinAcc.zero();
AngVelocity zeroAngVel;zeroAngVel.zero();
this->pimpl->SixAxisFTSensorsMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE),zeroWrench);
this->pimpl->AccelerometerMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::ACCELEROMETER),zeroLinAcc);
this->pimpl->GyroscopeMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::GYROSCOPE),zeroAngVel);
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;
}
void checkInvariance(const Transform & trans, const ArticulatedBodyInertia & inertia, const SpatialAcc & twist)
{
Transform invTrans = trans.inverse();
Wrench momentumTranslated = trans*(inertia*twist);
Wrench momentumTranslatedCheck = (trans*inertia)*(trans*twist);
SpatialAcc twistTranslated = trans*twist;
ArticulatedBodyInertia inertiaTranslated = trans*inertia;
Vector6 momentumTranslatedCheck2;
Vector6 momentumTranslatedCheck3;
Vector6 twistTranslatedCheck;
Matrix6x6 transAdjWrench = trans.asAdjointTransformWrench();
Matrix6x6 inertiaRaw = inertia.asMatrix();
Matrix6x6 transInvAdj = trans.inverse().asAdjointTransform();
Matrix6x6 transAdj = trans.asAdjointTransform();
Matrix6x6 inertiaTranslatedCheck;
Vector6 twistPlain = twist.asVector();
toEigen(momentumTranslatedCheck2) = toEigen(transAdjWrench)*
toEigen(inertiaRaw)*
toEigen(twistPlain);
toEigen(momentumTranslatedCheck3) = toEigen(transAdjWrench)*
toEigen(inertiaRaw)*
toEigen(transInvAdj)*
toEigen(transAdj)*
toEigen(twistPlain);
toEigen(twistTranslatedCheck) = toEigen(transAdj)*
toEigen(twistPlain);
toEigen(inertiaTranslatedCheck) = toEigen(transAdjWrench)*
toEigen(inertiaRaw)*
toEigen(transInvAdj);
ASSERT_EQUAL_MATRIX(inertiaTranslatedCheck,inertiaTranslated.asMatrix());
ASSERT_EQUAL_VECTOR(twistTranslated.asVector(),twistTranslatedCheck);
ASSERT_EQUAL_VECTOR(momentumTranslated.asVector(),momentumTranslatedCheck3);
ASSERT_EQUAL_VECTOR(momentumTranslated.asVector(),momentumTranslatedCheck2);
ASSERT_EQUAL_VECTOR(momentumTranslated.asVector(),momentumTranslatedCheck.asVector());
Wrench momentum = invTrans*momentumTranslated;
Wrench momentumCheck = (invTrans*(trans*inertia))*(invTrans*(trans*twist));
ASSERT_EQUAL_VECTOR(momentum.asVector(),momentumCheck.asVector());
}
void checkDotProductInvariance(const Transform & trans, const Wrench & w, const Twist & twist)
{
double power = w.dot(twist);
double powerCheck = twist.dot(w);
double powerCheck2 = (trans*w).dot(trans*twist);
double powerCheck3 = (trans*twist).dot(trans*w);
ASSERT_EQUAL_DOUBLE(power,powerCheck);
ASSERT_EQUAL_DOUBLE(power,powerCheck2);
ASSERT_EQUAL_DOUBLE(power,powerCheck3);
}
void FramesTest::TestWrench2(Wrench& w) {
// Wrench
Wrench w2;
CPPUNIT_ASSERT_EQUAL(2*w-w,w);
CPPUNIT_ASSERT_EQUAL(w*2-w,w);
CPPUNIT_ASSERT_EQUAL(w+w+w-2*w,w);
w2=w;
CPPUNIT_ASSERT_EQUAL(w,w2);
w2+=w;
CPPUNIT_ASSERT_EQUAL(2*w,w2);
w2-=w;
CPPUNIT_ASSERT_EQUAL(w,w2);
w.ReverseSign();
CPPUNIT_ASSERT_EQUAL(w,-w2);
}
Wrench::Wrench(const Wrench& other):
SpatialForceVectorRaw(other.data(),6)
{
}
