The logic of velocity constraints and geometric error constraints in gear constraints is inconsistent

[SHiziw]

In gear constraint, I believe that from line 269 to line 270 constraints the absolute velocities (or global velocities) of two rigid bodies. (https://github.com/NVIDIA-Omniverse/PhysX/blob/main/physx/source/physxextensions/src/ExtGearJoint.cpp#L269-L270).
But when I continue to look at the logic of calculating con.geometricError in the function GearJoint::updateError(), then jump to line 156(https://github.com/NVIDIA-Omniverse/PhysX/blob/main/physx/source/physxextensions/src/ExtGearJoint.cpp#L156), I found that it obtains the angle from the relative position relationship of the joints.

This means that for the two joints that make up a gear, if the other rigid, which does not belong to the gear, of the joint is also moving, then the calculation of the above geometric error is incorrect.

To illustrate this problem more specifically, I have provided a snippet of code.
In this snippet, dynamicA and dynamicB make up a gear. DynamicB is rotation around dynamicB1, while dynamicB1 is also rotating.
I believe that under this gear constraint, dynamicA should not rotate, but in the simulation, it does rotate.

Also, I wonder if future versions will support planetary gears? It seems impossible to design a constraint for a planetary gear using the existing gear constraints.

Thanks!

Library and Version

PhysX v5.3.0

Code Snippet

#include <ctype.h>
#include "PxPhysicsAPI.h"
#include <extensions/PxD6JointCreate.h>// used by Joint
#include "../snippetcommon/SnippetPrint.h"
#include "../snippetcommon/SnippetPVD.h"
#include "../snippetutils/SnippetUtils.h"

using namespace physx;

static PxDefaultAllocator		gAllocator;
static PxDefaultErrorCallback	gErrorCallback;
static PxFoundation*			gFoundation = NULL;
static PxPhysics*				gPhysics	= NULL;
static PxDefaultCpuDispatcher*	gDispatcher = NULL;
static PxScene*					gScene		= NULL;
static PxMaterial*				gMaterial	= NULL;
static PxPvd*					gPvd        = NULL;
static PxD6Joint*               gMotor = NULL;
static PxD6Joint*               gMotor1 = NULL;

static PxRigidDynamic* createDynamic(const PxTransform& t, const PxGeometry& geometry, const PxVec3& velocity = PxVec3(0))
{
	PxRigidDynamic* dynamic = PxCreateDynamic(*gPhysics, t, geometry, *gMaterial, 10.0f);
	dynamic->setAngularDamping(0.5f);
	dynamic->setLinearVelocity(velocity);
	gScene->addActor(*dynamic);
	return dynamic;
}

void initPhysics(bool /*interactive*/)
{
	gFoundation = PxCreateFoundation(PX_PHYSICS_VERSION, gAllocator, gErrorCallback);
	gPvd = PxCreatePvd(*gFoundation);
	PxPvdTransport* transport = PxDefaultPvdSocketTransportCreate(PVD_HOST, 5425, 10);
	gPvd->connect(*transport,PxPvdInstrumentationFlag::eALL);

	auto tor = PxTolerancesScale();
	tor.length = 1.0f;
	tor.speed = 10.0f;
	gPhysics = PxCreatePhysics(PX_PHYSICS_VERSION, *gFoundation, tor, true, gPvd);
	PxInitExtensions(*gPhysics, gPvd);

	PxSceneDesc sceneDesc(gPhysics->getTolerancesScale());
	sceneDesc.gravity = PxVec3(0.0f, -9.81f, 0.0f);
	gDispatcher = PxDefaultCpuDispatcherCreate(2);
	sceneDesc.cpuDispatcher	= gDispatcher;
	sceneDesc.filterShader	= PxDefaultSimulationFilterShader;
	sceneDesc.solverType = PxSolverType::ePGS;
	gScene = gPhysics->createScene(sceneDesc);

	PxPvdSceneClient* pvdClient = gScene->getScenePvdClient();
	if(pvdClient)
	{
		pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_CONSTRAINTS, true);
		pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_CONTACTS, true);
		pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_SCENEQUERIES, true);
	}

	gMaterial = gPhysics->createMaterial(0.5f, 0.5f, 0.6f);

	PxRigidStatic* groundPlane = PxCreatePlane(*gPhysics, PxPlane(0,1,0,0), *gMaterial);
	gScene->addActor(*groundPlane);

	PxTransform glbPoseB{ 0.0f, 10.0f, 0.0f };
	PxTransform glbPoseB1{ 0.0f, 5.0f, 0.0f };
	PxTransform glbPoseA{ 0.0f, 10.0f, -10.0f };
	PxTransform glbFrameA{ PxVec3(0,10.0f,-10.0f), PxQuat(0, 0, 0.7071068f, 0.7071068f) };
	PxTransform glbFrameB{ PxVec3(0,10.0f,-0.0f), PxQuat(0, 0, 0.7071068f, 0.7071068f) };

	PxRigidDynamic* dynamicB = createDynamic(glbPoseB, PxBoxGeometry(2.0f, 0.5f, 0.5f));
	PxRigidDynamic* dynamicB1 = createDynamic(glbPoseB1, PxBoxGeometry(3.0f, 0.5f, 0.5f));
	PxRigidDynamic* dynamicA = createDynamic(glbPoseA, PxBoxGeometry(2.0f, 0.5f, 0.5f));
	dynamicA->setSolverIterationCounts(80, 5);
	dynamicB->setSolverIterationCounts(80, 5);

	PxD6Joint* pJointB1 = PxD6JointCreate(*gPhysics, nullptr, glbFrameB, dynamicB1, glbPoseB1.transformInv(glbFrameB));
	pJointB1->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
	pJointB1->setMotion(PxD6Axis::eX, PxD6Motion::eLOCKED);
	pJointB1->setMotion(PxD6Axis::eY, PxD6Motion::eLOCKED);
	pJointB1->setMotion(PxD6Axis::eZ, PxD6Motion::eLOCKED);
	pJointB1->setMotion(PxD6Axis::eSWING1, PxD6Motion::eLOCKED);
	pJointB1->setMotion(PxD6Axis::eSWING2, PxD6Motion::eLOCKED);

	PxD6Joint* pJointB = PxD6JointCreate(*gPhysics, dynamicB1, glbPoseB1.transformInv(glbFrameB), dynamicB, glbPoseB.transformInv(glbFrameB));
	pJointB->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
	pJointB->setMotion(PxD6Axis::eX, PxD6Motion::eLOCKED);
	pJointB->setMotion(PxD6Axis::eY, PxD6Motion::eLOCKED);
	pJointB->setMotion(PxD6Axis::eZ, PxD6Motion::eLOCKED);
	pJointB->setMotion(PxD6Axis::eSWING1, PxD6Motion::eLOCKED);
	pJointB->setMotion(PxD6Axis::eSWING2, PxD6Motion::eLOCKED);

	PxD6Joint* pJointA = PxD6JointCreate(*gPhysics, nullptr, glbFrameA, dynamicA, glbPoseA.transformInv(glbFrameA));
	pJointA->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
	pJointA->setMotion(PxD6Axis::eX, PxD6Motion::eLOCKED);
	pJointA->setMotion(PxD6Axis::eY, PxD6Motion::eLOCKED);
	pJointA->setMotion(PxD6Axis::eZ, PxD6Motion::eLOCKED);
	pJointA->setMotion(PxD6Axis::eSWING1, PxD6Motion::eLOCKED);
	pJointA->setMotion(PxD6Axis::eSWING2, PxD6Motion::eLOCKED);

	PxGearJoint* gearConstraint = PxGearJointCreate(*gPhysics, dynamicA, glbPoseA.transformInv(glbFrameA), dynamicB, glbPoseB.transformInv(glbFrameB));
	auto watch = gearConstraint->setHinges(pJointA, pJointB);
	PX_UNUSED(watch);
	gearConstraint->setGearRatio(1);

	
	PxD6Joint* motor1 = PxD6JointCreate(*gPhysics, nullptr, glbFrameB, dynamicB1, glbPoseB1.transformInv(glbFrameB));
	motor1->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
	motor1->setMotion(PxD6Axis::eX, PxD6Motion::eLOCKED);
	motor1->setMotion(PxD6Axis::eY, PxD6Motion::eLOCKED);
	motor1->setMotion(PxD6Axis::eZ, PxD6Motion::eLOCKED);
	motor1->setMotion(PxD6Axis::eSWING1, PxD6Motion::eLOCKED);
	motor1->setMotion(PxD6Axis::eSWING2, PxD6Motion::eLOCKED);
	motor1->setDrive(PxD6Drive::eTWIST, PxD6JointDrive(0, 1000, PX_MAX_REAL, true));
	gMotor1 = motor1;

	PxD6Joint* motor = PxD6JointCreate(*gPhysics, dynamicB1, glbPoseB1.transformInv(glbFrameB), dynamicB, glbPoseB.transformInv(glbFrameB));
	motor->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
	motor->setMotion(PxD6Axis::eX, PxD6Motion::eLOCKED);
	motor->setMotion(PxD6Axis::eY, PxD6Motion::eLOCKED);
	motor->setMotion(PxD6Axis::eZ, PxD6Motion::eLOCKED);
	motor->setMotion(PxD6Axis::eSWING1, PxD6Motion::eLOCKED);
	motor->setMotion(PxD6Axis::eSWING2, PxD6Motion::eLOCKED);
	motor->setDrive(PxD6Drive::eTWIST, PxD6JointDrive(0, 1000, PX_MAX_REAL, true));
	gMotor = motor;
}

