Featherstone's solver for articulations

Thank you, kstorey.

I am using drives on the fingers (yields the best friction); I’m not sure the current Unity API allows for Articulation roots to be parented/constrained to other Rigidbodies or have spatial linear drives... yet... but luckily the Force/Torque solution I’m using works relatively well.

The new properties in 2020.1a23 should be very useful for suppressing solver instability. The physics timestep is currently running at 90hz, but increasing that may be another path towards improvement (since I’m able to sample interpolated hand tracking frames for each physics timestep as well).

Is there a plan for implementing “Rigidbody interpolation” for articulations, Yant? (It re-interpolates the motion of the objects from the physics timestep to the render timestep). If not, I’ll need to keep the simulation timestep as a multiple of the framerate to maintain smooth motion...

 

How to enable..? Are PhysX Articulations calculated on GPU?

 

It's ok, new AMD processors have very high core count

 

Cool! That would make Unity much more universal.

Upgrading to new alpha version as we speak, might report soon about improvements.

 

So I made a simple test scene for spherical joints.

It looks like there's a bug in the way that the X, Y, and Z drives work together... I suspect there's a wrongly-ordered quaternion multiplication somewhere.

Here's the code and scene I'm using:

Code (CSharp):

1. using UnityEngine;
2. public class ArticulationTest : MonoBehaviour {
3.     public ArticulationBody body;
4.     void FixedUpdate() {
5.         // Normalize Quaternion to suppress greater than 360 degree rotations
6.         Quaternion normalizedRotation = Quaternion.Normalize(transform.rotation);
7.  
8.         // Calculate drive targets
9.         Vector3 driveTarget = new Vector3(
10.           Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.x),
11.           Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.y),
12.           Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.z));
13.  
14.         // Copy the X, Y, and Z target values into the drives...
15.         ArticulationDrive xDrive = body.xDrive; xDrive.target = driveTarget.x; body.xDrive = xDrive;
16.         ArticulationDrive yDrive = body.yDrive; yDrive.target = driveTarget.y; body.yDrive = yDrive;
17.         ArticulationDrive zDrive = body.zDrive; zDrive.target = driveTarget.z; body.zDrive = zDrive;
18.     }
19. }
20.  

 

After some trial-and-error, drive code without any singularities looks like this:

Code (CSharp):

1.  
2. using UnityEngine;
3. public class ArticulationTest : MonoBehaviour {
4.     public ArticulationBody body;
5.     public float Strength = 1f;
6.     void FixedUpdate() {
7.         // Calculate the delta between the current rotation and the desired rotation
8.         Quaternion deltaRotation = Quaternion.Normalize(Quaternion.Inverse(body.transform.localRotation) * transform.rotation);
9.  
10.         // Calculate drive velocity necessary to undo this delta in one fixed timestep
11.         ArticulationReducedSpace driveTargetForce = new ArticulationReducedSpace(
12.           ((Mathf.DeltaAngle(0, deltaRotation.eulerAngles.x) * Mathf.Deg2Rad) / Time.fixedDeltaTime) * Strength,
13.           ((Mathf.DeltaAngle(0, deltaRotation.eulerAngles.y) * Mathf.Deg2Rad) / Time.fixedDeltaTime) * Strength,
14.           ((Mathf.DeltaAngle(0, deltaRotation.eulerAngles.z) * Mathf.Deg2Rad) / Time.fixedDeltaTime) * Strength);
15.  
16.         // Apply the force in local space (unlike AddTorque which is global space)
17.         // Ideally we'd use inverse dynamics or jointVelocity, but jointVelocity is bugged in 2020.1a22
18.         body.jointForce = driveTargetForce;
19.     }
20. }
21.  

I assume this can be substituted in for the existing xDrive, yDrive, and zDrive code in the engine. The bug probably comes from one of two places...

