Featherstone's solver for articulations

Thanks Yant! some comments on the technical intro:

I don’t get to see all the joint information when I add a artictulationbody component. I only see the mass field and that’s it.. Ah now I see you have to put it in the hierarchy under the root body to make it appear. Perhaps good to know for others. But if the hierarchy is the way to define connections, how do we create circular joint configurations?

the damping ratio d is also allowed to be larger than 1. (In this case it is 'overdamped' and really slows down the dynamics.) Is the ‘Damping’ of the drive the actual damping or the damping ratio? My quess it is the damping ratio?

I’m also very interested in how these articulationbodies are supposed to interact with rigidbodies. Simply through their colliders and that’s it? It seems like it is that simple.. no caveats?

 

Thanks for creating new thread!

 

Suggestion:

Add planar joint type, you can build this behind the scenes with two prismatic joints to form a plane

 

Suggestion/need #2:

Would be nice to have way to update inertia tensor & center of mass of each ArticulationBody, but is not critical at the moment

 

I wrote an extension to ArticulationBody for GetPointVelocity and AddForceAtPosition. I attached the file to this post but here's the code, too. Please check if I got my math right. Testing with simple configurations it intuitively seems correct to me.

Code (CSharp):

1. public static class MyExtensions
2. {
3.     public static Vector3 GetPointVelocity(this ArticulationBody ab, Vector3 worldPosition)
4.     {
5.         var v = ab.velocity;
6.         var w = ab.angularVelocity;
7.         var lever = worldPosition - ab.centerOfMass;
8.  
9.         return v + Vector3.Cross(w, lever);
10.     }
11.  
12.     public static void AddForceAtPosition(this ArticulationBody ab, Vector3 force, Vector3 worldPosition)
13.     {
14.         var lever = worldPosition - ab.centerOfMass;
15.         ab.AddTorque(Vector3.Cross(lever, force));
16.         ab.AddForce(force);
17.     }
18. }

 

Seems fine:

https://github.com/NVIDIAGameWorks/.../PhysXExtensions/src/ExtRigidBodyExt.cpp#L397

 

Hi.

I will have to test it again. Quick note: The colliders were children GameObjects of ArticulationBody GOs, which doesn't seem to be case in your scenario. Will report ASAP.

 

Ok, never used this. As I see it, it also sends Unity project, right? I shall create separate project so you can get clean scenes/scenarios . Should speed things up for you and me .

 

If anyone needs to set axis in ArticulationBody

Code (CSharp):

1. UnityArticulation.anchorRotation = Quaternion.FromToRotation(axis, Vector3.right);
2.  

 

Unity crashes with 1 root articulation and 176 articulated prismatic joints. I could file a official bug but I think its no need to here.

Ok exact number is 65 articulations (1 parent, 64 child). I don't know internals, but it's maybe cause max children is 64 = 2^6

 

No, it still crashes :I.

 

Articulations are something I was waiting for.

btw. is this formula F = (currentPosition - target) * stiffness - (currentVelocity - targetVelocity) * damping
The same for linear movement and angular movement ?

I'm a little concerned about the fact that articulated body will be something different than Rigidbody. What will be the interaction, will we be able to connect normal joints to Articulated body ?
Since unity moved totally into c# maybe it is time to reconsider some interfaces instead of solid classes.

Secondly will disabling and enabling Articulation reset the initial settings like with normal joints ?

 

Isn't it possible to relax this limit? I know that upping it may impose bigger memory hit and CPU hit, but it's just non-sense limit tbh. Isn't it possible to make it modifiable in Project Setting->Physics menu?

Anyway - I think I figured out that there might be no need for 64 links at the moment.

 

Request, not a bug:

If possible, getting a current position (not just target position) of articualted joint would be nice. I am doing a workaround by tracking local transform rotation .

 

Oh god yes. I need this for rotation. Tracking the current rotation away from the starting rotation (within a configurable joint) is, for whatever reason, a nightmare.

 

Here, I did workaround:

Code (CSharp):

1.  
2.     public class RotationTracker : MonoBehaviour {
3.  
4.         public Vector3 LocalRotationAxis;
5.  
6.         Vector3 _ortho;
7.  
8.         public float Rotation {
9.             get {
10.  
11.                 Vector3 currOrtho = transform.localRotation * _ortho;
12.                 return  Vector3.SignedAngle(_ortho, currOrtho, LocalRotationAxis);
13.             }
14.         }
15.  
16.         private void Awake() {
17.  
18.             _ortho = transform.localRotation * new Vector3(LocalRotationAxis.y, LocalRotationAxis.z, LocalRotationAxis.x);
19.         }
20.     }

Still gotta calculate LocalRotationAxis from ArticulationBody somehow

 

Thanks for exposing this feature, looks promising!

