# Inverse Dynamics for Articulation Bodies

Discussion in 'Physics Previews' started by JuozasK, Nov 16, 2021.

### Unity Technologies

Joined:
Mar 23, 2017
Posts:
84
Hey everyone!

We have exciting news to share! The first iteration of Inverse Dynamics is ready for experimentation! It should allow for more control over your simulations in a strictly force centered environment.

The feature consists of 4 main getters:

• ArticulationBody.GetDriveForces(List<float> forces) and the related property getter ArticulationBody.driveForce

• ArticulationBody.GetJointGravityForces(List<float> forces)

• ArticulationBody.GetJointCoriolisCentrifugalForces(List<float> forces)

• ArticulationBody.GetJointForcesForAcceleration(ArticulationReducedSpace acceleration)

GetDriveForces and the associated property driveForce, will return the forces applied by the Articulation Drives. It can tell you how hard each drive is trying to reach the intended target. In a simple example, you could calculate the weight of objects, by how much force they exert on the scale.

GetJointGravityForces fills the supplied list of floats with the forces required to counteract gravity for each link in the chain. This tells you how much weight each link is experiencing due to gravity.

GetJointCoriolisCentrifugalForces fills the supplied list of floats with the forces required to counteract coriolis and centrifugal effects. If applied to the joints, the effects of those forces will be negated.

Otherwise, it tells you how much force is generated due to the coriolis and centrifugal forces.

GetJointForcesForAcceleration will return the forces required to reach the provided acceleration for the ArticulationBody. Applying this force to the joint will propel the body to the desired acceleration.

All of these demos can be found here: https://github.com/JuozasK/unity-inverse-dynamics-demo

Inverse Dynamics is available in 2022.1.0a16 and up

M_MG_S, makaka-org, NotaNaN and 12 others like this.
2. ### j_ho_74

Joined:
Aug 13, 2014
Posts:
24
Hi
this is great. Will definitly try it out.
Any plans on backporting it to 2021.2?

Cheers Joerg

### Unity Technologies

Joined:
Jul 24, 2013
Posts:
597
As it stands right now, it's unlikely to be approved for back-porting, unfortunately. Complex feature-work. Anthony.

4. ### AlanMattano

Joined:
Aug 22, 2013
Posts:
1,502
Absolutlly super usfull !!

5. ### RobotFlow

Joined:
Aug 11, 2020
Posts:
26
Hi, is there any plan to support joint friction?

### Unity Technologies

Joined:
Jul 24, 2013
Posts:
597
Could you elaborate a bit more on this? Isn't joint friction supported right now?

7. ### RobotFlow

Joined:
Aug 11, 2020
Posts:
26
Sorry for the confusion, I meant in the contact modeling context, a typical requirement is to do system parameter identification, which need to identify contact dampling, stiffness and friction. Friction in dynamics is very different from in static.

Here is a recent paper which compares the contact modeling between Drake, Bullet and mujoco. I could be wrong, but I think Unity has a similar way as Bullet's to model the contact, because they both adopt PGS solver for contact/collision, which is what I see in Unity's physics setting.

Pybullet has a useful API according to its manual: getDynamicsInfo. It returns mass, lateral_friction, rolling friction, spinning friction, contact damping/stiffness, inertia, etc.

I wonder if Unity can also expose the friction-related API if their mechanism to model the contact is similar. And it is relevant for dynamics modeling.

Thanks!

### Unity Technologies

Joined:
Jul 24, 2013
Posts:
597
Thanks for the context, I'll check on the linked paper shortly. With our latest robotics work we recommend to stick to TGS instead of PGS. Also, does it relate to the topic of the thread?

9. ### razzraziel

Joined:
Sep 13, 2018
Posts:
396
I was smiling until saw that 2022.1.0a16

Sab_Rango likes this.
10. ### RobotFlow

Joined:
Aug 11, 2020
Posts:
26
Hi, actually yes, it is relevant to inverse dynamics.

A typical dynamics equation is:
M(q)*acc + C(q, vel)* vel + G(q) = F

In current API's context:
F: ArticulationBody.GetDriveForces
G(q): ArticulationBody.GetJointGravityForces
C(q, vel) * vel: ArticulationBody.GetJointCoriolisCentrifugalForces
M(q): ArticulationBody.GetJointForcesForAcceleration