void stepPhysics(bool /*interactive*/)
{
	gScene->simulate(1.0f/60.0f);
	gScene->fetchResults(true);
	gMotor->setDriveVelocity(PxVec3(0), PxVec3(-3, 0, 0));
	gMotor1->setDriveVelocity(PxVec3(0), PxVec3(3, 0, 0));
}
	
void cleanupPhysics(bool /*interactive*/)
{
	PX_RELEASE(gScene);
	PX_RELEASE(gDispatcher);
	PxCloseExtensions();
	PX_RELEASE(gPhysics);
	if(gPvd)
	{
		PxPvdTransport* transport = gPvd->getTransport();
		gPvd->release();	gPvd = NULL;
		PX_RELEASE(transport);
	}
	PX_RELEASE(gFoundation);
	
	printf("SnippetJoint done.\n");
}

void keyPress(unsigned char key, const PxTransform& camera)
{
	switch(toupper(key))
	{
	case ' ':	createDynamic(camera, PxSphereGeometry(.5f), camera.rotate(PxVec3(0,0,-1))*200);	break;
	}
}

int snippetMain(int, const char*const*)
{
#ifdef RENDER_SNIPPET
	extern void renderLoop();
	renderLoop();
#else
	static const PxU32 frameCount = 100;
	initPhysics(false);
	for(PxU32 i=0; i<frameCount; i++)
		stepPhysics(false);
	cleanupPhysics(false);
#endif

	return 0;
}

