Torque Control for PxArticulationReducedCoordinate

[costashatz]

Hello,

This issue is related #149. I cannot find a way to apply my torques to an articulation. Here's a high-level description of my code:

PxArticulationReducedCoordinate* articulation = physics->createArticulationReducedCoordinate();

// Fill/Create articulation links/joints etc..
// Mostly Spherical joints

articulation->setArticulationFlag(PxArticulationFlag::eFIX_BASE, true);

scene->addArticulation(articulation);

// Cache
PxArticulationCache* cache = articulation->createCache();

// simulation loop
while (1) {
    scene->simulate(stepsize);
    scene->fetchResults(true);

    articulation->zeroCache(*cache);
    articulation->copyInternalStateToCache(*cache, PxArticulationCache::eALL);

    for (int i = 0; i < articulation->getDofs(); i++) {
        cache->jointForce[i] = 100 * (target_position[i] - cache->jointPosition[i]);
    }

    articulation->applyCache(*cache, PxArticulationCache::eFORCE);
}

With this, nothing happens. My articulation is behaving like not having any force at all. I get identical results if I do not put any "control".

Any ideas? I am surely missing something. Can you point what I am doing wrong?

Thanks in advance!

[preist-nvidia]

Hi Konstantinos!

What you are doing looks right - you are writing the forces into the cache and then applying them using applyCache and the correct flag. So I am not sure why you are not seeing any result. A few questions:

1. Sounds like it does, but did you verify that the joint motion is setup properly and the joints move?
2. Is your gain "100" properly tuned? I.e. does it related to the masses/inertias such that 100 * deltaPos actually could have an effect?
3. Do you get the proper behavior if you configure the articulation joint drive (PxArticulationJointReducedCoordinate::setDrive) with the same gain? The drives can also handle very large gains so you can just crank it up to check if it is a gain issue.

Hope this helps,
Philipp

[costashatz]

Hello,

Thanks for the fast reply.

1. Sounds like it does, but did you verify that the joint motion is setup properly and the joints move?

Yes the motions are correctly set-up and I am able to move the joints with the "traditional" pd drives.

2. Is your gain "100" properly tuned? I.e. does it related to the masses/inertias such that 100 * deltaPos actually could have an effect?

Hmm.. This was the issue. Putting a much higher gain I was able to make the joints move. Thanks!

I cannot however get the non linear forces (gravity, coriolis and centrifugal) to act on the system. I saw the documentation here, and I have a code as follows:

// inside the simulation loop
articulation->zeroCache(*cache);
articulation->commonInit();
articulation->copyInternalStateToCache(*cache, PxArticulationCache::eALL);
articulation->computeGeneralizedGravityForce(*cache);
articulation->copyInternalStateToCache(*cache, PxArticulationCache::eVELOCITY);
articulation->computeCoriolisAndCentrifugalForce(*cache);
articulation->computeJointAcceleration(*cache);
articulation->computeJointForce(*cache);

...

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

Is this correct? I guess I am missing something here, because the above does not have any effect on the system. Thanks again!

[preist-nvidia]

Hi Konstantinos,

Nice, glad you could resolve the issue.

For the inverse kinematics, you need to be aware of these two things:

1. Call commonInit after you set the articulation state (root pos/vel + joint pos/vel) - and keep it before the compute, but that you have already.
2. The compute functions take some cache elements as input, e.g. computeJointForce takes in the desired joint accelerations, and the full articulation state (same data as you use for setting the state before commonInit), and then writes the computed joint force to the cache force arrays. So if you call all the compute functions sequentially with the same cache, the force array in the cache is overwritten each time, and you end up with the force result of the last call in the cache. So you need to use separate caches and then sum in a cache that you then apply.

Philipp

[costashatz]

OK I see!

Thanks for the prompt answer once again! I will check and let you know.

I'll keep this open until I manage to solve it and then close it.

Thanks,
Konstantinos

[costashatz]

@preist-nvidia I have managed to apply gravity+coriolis+centrifugal forces to my articulations! Thanks for the help. I am using the following code:

// Get data from PhysX
pxreduced->zeroCache(*pxcache);
pxreduced->zeroCache(*pxcache1);
pxreduced->commonInit();
pxreduced->copyInternalStateToCache(*pxcache, PxArticulationCache::eALL);
pxreduced->copyInternalStateToCache(*pxcache1, PxArticulationCache::eALL);

pxreduced->computeGeneralizedGravityForce(*pxcache);
pxreduced->computeCoriolisAndCentrifugalForce(*pxcache1);

for(int i = 0; i < pxreduced->getDofs(); i++) {
        pxcache->jointForce[i] = pxcache->jointForce[i] + pxcache1->jointForce[i];
}

...

pxreduced->applyCache(*pxcache, PxArticulationCache::eFORCE);

I also checked the resulting force against other modelling libraries (e.g. pinocchio) and I am getting identical values. So I am confident that I am now doing this correctly.

However, I am getting a bit mixed results. Sometimes it works okayish (the simulation is being restarted every now and then):

gravity_compensation_success.mp4

Sometimes it fails completely:

gravity_compensation_fail.mp4

The first behavior is somewhat expected if PhysX in the background is not simply using the Featherstone algorithm and adds other constraints and iterations; we cannot have a perfect gravity compensation. The second behavior, however (the arm falling down completely), is not what I would have expected. I am using joints with quite big friction coefficient (set to 1; I also tried with zero and default values, did not change anything) and I would expect that the non-linear effects forces would make it stay up.

Is what I am seeing expected when using PhysX (i.e., PhysX's computations can lead to these behaviors), or I am still doing something wrongly? I tried computing the jointAccelerations (pxreduced->computeJointAccelerations(*pxcacache)) and applying forces and accelerations to the articulation, but it did not really change anything. Any ideas on what could be wrong? Thanks in advance!

[preist-nvidia]

Hmm not clear to me why it is failing, especially if you see force values that make sense. A little bit of drift could be expected, but not a complete failure.

Maybe do a simple test of applying joint forces to a 1-D setup (e.g. single revolute joint) to see if the application works?

Just in case, you can ensure that sleeping does not interfere, and set the articulation's wakeCounter to a high value (setWakeCounter)

[costashatz]

Hello! I found my issue. I had a small bug and I was setting the gravity to -98.1, so everything was "different".. Thanks for all the help!

[preist-nvidia]

Glad you found the issue! My pleasure!