However, the external force can be caused by many factors, dynamic joint friction is an example.

In the real world, the applied force should overcome many resistant force, so that it can control the movement as described in the Left hand of equation.

Thus it should be something like M(q)*acc + C(q, vel) * vel + G(q) + F_res = F
Joint friction is a major part of F_res.

Normally, the friction term can be modeled with respect to velocity:
F_fri = F_c*sign(vel) + F_v * vel + asys

F_c is the coulomb's coefficient
F_v is the viscosity coefficient (internal friction)
asys is a term for symmetry

But as you may find in the paper I mentioned earlier, in simulator, they usually have some adaptation to suit the virtual world.

My point is: I think Unity is a platform of great potential, and we have invested so much on this platform for our research or other purpose. Thus I hope it can get better and better.

Last edited: Dec 1, 2021

### Unity Technologies

Joined:
Mar 23, 2017
Posts:
84
^ That's a great overview!

Actually, PhysX does not include friction in the returned forces for Inverse Dynamics. I would assume you would need to include the calculations yourself, based on the friction/damping properties of your Articulation chain.

NotaNaN, drmgmt and AlanMattano like this.
12. ### Sab_Rango

Joined:
Aug 30, 2019
Posts:
121
Nice tech, I am keep learning "Quadratic Programming" and "Linear Complementary Problem" of the physics engine's contact and constraint solvers.
Once I make it I hope am gonna create that physics based character locomotion system.

So far "Toribash" did a great job regarding physics based animation like in the video.

I am pretty sure one day manual artist character animation system will be totally automated by several algorithms!

Last edited: Dec 2, 2021
Ivan_br likes this.
13. ### RobotFlow

Joined:
Aug 11, 2020
Posts:
26
Thanks, got it

14. ### RobotFlow

Joined:
Aug 11, 2020
Posts:
26
Hi,

Another feedback here.

It seems the return values of GetDriveForces are in generalized force form. I wonder how it can be converted to torque, as in common robot arm, like UR5, franka emika panda?

Most control equations in robotics are built upon torques. I think it is a serious issue.

Thanks!

P.S. We actually conduct a parallel experiment on both sim and real environments with Franka. The joint torques from real franka and the joint drive force from unity have similar trend, but very different scales.

### Unity Technologies

Joined:
Mar 23, 2017
Posts:
84
I'm not sure how that can be. The forces returned should be in reduced space. If the joint is a revolute one, then the force returned should equate to torque? Or am I misunderstanding something?

16. ### RobotFlow

Joined:
Aug 11, 2020
Posts:
26
Ok, so the unit is Nm? I will check the value of the physics properties for each joint and see if something wrong there.

Joined:
Mar 23, 2017
Posts:
84
18. ### wsinw

Joined:
Apr 23, 2017
Posts:
14
How has no one mentioned the cute little humanoid test-dummies in your examples? I love them!

19. ### TimHogklint

Joined:
Nov 24, 2017
Posts:
46
Hello - Im currently stabilizing a walking biped.

Code (CSharp):
1.         jntControl.root.maxAngularVelocity = 0f;
2.
3.         var uprightAngle = Vector3.Angle(transform.up, Vector3.up) / 180;
4.         var balancePercent = uprightTorqueFunction.Evaluate(uprightAngle);
5.         var uprightTorqueVal = balancePercent * zxTorqueForces;
6.
7.         var rot = Quaternion.FromToRotation(transform.up, Vector3.up);
8.         jntControl.root.angularVelocity = new Vector3(rot.x, rot.y, rot.z) * uprightTorqueVal;
9.
10.         float dot = Vector3.Dot((moveTarget.position - jntControl.root.transform.position).normalized, jntControl.root.velocity);
11.         jntControl.root.velocity = Vector3.ClampMagnitude(dot * jntControl.root.velocity, targetVelocity);
12.
13.         if (pelvisSpeed < targetVelocity)
14.             jntControl.root.AddForce((moveTarget.position - jntControl.root.transform.position).normalized * Input.GetAxis("Vertical") * 250f);
This is very naive and produce a very artificial look - even if I unlock y-axis gravity effects.

