The conservation of momentum is broken in btMultibody

Hi all,

I am using bullet as a physics enignee in my application.But when I try to verify the conservation law of (linear) momentum for btMultibody structure, the result is not in my expection.

Question Description

Briefly, I create a btMultibody with 3 links, disable all friction coeff, simulating it along with adding joint torque on the first spherical joint, then compare the linear momentum changes with external gravity impulse.In my opinion, The linear momentum of this btMultibody should reply on nothing except the gravity, isn't it?(If I am wrong please point it out).

I use The examples/MultiBody/MultiDofDemo.cpp for making a minimum reproduce code. If you guys paste all the following code stuffs in MultiDofDemo.cpp then run it, maybe you can get the same output as me...I dont know..

P.S. This project is based on macOS Mojave.

ANY suggestion is welcome!

The experiment result

b3Printf: old momentum -17.42704, -0.55479, 6.60006
b3Printf: new momentum -20.20674, -0.38032, 7.95865
b3Printf: gravity impulse 0.00000, -0.03000, 0.00000
b3Printf: momentum changes -2.77970, 0.17447, 1.35859
b3Printf: relative error = 10335.655%

We can see that the momentum changes is significantly different from the impulse.

minium reproduce code

#include "MultiDofDemo.h"
#include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "btBulletDynamicsCommon.h"

#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"

#include "BulletDynamics/Featherstone/btMultiBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"

#include "../OpenGLWindow/GLInstancingRenderer.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"

#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include <iostream>

btVector3 gGravity = btVector3(0, -10, 0);
class MultiDofDemo : public CommonMultiBodyBase
{
public:
    MultiDofDemo(GUIHelperInterface* helper);
    virtual ~MultiDofDemo();

    virtual void initPhysics();

    virtual void stepSimulation(float deltaTime);

    virtual void resetCamera()
    {
        float dist = 1;
        float pitch = -35;
        float yaw = 50;
        float targetPos[3] = {-3, 2.8, -2.5};
        m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
    }

    btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false);
    void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
    void addBoxes_testMultiDof();
    btVector3 calcLinearMomentum();
    void addJointTorque();

    btVector3 vel_buffer[20], omega_buffer[20];
};

static bool g_floatingBase = true;
static bool g_firstInit = true;
static float scaling = 0.4f;
static float friction = 1.;
static int g_constraintSolverType = 0;
#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Y 5
#define ARRAY_SIZE_Z 5

//maximum number of objects (and allow user to shoot additional boxes)
#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)

#define START_POS_X -5
//#define START_POS_Y 12
#define START_POS_Y 2
#define START_POS_Z -3

MultiDofDemo::MultiDofDemo(GUIHelperInterface* helper)
    : CommonMultiBodyBase(helper)
{
    m_guiHelper->setUpAxis(1);
}
MultiDofDemo::~MultiDofDemo()
{
}

void MultiDofDemo::stepSimulation(float deltaTime)
{
    //use a smaller internal timestep, there are stability issues
    // float internalTimeStep = 1. / 240.f;
    deltaTime = 1e-3;
    btVector3 old_momentum = calcLinearMomentum();

    addJointTorque();
    m_dynamicsWorld->stepSimulation(deltaTime, 1, deltaTime);
    btVector3 new_momentum = calcLinearMomentum();

    // verify the conservation of momentum
    btVector3 momenum_changes = new_momentum - old_momentum;
    btVector3 gravity_impulse = btVector3(0,0,0);
    {
        auto multibody = m_dynamicsWorld->getMultiBody(0);
        double total_mass = 0.0;

        total_mass+= multibody->getBaseMass();
        for(int i=0; i<multibody->getNumLinks(); i++) total_mass += multibody->getLinkMass(i);
        gravity_impulse = total_mass * gGravity * deltaTime;
    }
    double relative_error = (gravity_impulse - momenum_changes).norm() / gravity_impulse.norm();

    b3Printf("-------------new frame -------------");
    b3Printf("old momentum %.5f, %.5f, %.5f", old_momentum[0], old_momentum[1], old_momentum[2]);
    b3Printf("new momentum %.5f, %.5f, %.5f", new_momentum[0], new_momentum[1], new_momentum[2]);
    b3Printf("gravity impulse %.5f, %.5f, %.5f", gravity_impulse[0], gravity_impulse[1], gravity_impulse[2]);
    b3Printf("momentum changes %.5f, %.5f, %.5f", momenum_changes[0], momenum_changes[1], momenum_changes[2]);
    b3Printf("relative error = %.3f%%", relative_error * 100);

}

