void checkInvariance(const Transform & trans, const SpatialAcc & acc1, const SpatialAcc & acc2)
{
SpatialAcc sumAccTrans = trans*(acc1+acc2);
SpatialAcc sumAccTransCheck = trans*acc1 + trans*acc2;
ASSERT_EQUAL_VECTOR(sumAccTrans.asVector(),sumAccTransCheck.asVector());
}
void checkInversion()
{
SpatialInertia I = getRandomInertia();
Transform trans = getRandomTransform();
ArticulatedBodyInertia Ia = I;
Wrench f(LinearForceVector3(1.0,5.0,6.0), AngularForceVector3(3.0,6.0,-4.0));
SpatialAcc a = Ia.applyInverse(f);
SpatialAcc aTrans = trans*a;
SpatialAcc aTransCheck = (trans*Ia).applyInverse(trans*f);
ASSERT_EQUAL_VECTOR(aTrans.asVector(),aTransCheck.asVector());
}
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());
}
