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