[preist-nvidia]

Thank you for the detailed report and snippet! We will have a look. Internal Tracking: PX-4649

[PierreTerdiman]

I see the issue but I don't see the solution yet. We need a ground truth source for computing the geometric error, so we cannot use the world-space rotations like the solver does, since they would include the error we wish to correct.

A potential workaround would be to omit the setHinges call:

auto watch = gearConstraint->setHinges(pJointA, pJointB);

This is only used to compute & update the geometric error. If you omit that call the gear joints still work, but they never try to correct potential drift (which means the gear teeth can start overlapping after errors accumulate). If you omit that call in your snippet, I think you get the behavior you expect.

Another thing: I am not sure why you create another joint here:

		PxD6Joint* motor1 = PxD6JointCreate(*gPhysics, nullptr, glbFrameB, dynamicB1, glbPoseB1.transformInv(glbFrameB));
		motor1->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
		motor1->setMotion(PxD6Axis::eX, PxD6Motion::eLOCKED);
		motor1->setMotion(PxD6Axis::eY, PxD6Motion::eLOCKED);
		motor1->setMotion(PxD6Axis::eZ, PxD6Motion::eLOCKED);
		motor1->setMotion(PxD6Axis::eSWING1, PxD6Motion::eLOCKED);
		motor1->setMotion(PxD6Axis::eSWING2, PxD6Motion::eLOCKED);
		motor1->setDrive(PxD6Drive::eTWIST, PxD6JointDrive(0, 1000, PX_MAX_REAL, true));
		gMotor1 = motor1;

You could just add the drive to the already existing pJointB1 joint:

			pJointB1->setDrive(PxD6Drive::eTWIST, PxD6JointDrive(0, 1000, PX_MAX_REAL, true));
			gMotor1 = pJointB1;

(same for motor/pJointB) It does not change anything but I got confused by this setup.

I did not try planetary gears but I can see why the issue you pointed out could make them difficult to achieve.

I think we'll need more time to come up with a good solution to this issue.

[PierreTerdiman]

Got slightly obsessed with that one, and I might have something.

Here is an alternative version of the "updateError" function, that does not use the hinge joints anymore. It's an experimental work-in-progress but it seems to work in the test scenes I have that use gear joints. And as far as I can tell in your snippet the gears don't rotate anymore (as you expected). If this works we could also simplify our API and get rid of the setHinges() function, although this will require a lot more tests and more eyes on that code to confirm that it is sane.

static PX_FORCE_INLINE	PxQuat	getTwist(const PxQuat& q)
{
	// PT: TODO: we don't need to compute both quats here
	PxQuat swing, twist;
	PxSeparateSwingTwist(q, swing, twist);
	(void)swing;
	return twist;
}

static PxReal getTwistAngle_Internal(const PxQuat& q)
{
	const PxQuat twist = getTwist(q);

	PxReal angle = twist.getAngle();

	if(twist.x<0.0f)
		angle = -angle;

	return angle;
}

