floating base Dof in inverse dynamics with PxArticulationReducedCoordinate

[christoph-jaehne]

Hi everyone, I'm currently working on simulation and control of floating base robots. I'm using PxArticulationReducedCoordinate to represent my robot and would like to use the dynamic model of the robot (more precisely gravity (g), coriolis/centrifugal-forces (c), and the massmatrix (M) ) e.g. for control purposes. For all of these quantities, how and where are the 6 degrees of freedom of the floating base handled?

For example when I do the following to get the gravity terms, as far as I see I only get the terms acting in the joints of the system but nothing acting on the floating base..

    PxArticulationCache* cache = articulation->createCache();
    articulation->copyInternalStateToCache(*cache, PxArticulationCache::eALL);
    articulation->computeGeneralizedGravityForce(*cache);
    // .. and then read cache->jointForce[0 ... articulation->getDofs()-1]

Am I missing something? Are floating bases considered in the dynamics terms M c g?

[kstorey-nvidia]

Hi Christoph

The joint forces are propagated through the articulation so they do influence the floating link base. Please see the code snippet below (taken from our unit tests to verify the inverse dynamics). This test runs the same configuration with/without a fixed base. You can adjust the target joint acceleration for each joint by adjusting jointAccelScale. Setting all to zero holds the joints perfectly rigidly, adjusting to other values sees the respective joints accelerate at that rate. The floating base configuration falls under gravity, but maintains its respective joint target accelerations while doing so:

TEST_F(ArticulationControllerFunctionsTest, MassMatrixTest) { for (PxU32 c = 0; c < 2; ++c) { bool fixBase = (c == 0 ? true : false); PxArticulationReducedCoordinate* articulation = createArticulation(getPhysics(), &getScene(), NULL, fixBase);

    //runSim(1);

    PxArticulationCache* cache0 = articulation->createCache(); //for computeJointForce
    PxArticulationCache* cache1 = articulation->createCache(); //for computeGeneralisedGravityForce
    PxArticulationCache* cache2 = articulation->createCache(); //for computeCoriolisAndCentrifugalForce

    const PxU32 totalDofs = articulation->getDofs();
    const PxU32 nbJoints = NUM_LINKS - 1;
    //joint acceleration in maximum format(rx, ry, rz, tx, ty, tz)
    PxReal jointAcceleration[nbJoints * 6];
    PxReal mf[nbJoints * 6]; //this is memory is bigger than it need to be

    //The target joint accelerations for each respective joint (0th elements is null joint)
    const PxReal jointAccelScale[] = { 0.f, 0.02f, 0.0f };

    for (PxU32 ite = 0; ite < 100; ++ite)
    {

        //need to call this before all the inverse dynamic methods
        articulation->commonInit();

        PxArticulationLink* links[NUM_LINKS];
        articulation->getLinks(links, sizeof(PxArticulationLink) * NUM_LINKS, 0);

        //apply random joint torque.
        for (PxU32 i = 1; i < NUM_LINKS; ++i)
        {
            PxArticulationLink* link = links[i];
            PxArticulationJointReducedCoordinate* joint = static_cast<PxArticulationJointReducedCoordinate*>(link->getInboundJoint());

            PxReal* jA = &jointAcceleration[(i - 1) * 6];

            //Adjust the joint acceleration target.
            for (PxU32 j = 0; j < 6; ++j)
            {
                PxArticulationMotion::Enum motion = joint->getMotion(PxArticulationAxis::Enum(j));
                if (motion != PxArticulationMotion::eLOCKED)
                {
                    jA[j] = ite * jointAccelScale[i];
                }
            }
        }

        articulation->packJointData(jointAcceleration, cache0->jointAcceleration);

        articulation->computeGeneralizedMassMatrix(*cache0);

        //compute joint force based on gravity, joint velocity/joint acceleration are zero
        articulation->computeGeneralizedGravityForce(*cache1);

        //copy joint velocity from internal joint data to the cache
        articulation->copyInternalStateToCache(*cache2, PxArticulationCacheFlags(PxArticulationCache::eVELOCITY));
        //provided joint velocity, compute joint force. joint acceleration, gravity are zero
        articulation->computeCoriolisAndCentrifugalForce(*cache2);

        //cache0 has acceleration so we store the joint force in cache0
        articulation->computeJointForce(*cache0);

        PxReal* M = cache0->massMatrix;
        PxReal* jA = cache0->jointAcceleration;

        PxMemZero(mf, sizeof(PxReal) * nbJoints * 6);

        //compute M(q)*qddot
        for (PxU32 rowInd = 0; rowInd < totalDofs; ++rowInd)
        {
            PxReal* row = &M[rowInd * totalDofs];

            for (PxU32 j = 0; j < totalDofs; ++j)
            {
                mf[rowInd] += row[j] * jA[j];
            }
        }

        //apply M(q)*qddot + C(q,qdot) - g(q). �

        PxReal* jf0 = cache0->jointForce;   //joint force
        PxReal* jf1 = cache1->jointForce;   //gravity
        PxReal* jf2 = cache2->jointForce;   //centrifugal force

        const PxReal eps = 1e-3f;
        //store the combined force in cache1
        for (PxU32 i = 0; i < totalDofs; ++i)
        {
            const PxReal dif = mf[i] - jf0[i];
            EXPECT_TRUE(PxAbs(dif) < eps);
            jf1[i] = mf[i] + jf1[i] + jf2[i];
        }

        articulation->applyCache(*cache1, PxArticulationCacheFlags(PxArticulationCache::eFORCE));

        runSim(1);

        articulation->copyInternalStateToCache(*cache1, PxArticulationCacheFlags(PxArticulationCache::eACCELERATION | PxArticulationCache::eVELOCITY));

        PxReal* jA0 = cache0->jointAcceleration;
        PxReal* jA1 = cache1->jointAcceleration;
        PxReal* jV1 = cache1->jointVelocity;
        PxReal* jV2 = cache2->jointVelocity; //cache2 has previous frame's joint velocity

        const PxReal eps2 = 1e-4f;
        for (PxU32 i = 0; i < totalDofs; ++i)
        {
            const PxReal difA = jA0[i] - jA1[i];
            const PxReal jA2 = (jV1[i] - jV2[i]) / mTimeStepSize;
            const PxReal dif = jA0[i] - jA2;
            EXPECT_TRUE(PxAbs(difA) < eps2);
            EXPECT_TRUE(PxAbs(dif) < eps2);

        }

        //zero cache, otherwise, the joint force is accumulated
        articulation->zeroCache(*cache0);
        articulation->zeroCache(*cache1);
        articulation->zeroCache(*cache2);

        //run another simulation to change configuration
        //runSim(1);
    }

    articulation->releaseCache(*cache0);
    articulation->releaseCache(*cache1);
    articulation->releaseCache(*cache2);

    articulation->release();
}

} `

[christoph-jaehne]

Thanks for the example. I'm still missing the parts of M, c, g that address the floating base, e.g. the coupling in the mass matrix that tell me how accelerations of the floating base affect joint accelerations and vice versa. To be very clear, I wrote down in formulas the equations of motion of a floating base system with n joints: I argue that with what you show in your example code only the part indicated in red is provided to the user right now. The test you showed might still pass in case there are no or low accelerations of the floating base in the scenario you simulate. Ultimately I'm asking whether there is way for the user to get M_f ; M_f,q ; c_f and g_f or?

[michellelu-nvidia]

Hi Christoph

At the moment, the inverse dynamics functionality has been driven by the needs by our internal teams, who have not yet required anything besides joint torque control for a system acting under gravity, with contacts. While I haven't confirmed that this can't be worked around with the current code, I do think that you may be right that this is a missing feature.

I'd be happy to chat further with you and take a full set of requirements from you for your project so that we can schedule to implement the missing features, if you are happy to do so.

If you are happy to do this, perhaps we could conduct the discussion over direct communications.

Many thanks

Michelle

[christoph-jaehne]

Hi Michelle,

I would be happy to do so. A use case for having the mentioned missing terms could be a robot that needs balancing control, e.g. a humanoid that uses upper body or arm motion to balance to not fall over. It would be valuable in this cases as for any other underactuated robot to know how joint motions in actuated DOF affect the non-actuated dof (her those of the floating base).

How and where do you propose to have this discussion?

Best Christoph