https://imgur.com/FZP3Wst
Im looking to reduce the effects of gravity acting on the rig while moving towards a target.
Looking through the examples of inverse dynamics github Im quite stumped on how to
achieve this. I basically want to negate the additional forces lifting a leg creates on the rig
but yet this have it act physically - I hope that makes sense.

20. ### TimHogklint

Joined:
Nov 24, 2017
Posts:
46

Edit

In short - Its been explained to me that its possible to create a "virtual force" - for instance you have foot planted; In that case it should be "connected" to the ground.

The effect is similar to foot being held to the ground-point but can rotate freely ?

As described in the paper https://www.cs.ubc.ca/~van/papers/2010-TOG-gbwc/index.html
under : 3.4 Gravity Compensation

Last edited: May 24, 2022
21. ### makaka-org

Joined:
Dec 1, 2013
Posts:
1,093
If you're searching to understand what things mean.
Just the Learning: visualization of (Coriolis + Centrifugal = Total) Force.

This is not a joint, but shows the forces which Unity uses in GetJointCoriolisCentrifugalForces() method, if I understand correctly.

### Unity Technologies

Joined:
Mar 23, 2017
Posts:
84
Hey! So the implementation for this should be rather simple. You can use ArticulationBody.GetJointGravityForces(List<float> forces) to get the required forces to negate gravity for each joint in reduced space filled to the provided "forces" list. After calling that, if you just apply those same forces on the whole chain with ArticulationBody.SetJointForces, then it will effectively negate the effect of gravity on your whole biped. In reality I'm guessing what you want is some smaller value of that, so if you multiply each gravity force value by 0.1 and then apply it like before, you'll reduce the effect that gravity has on your biped by 10%. You can pretty much go from 0-100% of gravity negation like this.

If you want only some of the bodies to have a reduced gravity effect, you can apply the forces per ArticulationBody.jointForce, but you will need to construct an ArticulationReducedSpace object first and then plug that into the property.

As for which forces to get from the original "forces" list you supplied in the GetJointGravityForces method, you can index the list like this:
Code (CSharp):
1. ab.GetJointGravityForces(forces)
2. ab.GetDofStartIndices(indices)
3. forces[indices[targetAB.index]]
targetAB.index is the body that you wish to set the forces for.
This will get you the start of your forces for that ArticulationBody joint. If the joint is prismatic or revolute, that will be the only value you want when constructing the ArticulationReducedSpace object, but if it's a spherical joint you will need to take 3 values from that given index start, one for each degree of freedom.

I hope this clears up some of the confusion, let me know if this helps

Last edited: Jun 3, 2022
makaka-org and TimHogklint like this.

### Unity Technologies

Joined:
Mar 23, 2017
Posts:
84
Hey! I'm excited to share a small update on the Inverse Dynamics front! We've exposed another method that allows you to get the forces required to counteract any previously added forces in general space. The new method:
• ArticulationBody.GetJointExternalForces(List<float> forces)

This method will populate the provided list with forces in reduced space that are required to counteract any forces applied previously in general space with methods like ArticulationBody.AddForce or ArticulationBody.AddTorque.

Keep in mind that you should first apply all the forces you would like to counteract and then call the GetJointExternalForces method, since any forces applied afterwards will not get included in the calculation. This is important when considering things like adding force in OnTriggerStay callbacks, but calling the above getter in FixedUpdate and so on.

The External forces getter is available from 2022.2.0a14!

TimHogklint and makaka-org like this.

Joined:
Aug 11, 2020
Posts:
26
25. ### Pingomannen

Joined:
Jan 10, 2022
Posts:
2
I have noticed that ArticulationBody.GetDriveForces() and ArticulationBody.driveForce returns different (much lower and sometimes negative) values when my project has been built compared to in Play mode in the editor.

Has anyone seen this issue or is it probably an issue in my articulations?
I have seen this behaviour in both the beta 2022.1.0b5 and in the official release 2022.1.20f1.

### Unity Technologies

Joined:
Mar 23, 2017
Posts:
84
Hey! Could you send a bug report with a small repro project with this? Sounds like something to be investigated.

27. ### Pingomannen