I made a few tests, most of it just worked. Here are a few things I think are missing, broken or could be improved:

• It's not possible to set velocity and angular velocity of a body.
• A way to connect articulations to rigidbodies would be really useful.
  
• Setting target velocity doesn't seem to work in my tests. I tried it with a revolute joint, but there was just no force applied when I set a target speed. Target positions worked.
  
• Support for the other friction types would be nice to have.
  
• Better gizmos would be nice (currently it's a bit confusing with the overlapping transform handles).
  
• Immovable checkbox doesn't update the body if the value is changed in play mode

I also had a few crashes in the cases mentioned in this thread (revolute joint with default settings, adding too many articulations in a hierarchy).

 

Max 16 articulation bodies can exist for one articulation (which can be limiting)

Agree about connecting articulations to rigidbodies would be quite super.

Other friction types would be amazing addition too for more accurate contact simulation (e.g. grabbing).

 

Glad to hear 360° issue was fixed, it was important fix .

 

I'd like clarification on this. According to this https://gameworksdocs.nvidia.com/PhysX/4.1/documentation/physxapi/files/classPxArticulation.html articulations can have up to 64 links, what does the 16 number refer to?

 

My bad, I think it's 64 , not 16.

 

is it possible to have FreezeRotation Contraints like those in RidgidBody? I am trying to port my Reinforcement Learning environments and I use FreezeRotation for the 2D agents such as hopper and walker

 

Thanks for adding this feature, just thought I’d post some issues I’m having. I’m using Unity 2020.1.0a20.2541.

If I use a revolute joint I can safely rotate it to any angle. When I use a spherical joint it seems to explode when I move any axis past 180 degrees. I can work around this by just creating three revolute joints instead of the spherical joint (I thought a spherical was already just three revolute joints).

I’d also like to confirm if the 64 joint limit is for a joint type or per axis e.g. can I have 64 spherical joints or only 21 (xyz rotation, the same as three revolute joints)?

I get an error when the mass is set to zero. Is it possible to allow for zero mass joints e.g. similar to the FreezeRotation constraints comment by sohojoe: I want to set the root object to immovable, then add an x and y axis prismatic joint and also a z axis revolute joint. This would constrain it to a 2d axis. For this example I’d like the three joints to have no drives (zero stiffness), zero mass (can set to nearly zero for now) and they don’t require collision shapes. The other articulated bodies would then use the standard mass and a collision shapes.

I’m hoping that inverse dynamics and circular joints will be supported in the future.

 

Very low masses are going to be unstable with just about any solver. I don't know if you can specify joint inertia's that do not propagate to other bodies, but a common technique for freezing DOF is setting the inverse inertia to 0 or the inertia to infinity.

 

You may use Box2D if you really wish to use 2D agents.

But thats messy, better approach might be just correct the agent rotations with PID and ApplyTorque/ApplyForce.

 

I'm having issues trying to reset my agent using ArticulationBody.TeleportRoot using Unity 2020.1.0a20.2541.
The position does indeed get set, but the velocity increases and causes issues like this: https://powback.com/u/2946b6fc1bde724314b2e5e15c4b5528.mp4

While using chained bodies the effect is amplified. It causes the body to almost immediately go out of bounds and the position becomes an NaN value. This sometimes cause a Unity crash.

A reset function would be very helpful to prevent issues like this.

Edit:
I should note that the ArticulationBodies work as expected if I'm not teleporting. Resetting is in my case essential due to the nature of the environment. https://powback.com/u/a867d9d89a501b2f243cfccd0ca0b758.png

 

Thanks @yant, normally when I try other Featherstone implementations it’s one link per DOF so unticking the immovable option on the root body would create six links (rotation and position). 64 articulated body components should be more than enough.

I found that the zero mass error was because I had a collider attached, it works fine after I removed it. I also forgot that I can set an anchor rotation so it’s unlikely that I’d ever go past 180 degrees from the rest rotation.

I tried a basic hopper with the rotation frozen (could have allowed just the z axis but then it would have fallen over). I’m quite impressed with the new Featherstone solver. When I tried the same thing with standard PhysX rigid bodies the piston would never stay linear on contact, it never had enough force to jump, it didn’t have enough ground friction to move and different masses would break the system (wanted a large base mass with light legs). Thanks again for adding this amazing feature, I’m looking forward to using it in the future.

 

Amazing ) How did u control the hopper?

 

Thanks, I just used a control pad to test it. The x axis controlled the leg rotation and the y axis controlled the leg length / piston. Because the base rotation is frozen it won’t fall over (might test it with a rigged character / walking animation). I might also unfreeze the base rotation and try a basic control system like in the old “3D One-Leg Hopper” paper.

 

+1 for Inverse Dynamics!