1. You can't just reconstruct the body's orientation from the reducedCoordinate joint positions (it's unclear why not, probably a different ordering in which x, y, and z are applied):

Code (CSharp):

1.         // This will give you the incorrect orientation
2.         Quaternion jointRotation = Quaternion.Euler(
3.             body.jointPosition[0] * Mathf.Rad2Deg,
4.             body.jointPosition[1] * Mathf.Rad2Deg,
5.             body.jointPosition[2] * Mathf.Rad2Deg);

2. It's possible the ordering of the quaternion multiply in line 8 above is reversed; I see similar unstable behavior in my code when I intentionally reverse the order...

EDIT: It looks like setting the jointVelocity directly is bugged for any of the joints further down the kinematic chain... Looks like another bug...

EDIT 2: Setting jointForce has a similar effect, but it propagates down the chain properly. Please fix jointVelocity!

EDIT 3: Colliders that are children of the articulation do not appear to be detected by the articulation's joints... Do they need to be fixed joint articulations?

EDIT 4: Fixed Joint articulation children do not appear to be incorporated properly either... Looks like I'm not going to be able to weasel out of some deep quaternion management if I want oriented colliders in my articulations...

EDIT 5: Setting jointForce on a branching articulation (eg a humanoid) hard-crashes the editor

 

Hey, try to have at least one collider on every Articulation, with non-zero weight. It can be very small sphere, I think that helped me in the past.

 

Thank you, I did try that, but only the rotary joint's small, centered collider affected the inertia of the system appropriately.

 

Btw, I noticed new alpha is out:

 

Unfortunately I wasn't able to use the maxAngularVelocity or maxDepenetrationVelocities to achieve any additional stability with respect to spherical joints... (and setting jointVelocity still crashes ).

 

Hey yant, how hard it would it be to get other friction types working? It's not urgent right now, but I believe it will come handy with gripper physics.

 

Just started messing with Articulations in 2020.1.0a25 today, super fun! I setup a simple object with a few revolute joints and I'm trying to create motor-ish controls for the joints for some RL experiments I want to run -- I've got it working, but some of the behavior isn't quite what I'd expect. Here's my test controller:

Code (CSharp):

1. public class RevoluteController : MonoBehaviour {
2.     public Vector3 torque = Vector3.forward * 5.0f;
3.     void FixedUpdate() {
4.         // Set torque to be applied based on state of F and G keys
5.         if (!Input.GetKey(KeyCode.F) && !Input.GetKey(KeyCode.G)) { return; }
6.         Vector3 tt = torque * Time.deltaTime * 100.0f;
7.         if (Input.GetKey(KeyCode.G)) { tt *= -1.0f; }
8.  
9.         // Apply torque to children
10.         Vector3 antiTorque = Vector3.zero;
11.         foreach (Transform child in transform) {
12.             child.gameObject.GetComponent<ArticulationBody>().AddRelativeTorque(tt);
13.             antiTorque -= child.localToWorldMatrix.MultiplyVector(tt);
14.         }
15.  
16.         // Apply inverse torque to this object
17.         gameObject.GetComponent<ArticulationBody>().AddTorque(antiTorque);
18.     }
19. }

Without applying the inverse torque to the parent, pressing F/G keys will spin the whole object even when the children have reached limits; with the inverse torque applied, angular moment seems to be conserved. The issue I'm running into is that non-angular momentum is *not* conserved -- the object can jump into the air (expected) and then start flying around in different directions as I toggle the F and G keys (not expected!).

I spent a little bit of time trying to figure out how to use jointForce for the antiTorque instead of naively using the applied torque as it will cause weird behavior if the applied torque isn't perfectly aligned with the unconstrained axis, but couldn't figure out how to convert jointForce to a local or world vector.

I then tried to switch to a drive-based approach, but couldn't figure out how to get my controller to set the xDrive target. This...

Code (CSharp):

1.         foreach (Transform child in transform) {
2.             ArticulationBody ab = child.gameObject.GetComponent<ArticulationBody>();
3.             ArticulationDrive ad = ab.xDrive;
4.             ad.target = targetRotation;
5.         }

...does not seem to be updating the drive target. Meanwhile, this...

Code (CSharp):

1.         foreach (Transform child in transform) {
2.             ArticulationBody ab = child.gameObject.GetComponent<ArticulationBody>();
3.             ab.xDrive.target = targetRotation;
4.         }

...gives a compile error: "error CS1612: Cannot modify the return value of 'ArticulationBody.xDrive' because it is not a variable"

So a few questions:

1. How can I simulate a motor and apply torque in a way that does not affect the momentum of the Articulation object?
2. How can I convert jointForce to a vector in local or world space?
3. What's the best way to update an ArticulationBody drive target from a script?
4. Am I going about this all wrong?

Thanks!

 

ArticulationDrive is a struct, try:

Code (CSharp):

1. foreach (Transform child in transform) {
2.     ArticulationBody ab = child.gameObject.GetComponent<ArticulationBody>();
3.     ArticulationDrive ad = ab.xDrive;
4.     ad.target = targetRotation;
5.     ab.xDrive = ad;
6. }

 

@tri-quad
Yep, what CodeKiwi said.
Structs are copied every time, classes are not. Modifying struct in one place does not reflect changes in other places.

 

hello, is there a way to lock the tip of an articulation tree (position and rotation). For example the tip of a rope that's gets stuck in a branch; Or a robotic arm trying to lift something that is fixed in world space, so it gets stuck.

 

Anyone has problems with Articulated bodies drifting over time?

No, not yet. Supposedly Articulations with Joints will be coming soon.

 

Ok, I have my robot on wheels; and I found out if the root articulation has bigger mass, the more it drifts. Otherwise,if I set bigger mass onto my wheels, the drifting stops.

 

How have you put the articulation on wheels?

 

It's caster wheel.

SphereCollider - Revolute Articulation (wheel axis) - Revolute Articulation (caster axis) - Prismatic Articulation (a bit of spring/damping) - Root Body.

I have 4 of those those.

 

Any chance for other friction models?

 

I've just asked about that:
https://forum.unity.com/threads/new-friction-features-in-physx-4.864610/

 

I don't know if this is a thing but when I was rigging a ragdoll with it I noticed that adding and deleting articulations on random parent/child objects crashed Unity 2020.1 b4 fairly often. Perhaps it's the code that checks the hierarchy is at fault and does not recognise a removed body, feels like that sort of thing.

It only happens with articulations.

A little feedback: I love the unified body/joint concept, and the easy relationship in the hierarchy. Sometimes you need strong joints on things that are actually also physics objects such as car doors. What is the advice here? Should I teleport the door every fixed update to match the car body position?

 

Thank you for your work, I'm very impressed by how powerful and usable the integration is coming along!

 

When the door in this scenario is hit, will forces propagate to the rest of the regular rigidbodies that are connected?

If not I can always apply a force at position for the the other bodies, but I thought I would ask.

 

Of course it helps a ton, thanks!

 

Happy to hear this articulation & old joint is WIP, it opens more doors for robotics.

 

I was thinking of robust joints for VR stuff really. Also ragdoll too!

 

Thanks. Patch friction only works with Articulations, all others crash the Unity. This is why I asked.

Didn't knew there were even more options - I welcome all of them, since I am noticing the un-realism in my case too.

 

Thank you for this functionality. We're working towards simulating our humanoid robot in Unity (to have much higher fidelity cameras than other physics engines).

A major part of getting our controller running with Unity is to simulate an IMU (inertial measurement unit). For this we need orientation, angular velocity and linear acceleration. Orientation and angular velocities are easy, but is there any way to expose the linear acceleration? Differentiating the linear velocity could add noise to the measurement and it is not ideal.

Also, am I correct that the jointForce and drive output is additive? Our controller is force-based and calculates joint torques directly.

 

We would like to use the same model for simulation and visualization of external data, therefore we have a need to disable/enable the articulation bodies (in the editor, not at runtime).

When playing with disabling articulation bodies I found some small issues

- Disabling ArticulationBodies not starting from the leaf results in a PhysX error (cannot disable non-leaf bodies)
- After disabling all ArticulationBodies from the leaf to the root, disabling the root results in a hard Unity crash when selecting a child node.

To avoid this, I created a helper script that stores all properties of the joint/link and then removes the ArticulationBody if I do not need physics.

Again, thanks for this feature. This really pushes forward our plan to create a Unity based simulator for our robot.

 

hey there,

I suspect ArticulationBodies also use colliders attached to disabled GameObjects to calculate inertia. I'm not sure if this is the intended behaviour, but it seems like it's different from the behaviour of regular Rigidbodies.

In my case, I have objects with colliders that are used for regular collisions (convex mesh colliders) but also non-convex mesh colliders that I just want to use for selecting parts of the object in game, so they are only used for Raycasts. Those mesh colliders are not only non-convex, but also don't form a volume (for example just a single triangle). When I attach an ArticulationBody to the described game object, I get the following errors:

Code (CSharp):

1. [Physics.PhysX] computeMassAndInertia: Dynamic actor with illegal collision shapes
2. [Physics.PhysX] PxRigidBodyExt::setMassAndUpdateInertia: Mass and inertia computation failed, setting mass to 1 and inertia to (1,1,1)

Those errors do not appear when Rigidbodies are added. Would it be possible to change the behaviour to either silently not to take degenerate colliders into account or not to use colliders that are attached to disabled game objects?

 

Glad to hear that .

 

Just tested 2020.1b8 and seems like it works now. Thanks a bunch!

 

I see a major overhead in querying ArticulationBody.jointPosition and ArticulationBody.jointVelocity. (taking about 0.2ms for 23 joints)

We are running a humanoid robot controller @ 1KHz fixedUpdate rate (this is the same as on the real robot and required for a stable sim). Running the controller in a separate thread, we can get 0.8x realtime rate. However, if we do not poll the joint position/velocity we can get 1x realtime rate (although it'll fall of course).

Is there a possibility to speed up polling these values?

 

@JesperSmith did you ever manage to answer this question
"Also, am I correct that the jointForce and drive output is additive? Our controller is force-based and calculates joint torques directly."

I would like to implement a force-based controller on a simulated robot, but I'm unclear on how this could be deployed without conflicts with ArticulationDrive

 

Bug: When you raycast against an ArticulationBody, the resulting RayCastHit will have a non-null rigidbody field (even though ArticulationBody's don't inherit from Rigidbody). If you try to access any of the properties on the rigidbody, it crashes the editor!

 

Hello @yant, I just made a quick test with articulationBody and indeed its a step forward, especially for a full physic vehicle (vehicle with actual rotating wheels). Btw, I found two issues:
1) maxAngVelo seems to be capped to 100 rads/sec no matter what.
2) PGS solver seems more stable than TGS solver at high angVelo

Also, is there a way to obtain the collision impulse like with regular rigidbody/collider?

 

@yant I'm noticing some performance issues when increasing time.fixedTimestep to address stabilities issues.

Is there anything you recommend either to increase stability at lower frequencies or to improve performance?

I did not see the same performance hit when I was using ConfiguabeJoints and scaling up to even higher frequencies.

One thing to note is that I keep the update to the joint a constant 30 fps regardless of how much I scale fixedTimestep. With ConfiguabeJoints, I noticed a performance hit if I updated the joint at every physics step. Therefore, I suspect that ArticulationBody applies forces to the underlying physics objects at each step, and that this is why I see the impact on performance.

To put in context, I could train my ConfiguabeJoint agent for 128m steps in 10hrs. With ArticulationBody it is taking about 30hrs (so 3x)

- foot goes through the floor when time.fixedUpdate = 1/60

is much more stable when time.fixedUpdate = 1/150

 

I did some more tests - in the video, I trained the model with the physics running at 50fps.

From 0-16s the physics is running at 150fps, from 16s I run the physics at 60fps (normal speed and slow mo) - the way the leg goes through the floor looks more like a bug. I tested with a plane and with terrain and get the same problem with both