void GearJoint::updateError()
{
	GearJointData* data = static_cast<GearJointData*>(mData);

	PxRigidActor* gearActor0;
	PxRigidActor* gearActor1;
	getActors(gearActor0, gearActor1);

	// globalPose = body.getBody2World() * body.getBody2Actor().getInverse();
	// globalPose = body.getBody2World() * body.getCMassLocalPose().getInverse();
	// globalPose = body.getBody2World() * body.getCMassLocalPose().getInverse();
	// globalPose * body.getCMassLocalPose() = body.getBody2World()

	// #### todo: cannot be static?
	PxRigidDynamic* dynActor0 = static_cast<PxRigidDynamic*>(gearActor0);
	PxRigidDynamic* dynActor1 = static_cast<PxRigidDynamic*>(gearActor1);

	const PxTransform pose0 = dynActor0->getGlobalPose() * dynActor0->getCMassLocalPose();
	const PxTransform pose1 = dynActor1->getGlobalPose() * dynActor1->getCMassLocalPose();

	PxTransform32 cA2w, cB2w;
	joint::computeJointFrames(cA2w, cB2w, *data, pose0, pose1);

	// #### todo: simplify
	const PxVec3 gearAxis0 = cA2w.rotate(PxVec3(1.0f, 0.0f, 0.0f)).getNormalized();
	const PxVec3 gearAxis1 = cB2w.rotate(PxVec3(1.0f, 0.0f, 0.0f)).getNormalized();

	float Angle0, Angle1;

	{
		const PxPlane gear0plane(cA2w.p, gearAxis0);
		const PxVec3 p1proj = gear0plane.project(cB2w.p);
	//	const PxVec3 dir = (cA2w.p - cB2w.p).getNormalized();
		const PxVec3 dir = (cA2w.p - p1proj).getNormalized();

		const PxVec3 n = gearAxis0.cross(dir).getNormalized();
		PxMat33 m(gearAxis0, -n, dir);
		PxQuat q(m);

//		PxMat33 test(cA2w.q);

		PxTransform tmp(q);
		Angle0 = ::getTwistAngle_Internal(tmp.transformInv(cA2w).q);
	}
	{
		const PxPlane gear1plane(cB2w.p, gearAxis1);
		const PxVec3 p0proj = gear1plane.project(cA2w.p);
	//	const PxVec3 dir = (cA2w.p - cB2w.p).getNormalized();
		const PxVec3 dir = (p0proj - cB2w.p).getNormalized();

		const PxVec3 n = gearAxis1.cross(dir).getNormalized();
		PxMat33 m(gearAxis1, -n, dir);
		PxQuat q(m);

//		PxMat33 test(cB2w.q);

		PxTransform tmp(q);
		Angle1 = ::getTwistAngle_Internal(cB2w.transformInv(tmp).q);
	}

	if(!mInitDone)
	{
		mInitDone = true;
		mPersistentAngle0 = Angle0;
		mPersistentAngle1 = Angle1;
	}

	{
		const float travelThisFrame0 = angleDiff(Angle0, mPersistentAngle0);
		const float travelThisFrame1 = angleDiff(Angle1, mPersistentAngle1);
		mVirtualAngle0 += travelThisFrame0;
		mVirtualAngle1 += travelThisFrame1;

		//printf("travelThisFrame0: %f\n", travelThisFrame0);
		//printf("travelThisFrame1: %f\n", travelThisFrame1);
		//printf("ratio: %f\n", travelThisFrame1/travelThisFrame0);
		mPersistentAngle0 = Angle0;
		mPersistentAngle1 = Angle1;
	}

	//const float error = Sign0*mVirtualAngle0*data->gearRatio - Sign1*mVirtualAngle1;
	const float error = mVirtualAngle0*data->gearRatio - mVirtualAngle1;
//	printf("error: %f\n", error);
//	printf("mVirtualAngle0:  %f\n", mVirtualAngle0);
//	printf("mVirtualAngle1:  %f\n", mVirtualAngle1);
//	printf("\n");

	data->error = error;
	markDirty();
}

[SHiziw]

Hi PT:
Glad to receive your reply.
I would like to explain first why I would add an extra joint to dynamicB1. In real life, we use bearings on a shaft (in many cases, even more than one bearing) to allow the shaft to rotate around its own axis. And if we want this shaft to rotate, we will use an additional motor linked to one end of the shaft, rather than adding a driver or something at the bearing.
In order to simulate the movement of the shaft in reality as much as possible, I added this extra hinge joint to approximate the function of the motor.

And the new method of calculating the cumulative error seems to have obtained the absolute pose of the rigidbody? Looks cool! I think I can have a try to see how it performs :)

[preist-nvidia]

Closing for now, please reopen if there is more to discuss.