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