void MultiDofDemo::initPhysics()
{
    m_guiHelper->setUpAxis(1);

    if (g_firstInit)
    {
        m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10. * scaling));
        m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50);
        g_firstInit = false;
    }
    ///collision configuration contains default setup for memory, collision setup
    m_collisionConfiguration = new btDefaultCollisionConfiguration();

    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
    m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);

    m_broadphase = new btDbvtBroadphase();

    if (g_constraintSolverType == 4)
    {
        g_constraintSolverType = 0;
        g_floatingBase = !g_floatingBase;
    }

    btMultiBodyConstraintSolver* sol;
    btMLCPSolverInterface* mlcp;
    switch (g_constraintSolverType++)
    {
        case 0:
            sol = new btMultiBodyConstraintSolver;
            b3Printf("Constraint Solver: Sequential Impulse");
            break;
        case 1:
            mlcp = new btSolveProjectedGaussSeidel();
            sol = new btMultiBodyMLCPConstraintSolver(mlcp);
            b3Printf("Constraint Solver: MLCP + PGS");
            break;
        case 2:
            mlcp = new btDantzigSolver();
            sol = new btMultiBodyMLCPConstraintSolver(mlcp);
            b3Printf("Constraint Solver: MLCP + Dantzig");
            break;
        default:
            mlcp = new btLemkeSolver();
            sol = new btMultiBodyMLCPConstraintSolver(mlcp);
            b3Printf("Constraint Solver: MLCP + Lemke");
            break;
    }

    m_solver = sol;

    //use btMultiBodyDynamicsWorld for Featherstone btMultiBody support
    btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration);
    m_dynamicsWorld = world;
    //  m_dynamicsWorld->setDebugDrawer(&gDebugDraw);
    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
    m_dynamicsWorld->setGravity(gGravity);
    m_dynamicsWorld->getSolverInfo().m_globalCfm = 1e-3;

    ///create a few basic rigid bodies
    btVector3 groundHalfExtents(50, 50, 50);
    btCollisionShape* groundShape = new btBoxShape(groundHalfExtents);
    //groundShape->initializePolyhedralFeatures();
    //  btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);

    m_collisionShapes.push_back(groundShape);

    btTransform groundTransform;
    groundTransform.setIdentity();
    groundTransform.setOrigin(btVector3(0, -50, 00));

    /////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////

    bool damping = false;
    bool gyro = true;
    int numLinks = 2;
    bool spherical = true;  //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
    bool multibodyOnly = true;
    bool canSleep = false;
    bool selfCollide = true;
    bool multibodyConstraint = false;
    btVector3 linkHalfExtents(0.05, 0.37, 0.1);
    btVector3 baseHalfExtents(0.05, 0.37, 0.1);

    btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
    //mbC->forceMultiDof();                         //if !spherical, you can comment this line to check the 1DoF algorithm

    mbC->setCanSleep(canSleep);
    mbC->setHasSelfCollision(selfCollide);
    mbC->setUseGyroTerm(gyro);
    //
    if (!damping)
    {
        mbC->setLinearDamping(0.f);
        mbC->setAngularDamping(0.f);
    }
    else
    {
        mbC->setLinearDamping(0.1f);
        mbC->setAngularDamping(0.9f);
    }
    //
    m_dynamicsWorld->setGravity(btVector3(0, -9.81, 0));
    //m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
    //////////////////////////////////////////////
    if (numLinks > 0)
    {
        btScalar q0 = 45.f * SIMD_PI / 180.f;
        if (!spherical)
        {
            mbC->setJointPosMultiDof(0, &q0);
        }
        else
        {
            btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
            quat0.normalize();
            mbC->setJointPosMultiDof(0, quat0);
        }
    }
    ///
    addColliders_testMultiDof(mbC, world, baseHalfExtents, linkHalfExtents);

    /////////////////////////////////////////////////////////////////
    btScalar groundHeight = -51.55;
    if (!multibodyOnly)
    {
        btScalar mass(0.);

        //rigidbody is dynamic if and only if mass is non zero, otherwise static
        bool isDynamic = (mass != 0.f);

        btVector3 localInertia(0, 0, 0);
        if (isDynamic)
            groundShape->calculateLocalInertia(mass, localInertia);

        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
        groundTransform.setIdentity();
        groundTransform.setOrigin(btVector3(0, groundHeight, 0));
        btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
        btRigidBody* body = new btRigidBody(rbInfo);

        //add the body to the dynamics world
        m_dynamicsWorld->addRigidBody(body, 1, 1 + 2);  //,1,1+2);
    }
    /////////////////////////////////////////////////////////////////
    if (!multibodyOnly)
    {
        btVector3 halfExtents(.5, .5, .5);
        btBoxShape* colShape = new btBoxShape(halfExtents);
        //btCollisionShape* colShape = new btSphereShape(btScalar(1.));
        m_collisionShapes.push_back(colShape);

        /// Create Dynamic Objects
        btTransform startTransform;
        startTransform.setIdentity();

        btScalar mass(1.f);

        //rigidbody is dynamic if and only if mass is non zero, otherwise static
        bool isDynamic = (mass != 0.f);

        btVector3 localInertia(0, 0, 0);
        if (isDynamic)
            colShape->calculateLocalInertia(mass, localInertia);

        startTransform.setOrigin(btVector3(
            btScalar(0.0),
            0.0,
            btScalar(0.0)));

        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
        btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, colShape, localInertia);
        btRigidBody* body = new btRigidBody(rbInfo);

        m_dynamicsWorld->addRigidBody(body);  //,1,1+2);

        if (multibodyConstraint)
        {
            btVector3 pointInA = -linkHalfExtents;
            //      btVector3 pointInB = halfExtents;
            btMatrix3x3 frameInA;
            btMatrix3x3 frameInB;
            frameInA.setIdentity();
            frameInB.setIdentity();
            btVector3 jointAxis(1.0, 0.0, 0.0);
            //btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis);
            btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC, numLinks - 1, mbC, numLinks - 4, pointInA, pointInA, frameInA, frameInB);
            p2p->setMaxAppliedImpulse(2.0);
            m_dynamicsWorld->addMultiBodyConstraint(p2p);
        }
    }

    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);

    /////////////////////////////////////////////////////////////////
}

btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating)
{
    //init the base
    btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
    float baseMass = 1.f;

    if (baseMass)
    {
        btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
        pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
        delete pTempBox;
    }

    bool canSleep = false;

    btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);

    btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
    pMultiBody->setBasePos(basePosition);
    pMultiBody->setWorldToBaseRot(baseOriQuat);
    btVector3 vel(0, 0, 0);
    //  pMultiBody->setBaseVel(vel);

    //init the links
    btVector3 hingeJointAxis(1, 0, 0);
    float linkMass = 1.f;
    btVector3 linkInertiaDiag(0.f, 0.f, 0.f);

    btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
    pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
    delete pTempBox;

    //y-axis assumed up
    btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0);                      //par body's COM to cur body's COM offset
    btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0);                         //cur body's COM to cur body's PIV offset
    btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;  //par body's COM to cur body's PIV offset

    //////
    btScalar q0 = 0.f * SIMD_PI / 180.f;
    btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
    quat0.normalize();
    /////

    for (int i = 0; i < numLinks; ++i)
    {
        if (!spherical)
            pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
        else
            //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
            pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
    }

    pMultiBody->finalizeMultiDof();

    ///
    pWorld->addMultiBody(pMultiBody);
    ///
    return pMultiBody;
}

