Unstable Behavior with TGS Solver and Non-Identity ChildFrame in Articulations with Limited Spherical Joints

[dtotsila]

Hello!

I am encountering unstable behaviors when utilizing articulations and spherical PxArticulationJoints . Specifically, the issue arises when the childFrame is set to a non-Identity value, and the joint is limited. Everything functions correctly when using the PGS solver. However, the TGS solver exhibits unexpected instability. Is this a known issue? Is there a workaround? or is it just a misconception on my part?

Library and Version

PhysX v5.1.3

Operating System

Linux

Steps to Trigger Behavior

1. Create a Simple Articulated body with two links and a spherical joint.
2. Set the child frame of the joint to a non-identity matrix.
3. Set Limit Params for every axis of motion.
4. Switch Between PGS and TGS Solver

Code Snippet to Reproduce Behavior

Articulation Create Code Snippet

static void createArticulation() {

    // Root Transformation Matrix
    PxMat44 restTf;
    restTf.column0 = PxVec4(0.9550508869395391, 0.2964415309802544f, -0.0004712388805974386f, 0.f);
    restTf.column1 = PxVec4(-0.29644151513976624f, 0.9550510023739125f, -0.00010471974330090952f, 0.f);
    restTf.column2 = PxVec4(0.00048110044630012515f, 0.f, 1.f, 0.f);
    restTf.column3 = PxVec4(3.f, 3.254410193471301f, 0.f, 1.f);

    // General Articulation Config
   //  gArticulation->setSolverIterationCounts(255U);
    gArticulation->setSolverIterationCounts(8, 1);
    gArticulation->setArticulationFlag(PxArticulationFlag::eFIX_BASE, true);
    gArticulation->setArticulationFlag(PxArticulationFlag::eDISABLE_SELF_COLLISION, true);


    // Root Link Shape, Material, and Properties
    PxMaterial* material = gPhysics->createMaterial(0.6f, 0.6f, 0.01f);
    PxShape* rootShape = gPhysics->createShape(PxCapsuleGeometry(1.f, 8.289946556091309 * 0.5f), *material);
    rootShape->setFlag(PxShapeFlag::eSIMULATION_SHAPE, true);
    rootShape->setRestOffset(0.02f);

    PxTransform pxtm{PxVec3(4.144f, 0.f, 0.f)};
    rootShape->setLocalPose(pxtm);

    const auto tol = gPhysics->getTolerancesScale();
    rootShape->setContactOffset(0.02f * tol.length);

    // Add Root Link to Articulation
    PxArticulationLink* rootLink = gArticulation->createLink(NULL, PxTransform(PxTransform(restTf)));
    rootLink->attachShape(*rootShape);
    rootLink->setName("root");
    rootLink->setMaxContactImpulse(PX_MAX_F32);
    rootLink->setMaxDepenetrationVelocity(PX_MAX_F32);
    rootLink->setLinearDamping(0.5f);
    rootLink->setAngularDamping(1.f);
    PxRigidBodyExt::updateMassAndInertia(*rootLink, 30.f);

    // Link 1 Shape, Material, and Properties
    PxMaterial* material1 = gPhysics->createMaterial(0.6f, 0.6f, 0.01f);
    PxShape* link1Shape = gPhysics->createShape(PxCapsuleGeometry(1.f, 1.7120577096939087 * 0.5f), *material1);
    link1Shape->setFlag(PxShapeFlag::eSIMULATION_SHAPE, true);
    link1Shape->setRestOffset(0.02f);

    pxtm = PxTransform{PxVec3(0.8560288548469543f, 0.f, 0.f)};
    link1Shape->setLocalPose(pxtm);

    rootShape->setContactOffset(0.02f * tol.length);

    // Add Link 1 to Articulation
    PxArticulationLink* link1 = gArticulation->createLink(rootLink, PxTransform(PxIdentity));
    link1->attachShape(*link1Shape);
    link1->setName("link1");
    link1->setMaxContactImpulse(PX_MAX_F32);
    link1->setMaxDepenetrationVelocity(PX_MAX_F32);
    link1->setLinearDamping(0.5f);
    link1->setAngularDamping(1.f);
    PxRigidBodyExt::updateMassAndInertia(*link1, 2.1980769634246826f);

    // Add joint between root and link 1
    PxArticulationJointReducedCoordinate* joint1 = link1->getInboundJoint();

    // Spherical joint
    joint1->setJointType(PxArticulationJointType::eSPHERICAL);

    // Define parent (from root to joint) and child frames (from link1 to joint) for joint
    PxMat44 parentFrame;
    parentFrame.column0 = PxVec4(0.201f, -0.980f, 0.f, 0.f);
    parentFrame.column1 = PxVec4(0.980f, 0.201f, 0.f, 0.f);
    parentFrame.column2 = PxVec4(0.f, 0.f, 1.f, 0.f);
    parentFrame.column3 = PxVec4(8.289946135028046f, 0.f, 0.f, 1.f);
    joint1->setParentPose(PxTransform(parentFrame));

    PxMat44 childFrame;
    childFrame.column0 = PxVec4(-1.0f, 0., 0.f, 0.f);
    childFrame.column1 = PxVec4(0.0f, -1.0f, 0.f, 0.f);
    childFrame.column2 = PxVec4(0.f, 0.f, 1.f, 0.f);
    childFrame.column3 = PxVec4(0.f, 0, 0.f, 1.f);
    joint1->setChildPose(PxTransform(childFrame));

    // We want joint to be limited
    joint1->setMotion(PxArticulationAxis::eTWIST, PxArticulationMotion::eLIMITED);
    joint1->setMotion(PxArticulationAxis::eSWING1, PxArticulationMotion::eLIMITED);
    joint1->setMotion(PxArticulationAxis::eSWING2, PxArticulationMotion::eLIMITED);

    // 30 deg -> 0.523599 rad
    PxArticulationLimit pxlimit(-0.523599, 0.523599);
    joint1->setLimitParams(PxArticulationAxis::eTWIST, pxlimit);

    // 45 deg -> 0.785398 rad
    pxlimit = PxArticulationLimit(-0.785398, 0.785398);
    joint1->setLimitParams(PxArticulationAxis::eSWING1, pxlimit);

    // 37.223 deg -> 0.65 rad
    pxlimit = PxArticulationLimit(-0.65, 0.65);
    joint1->setLimitParams(PxArticulationAxis::eSWING2, pxlimit);

    // Add Articulation to Scene
    gScene->addArticulation(*gArticulation);

    // SetGlobalPose instead of using cache to keep code as short as possible
    gArticulation->setRootGlobalPose(PxTransform(restTf));
    gArticulation->updateKinematic(PxArticulationKinematicFlag::ePOSITION);
}