Being able to set joint torques based on observations will be hugely useful for tracked hand systems (where a Leap, Oculus, or Microsoft Hans Tracker is driving an articulation hand in the scene).

Articulations get us most of the way to those beautifully stable interactions that are seen in MuJoCo simulations like this one:

Reinforcement learning, physics interactions, and robotics will all benefit (not to mention, it will make it easier/possible for artists to control physical systems as well).

Thank you!

 

@yant - could you let me know either way if you plan to support FreezeRotation Constraints as I need the same underlying physics between 2d and 3d agents. Thanks!!

 

Ran a small experiment with the Articulation Bodies to test their stability vs rigid bodies; it's much more stable as long as certain rules are followed.

The points where both hands disappear are tracking glitches caused by a weak USB connection Nothing to do with the Physics...


1. Crashes
- I am largely able to avoid crashes by avoiding creating/destroying Articulation Bodies multiple times, and by catching exploding bodies with judicious "TeleportRoot" and "SetImmobile" calls (whenever the hand moves further than 1m away from its target position).

2. Explosions/Instability
- Spherical Joints are cursed, even when you set large limits on any combination of axes. I had to remove the spherical joints underneath the top four fingers to keep the explosions per minute to a plausible level.
- Explosions can be mitigated by turning up the Joint Friction and Damping.

3. Joint Limit Inversions
- During explosions, joints can frequently reach an inverted state where they are somehow stuck on the wrong side of their joint limits. I had to disable limits entirely in the video.
- It's possible this is because the joint lower/upper limits are set to negative and positive numbers (though I was not able to find any combination of two positive numbers that wasn't degenerate, so I suspect this is the intended setting).



In order to get the hand to move with a reasonable level of control, I use a three-step process:
1. Counter gravity on the root and each child, simply:

Code (CSharp):

1. aBody.AddForce(-Physics.gravity * aBody.mass); // Repeat for children

2. Counter the body's existing velocity. Just take the finite difference aBody.centerOfMass between FixedUpdates to get the velocity, and then apply a reverse force to the root and each child according to "force = (velocity * mass) / deltaTime". This is approximate, but largely effective... Ideally there would be a function to cancel out an articulationBody's existing inertia/velocities.... Perhaps Damping?

3. Apply a new velocity using the same formula so that your system arrives at the target destination in one deltaTime (though it rarely ever works out this way...)

4. Apply a Torque based on the inertia and desired angular velocity.

 

Thank you for your interest in us being intersted in physics too!

 

Very beautiful, looks quite functional!

 

I was pointed to this post by a colleague. Really great-looking results!

Improvements to the spherical joints are incoming. Unfortunately, the Featherstone algorithm doesn't naturally support spherical joints but we are working on a slightly different way of expressing the problem that early experiments suggest should be a lot more stable and also far more flexible.

There is link and joint damping supported in PhysX. There are also maxLinear/Angular velocity and maxJointVelocity terms, which can be used to limit velocity and ensure that sources of instabilities are controlled. Featherstone articulations are stable if simulated at small time-steps but they can be challenging at larger time-steps. The internal accelerations/forces acting on the articulations are proportional to velocity^2 and this can become unstable when integrated at larger time-steps. This is where damping/clamping comes in to help you. Limiting joint velocities to something like ~20 rads/s and keeping angular velocities under ~30 with some reasonable damping in there tends to work very well with 60hz simulation.

PhysX provides access to joint torques to be able to apply forces/torques in joint space. There is also access directly to joint angles, joint velocities and joint accelerations. This is also how users can directly set the velocities/poses of articulation links - these buffers can be read from and written to. We also provide joint drives that allow you to set target joint angles or joint velocities that the solver will drive towards, controlled by spring stiffness/damping and maxForce parameters. If these are exposed in unity, you could potentially use these as a replacement to your solutions above.

The simplest way to make the root link follow a world-space trajectory is to constrain the root link to a kinematic actor and move the kinematic actor. You need to be careful to allow the joint to fail gracefully if it is not able to be satisfied e.g. if you try to push it through the table. To do this, you need to use a soft joint (free motion with a drive component on each axis) and limit the forces that the solver can apply using the maxForce term on the drive. The drive can be incredibly stiff with a reasonable force limit and they should be stable.

 

Hi there. We are simulating robots with Unity (articulations), and around 400hz simulation tends to work quite well (and it is realtime still).

I know that is not applicable to games with lots of bodies (it gets bogged down quite quickly).

 

Nice! Thanks for heads up; so this means we will be able to ramp up articulation quality only when needed (aka. when robot is grabbing something)?

And one question and request: what parameters should I tune to get precise grabbing/gripping, and how hard is it to get coulomb friction model implemented for articulations?

 

Cough...GPU acceleration...Cough