void MultiDofDemo::addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
{
    btAlignedObjectArray<btQuaternion> world_to_local;
    world_to_local.resize(pMultiBody->getNumLinks() + 1);

    btAlignedObjectArray<btVector3> local_origin;
    local_origin.resize(pMultiBody->getNumLinks() + 1);
    world_to_local[0] = pMultiBody->getWorldToBaseRot();
    local_origin[0] = pMultiBody->getBasePos();

    {
        //  float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
        btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};

        if (1)
        {
            btCollisionShape* box = new btBoxShape(baseHalfExtents);
            btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
            col->setCollisionShape(box);

            btTransform tr;
            tr.setIdentity();
            tr.setOrigin(local_origin[0]);
            tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
            col->setWorldTransform(tr);

            pWorld->addCollisionObject(col, 2, 1 + 2);

            col->setFriction(friction);
            pMultiBody->setBaseCollider(col);
        }
    }

    for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
    {
        const int parent = pMultiBody->getParent(i);
        world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
        local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
    }

    for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
    {
        btVector3 posr = local_origin[i + 1];
        //  float pos[4]={posr.x(),posr.y(),posr.z(),1};

        btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};

        btCollisionShape* box = new btBoxShape(linkHalfExtents);
        btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);

        col->setCollisionShape(box);
        btTransform tr;
        tr.setIdentity();
        tr.setOrigin(posr);
        tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
        col->setWorldTransform(tr);
        col->setFriction(friction);
        pWorld->addCollisionObject(col, 2, 1 + 2);

        pMultiBody->getLink(i).m_collider = col;
    }
}

void MultiDofDemo::addBoxes_testMultiDof()
{
    //create a few dynamic rigidbodies
    // Re-using the same collision is better for memory usage and performance

    btBoxShape* colShape = new btBoxShape(btVector3(1, 1, 1));
    //btCollisionShape* colShape = new btSphereShape(btScalar(1.));
    m_collisionShapes.push_back(colShape);

    /// Create Dynamic Objects
    btTransform startTransform;
    startTransform.setIdentity();

    btScalar mass(1.f);

    //rigidbody is dynamic if and only if mass is non zero, otherwise static
    bool isDynamic = (mass != 0.f);

    btVector3 localInertia(0, 0, 0);
    if (isDynamic)
        colShape->calculateLocalInertia(mass, localInertia);

    float start_x = START_POS_X - ARRAY_SIZE_X / 2;
    float start_y = START_POS_Y;
    float start_z = START_POS_Z - ARRAY_SIZE_Z / 2;

    for (int k = 0; k < ARRAY_SIZE_Y; k++)
    {
        for (int i = 0; i < ARRAY_SIZE_X; i++)
        {
            for (int j = 0; j < ARRAY_SIZE_Z; j++)
            {
                startTransform.setOrigin(btVector3(
                    btScalar(3.0 * i + start_x),
                    btScalar(3.0 * k + start_y),
                    btScalar(3.0 * j + start_z)));

                //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
                btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
                btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, colShape, localInertia);
                btRigidBody* body = new btRigidBody(rbInfo);

                m_dynamicsWorld->addRigidBody(body);  //,1,1+2);
            }
        }
    }
}

btVector3 MultiDofDemo::calcLinearMomentum()
{
    btVector3 linear_momentum = btVector3(0, 0, 0);
    auto multibody = m_dynamicsWorld->getMultiBody(0);
    assert(multibody != nullptr);

    // 1. calculate velocity of each link COM in their own local frame
    multibody->compTreeLinkVelocities(omega_buffer, vel_buffer);

    // 2. sum all vel*mass, get the final result
    std::vector<btVector3> link_vel_world(multibody->getNumLinks() + 1);
    for (int full_link_id = 0; full_link_id < multibody->getNumLinks() + 1; full_link_id++)
    {
        if (0 == full_link_id)
        {
            link_vel_world[0] = multibody->getBaseVel();
            linear_momentum += link_vel_world[0] * multibody->getBaseMass();
        }
        else
        {
            int multibody_link_id = full_link_id - 1;
            auto & cur_trans = multibody->getLinkCollider(multibody_link_id)->getWorldTransform();
            link_vel_world[full_link_id] = quatRotate(cur_trans.getRotation(), vel_buffer[full_link_id]);
            linear_momentum += link_vel_world[full_link_id] * multibody->getLinkMass(multibody_link_id);
        }
    }

    // 3. return
    return linear_momentum;
}

void MultiDofDemo::addJointTorque()
{
    btVector3 linear_momentum = btVector3(0, 0, 0);
    auto multibody = m_dynamicsWorld->getMultiBody(0);
    btScalar torque[3] = {10, 10, 10};
    multibody->addJointTorqueMultiDof(0, torque);
}

class CommonExampleInterface* MultiDofCreateFunc(struct CommonExampleOptions& options)
{
    return new MultiDofDemo(options.m_guiHelper);
}

bulletphysics / bullet3