Init Physics Code Snippet

void initPhysics(bool /*interactive*/) {
    gFoundation = PxCreateFoundation(PX_PHYSICS_VERSION, gAllocator, gErrorCallback);
    gPhysics = PxCreatePhysics(PX_PHYSICS_VERSION, *gFoundation, PxTolerancesScale());

    PxSceneDesc sceneDesc(gPhysics->getTolerancesScale());
    sceneDesc.gravity = PxVec3(0.0f, -9.81f, 0.0f);

    // // TGS Causes unstable behavior
    sceneDesc.solverType = PxSolverType::eTGS;
    // sceneDesc.solverType = PxSolverType::ePGS;
    sceneDesc.filterShader = PxDefaultSimulationFilterShader;

    PxU32 numCores = SnippetUtils::getNbPhysicalCores();
    gDispatcher = PxDefaultCpuDispatcherCreate(numCores == 0 ? 0 : numCores - 1);
    sceneDesc.cpuDispatcher = gDispatcher;
    gScene = gPhysics->createScene(sceneDesc);

    // Add a ground plane
    gMaterial = gPhysics->createMaterial(0.5f, 0.5f, 0.f);
    PxRigidStatic* groundPlane = PxCreatePlane(*gPhysics, PxPlane(0, 1, 0, 0), *gMaterial);
    gScene->addActor(*groundPlane);

    gArticulation = gPhysics->createArticulationReducedCoordinate();
    createArticulation();
}

Step Physics Code Snippet

void stepPhysics(bool /*interactive*/) {
    // Small dt, want to match my use case, tried larger dt as well but error persits
    const PxReal dt = 0.0052;

    gScene->simulate(dt);
    gScene->fetchResults(true);
}
int main() {
    // using render Loop from articulationrc snippet
    extern void renderLoop();
    renderLoop();
    return 0;
}

Expected Behavior

The articulation with the spherical joint should exhibit stable and accurate behaviors, regardless of the childFrame configuration. Both the TGS solver and the PGS solver should yield consistent results.

Actual Behavior

TGS

tgs.mp4

PGS

pgs.mp4

Thank you in advance!

[dtotsila]

@preist-nvidia did you have time to look into this issue?

[preist-nvidia]

Not yet, but it's on my list. And thank you for the excellent repro/bug report.

FYI, we are aware that the spherical articulation joints have several issues/inconsistencies that we are going to address after some higher-priority issues are done. A fix in 5.2.0 is unlikely at this point, however.

[preist-nvidia]

@dtotsila - any chance you can try to work around this issue using an identity CmassLocalPose?

[jcarius-nv]

Hi @dtotsila
thank you very much for reporting the issue and providing an excellent reproduction case.
I traced the instability down to a problem where the conversion from a quaternion to the angle-axis representation caused a flip of the joint angle by two pi. While visually not visible from the link state, this flip triggered the joint limits, which would then aggressively push the joint angle out of the limit and cause oscillatory motion.

The fix will appear in an upcoming release of PhysX. In the meantime, you can apply the following changes to your local version:

1. In the function PxQuat computeSphericalJointPositions(...) in file DyFeatherstoneForwardDynamic.cpp and
2. In the function PxTransform FeatherstoneArticulation::propagateTransform(...) in file DyFeatherstoneArticulation.cpp, add the lines

if(jointRotation.w < 0.0f)
  jointRotation = -jointRotation;

before that quaternion is converted into angle-axis format.

I hope this solves your problem. Please re-open the issue if it still persists after applying the changes.