Exemplos de SpatialForceVector::asVector em C++ (Cpp)

Exemplo n.º 1

Arquivo: ArticulatedBodyInertiaUnitTest.cpp Projeto: PerryZh/idyntree

void checkInertiaAccProduct(const ArticulatedBodyInertia & inertia, const SpatialMotionVector & twist)
{
    SpatialForceVector momentum = inertia*twist;
    Vector6         momentumCheck;

    Matrix6x6 I = inertia.asMatrix();

    Vector6 twistPlain = twist.asVector();
    toEigen(momentumCheck) = toEigen(I)*toEigen(twistPlain);

    ASSERT_EQUAL_VECTOR(momentum.asVector(),momentumCheck);
}