multibody m_appliedImpulse zero

Post Reply
cloud9
Posts: 5
Joined: Sat Nov 19, 2016 9:47 am

multibody m_appliedImpulse zero

Post by cloud9 »

my setup code:

Code: Select all

        
        dispatcher = new btCollisionDispatcher(&config);
        broadphase = new btDbvtBroadphase();
        
        solver = new btMultiBodyConstraintSolver;
        world = new btMultiBodyDynamicsWorld(dispatcher, broadphase, solver, &config);

        m_world->dynamicWorldRef()->setInternalTickCallback(OgreIScene::onPreTickCallback, this, true);
its in the onPreTickCallback that I want to handle the contacts points impulse value, but find its always zero on the first hit.

I found this link

https://github.com/bulletphysics/bullet3/issues/335

and when I looked noticed btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish had a comment
//do a callback here?
which is ... interesting.

my code for the pre-tick callback might not be relevant, might be very wrong idk, but here it is

Code: Select all

    void GroundReaction::addContactForce(btDispatcher* dispatcher,
                                         const Eigen::Vector3d& com,
                                         const std::vector<int>& linkToBodyIndex,
                                         const rbd::MultiBody& mb,
                                         rbd::MultiBodyConfig& mbc,
                                         const btMultiBody* btmb,
                                         btScalar timeStep)
    {
        auto log_size = []( const std::string& name, const Eigen::MatrixXd& m ) { std::cout << name << " (" << m.rows() << ", " << m.cols() << ")" << std::endl; };
        
        auto to_Vector3d = []( const btVector3& v ) -> Eigen::Vector3d { return Eigen::Vector3d( v.x(), v.y(), v.z() ); };
        
        auto skew_matrix = [](const Eigen::Vector3d &v) -> Eigen::Matrix3d
        {
            Eigen::Matrix3d m;
            m << 0., -v[2], v[1],
            v[2], 0., -v[0],
            -v[1], v[0], 0.;
            return m;
        };
        
        int nrDof = mb.nrDof();
        
        Eigen::VectorXd wrench(6);
        wrench.setZero();
        
        Eigen::MatrixXd jacLambda(1, nrDof);
        
        Eigen::MatrixXd fullJacLambda(1, nrDof);
        fullJacLambda.setZero();
        
        int numManifolds = dispatcher->getNumManifolds();
        for (int i = 0; i < numManifolds; i++)
        {
            btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
            int numContacts = manifold->getNumContacts();
            if(numContacts > 0)
            {
                const btMultiBodyLinkCollider* obA = btMultiBodyLinkCollider::upcast( manifold->getBody0() );
                
                if( obA && obA->m_multiBody == btmb )
                {
                    int body = linkToBodyIndex[ obA->m_link + 1 ];
                    const Eigen::MatrixXd& jac = jacVec_[body].bodyJacobian(mb, mbc);
                    
                    for (int j = 0; j < numContacts; j++)
                    {
                        btManifoldPoint& cp = manifold->getContactPoint(j);
                        
                        Eigen::Vector3d pos = to_Vector3d( cp.m_positionWorldOnB );
                        
                        Eigen::Vector3d force = to_Vector3d( cp.m_normalWorldOnB * cp.m_appliedImpulse/timeStep );
                        
                        std::cout << "impulse " << cp.m_appliedImpulse << std::endl;
                        
                        Eigen::Vector3d pt = (sva::PTransformd(pos) * mbc.bodyPosW[body].inv()).translation();
                        jacVec_[body].translateBodyJacobian(jac, mbc, pt, jacTrans_);

                        Eigen::Vector3d torque = skew_matrix(pos - com) * force;
                        wrench.segment(0, 3) = torque;
                        wrench.segment(3, 3) = force;
                        
                        //std::cout << "impulse " << cp.m_appliedImpulse << std::endl << force.transpose() << std::endl << torque.transpose() << std::endl << std::endl;

                        jacLambda = wrench.transpose() * jacTrans_.block(0, 0, 6, jacVec_[body].dof());
                        
                        
                        jacVec_[body].addFullJacobian(mb,
                                                      jacLambda.block(0, 0, 1, jacVec_[body].dof()),
                                                      fullJacLambda);
                    }
                }

            }
        }
   
        std::cout << "fullJacLambda " << fullJacLambda << std::endl << std::endl;
        
        int pos = mb.joint(0).dof();
        for(std::size_t i = 1; i < mbc.jointTorque.size(); ++i)
        {
            for(double& d : mbc.jointTorque[i])
            {
                d -= fullJacLambda(pos);
                ++pos;
            }
        }
    }
Any help would be much appreciated
User avatar
Erwin Coumans
Site Admin
Posts: 4221
Joined: Sun Jun 26, 2005 6:43 pm
Location: California, USA
Contact:

Re: multibody m_appliedImpulse zero

Post by Erwin Coumans »

The applied impulse should be available in the post-tick callback, if you enable if SOLVER_USE_WARMSTARTING in the solver info.

In PyBullet that is how we provide the contact normal and lateral friction forces and they are non-zero.
cloud9
Posts: 5
Joined: Sat Nov 19, 2016 9:47 am

Re: multibody m_appliedImpulse zero

Post by cloud9 »

Thanks for the reply Erwin,

I found using onPostTick certainly does fixes the zero impulse first hit problem. woot

for world->getSolverInfo().m_solverMode it seems SOLVER_USE_WARMSTARTING was already set, happens by default in btContactSolverInfo c'tor.

I still get some contact points that have a zero impulse at later stages, idk what these are actually doing, but I will skip over them.

I guess can't apply a torque that takes into account ground reaction force without staggering pre and post updates.

might need a bit of a re-think.
Post Reply