Joined:
Jan 10, 2022
Posts:
2
I actually got it to work. I had not used ArticulationBody.GetDofStartIndex() to get the correct index in the drive force array. I had gone directly on ArticulationBody.driveForce[0]. And it was that value that differed between running the application in the editor and the built application.

It seems strange that those values would differ, but I am very pleased that I got it to work.

### Unity Technologies

Joined:
Mar 23, 2017
Posts:
84
ArticulationBody.driveForce returns you the `ArticulationReducedSpace` object, which consists of 3 possible DOFs.
ArticulationBody.driveForce[0] just returns you the first degree of freedom of the drive forces that were applied to that specific AB

https://docs.unity3d.com/2022.2/Documentation/ScriptReference/ArticulationBody-driveForce.html

ArticulationBody.GetDriveForces() fills the provided list with the drive forces for all of the ArticulationBodies in the chain. You need to iterate over it yourself and figure out which forces are for which body. This is where the GetDofStartIndices method comes in, to let you know where each body's DOFs start

29. ### drmgmt

Joined:
Feb 19, 2020
Posts:
3
@JuozasK Is it possible to apply a specified torque for the joint between two ridged bodies (in the direction of twist,swing1,swing2) (Similar to the way Drives function) Can this be done using jointForce {set;}? Or is there another method available now or possibly in the near future?

For example an Articulation Joint Type: Spherical -

I tried this but it doesn't seem to work as expected joint.jointForce =new ArticulationReducedSpace(twist,swing1, swing2);

I think there are many developers who are looking for an alternative to the motor PD method with direct control for use with procedural animations, ML Agents etc.

Last edited: Dec 28, 2022

### Unity Technologies

Joined:
Mar 23, 2017
Posts:
84
To my knowledge, yes this should work with the jointForce setter. It will apply the force you provided on the joint in the next frame.

Could you try with a simpler example and work up, try a revolute joint and set the force to some value, see if it works? Spherical joints are generally pretty unstable and it's coming from the SDK level.

31. ### drmgmt

Joined:
Feb 19, 2020
Posts:
3

I tried a simpler example both with revolute and Spherical joints and I can confirm they both appear to work as expected.

It appears the issues I was experiencing might have been a result of high force force numbers.

Thank you again!

32. ### TimHogklint

Joined:
Nov 24, 2017
Posts:
46

'ArticulationBody' does not contain a definition for 'GetJointExternalForces' and no accessible extension method 'GetJointExternalForces' accepting a first argument of type 'ArticulationBody' could be found (are you missing a using directive or an assembly reference?)

Weird coming back to this and it not working :/

Last edited: Feb 17, 2023
33. ### andyjonesss

Joined:
Oct 7, 2021
Posts:
9
I'm trying to add a predefined torque to a revolute joint. I've tried a few different methods in a very basic test scenario, however haven't had any success. It seems I don't quite understand something, or it's just not possible?

I've seen in a few places that the following is true ---> Force = (targetAngle - currentAngle) * stiffness.
On that basis, it made sense to me that the following would be true ---> targetAngle = (Force / stiffness) + currentAngle.
If this is the case then the target angle can be used to apply the specific amount of force we want, however the resulting drive force was never equal to the requested drive force.

{
// Force = (targetAngle - currentAngle) * stiffness.
// Force / stiffness = targetAngle - currentAngle;
// (Force / stiffness) + currentAngle = targetAngle;

float stiffness = articulationBody.xDrive.stiffness;
float currentAngle;
float targetAngle;

// get the current angle

// calculate the target angle based on the desired force
targetAngle = (force / stiffness) + currentAngle;

// Set up the joint drive
ArticulationDrive drive = articulationBody.xDrive;
drive.target = targetAngle;

// Apply the drive to the articulation body
articulationBody.xDrive = drive;

// debug the applied force
float driveForce = articulationBody.driveForce[0];
Debug.Log(driveForce);
}

I also tried setting the force directly, as shown below. This again did not work, just seemed to want to reach a target angle as I changed the force value.

ArticulationReducedSpace forceRS = new ArticulationReducedSpace(force);
articulationBody.jointForce = forceRS;

If anyone can help it'd be much appreciated, I cant get my head around what's going on. It doesn't make sense to me that a force can be a coordinate, as is the case with articulationBody.driveForce and articulationBody.jointForce... what is it I'm missing here?