Hi guys,

TL;DR

We try to implement the inverse dynamics example in pybullet with exactly the same urdf model to verify the inverse dynamics API. But the results are very different. **The joint in Unity is not properly controlled.**

In the pybullet example, the code (1) imports the URDF, (2) disables the default motor, and (3) generates desired accelerations, (4) uses inverse dynamics API to calculate the torque, (5) put the torque to joint motor.

In the unity, we do:

(1) with URDF-importer

(2) disable the default motor by setting the joint parameters - stiffness and damping to zero, where force limits are 3e+38 (which is quite like the setting in inverse dynamics demo)

(3) generate the desired acceleration by the same logic

(4) use inverse dynamics API to get the torque

(5) put the torque to ArticulationBody.jointForce.

The code for our implementation is also attached, please instruct us what can we do:

```
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using RFUniverse.Controllers;
using Robotflow.RFUniverse.SideChannels;
using System.Linq;
public class NewBehaviourScript : MonoBehaviour
{
public ArticulationBody body1;
public ArticulationBody body2;
int steps;
float[,] q_pos_desired;
float[,] q_vel_desired;
float[,] q_acc_desired;
private void Awake()
{
float delta_t = 0.001f;
float start = 0;
float end = 1;
Time.fixedDeltaTime = delta_t;
steps = (int)((end - start) / delta_t);
float[] t = new float[steps];
q_pos_desired = new float[2, steps];
q_vel_desired = new float[2, steps];
q_acc_desired = new float[2, steps];
for (int s = 0; s < steps; s++)
{
t[s] = start + s * delta_t;
q_pos_desired[0, s] = 1f / (2f * Mathf.PI) * Mathf.Sin(2f * Mathf.PI * t[s]) - t[s];
q_pos_desired[1, s] = -1f / (2f * Mathf.PI) * (Mathf.Cos(2f * Mathf.PI * t[s]) - 1f);
q_vel_desired[0, s] = Mathf.Cos(2f * Mathf.PI * t[s]) - 1f;
q_vel_desired[1, s] = Mathf.Sin(2f * Mathf.PI * t[s]);
q_acc_desired[0, s] = -2f * Mathf.PI * Mathf.Sin(2f * Mathf.PI * t[s]);
q_acc_desired[1, s] = 2f * Mathf.PI * Mathf.Cos(2f * Mathf.PI * t[s]);
}
}
int i = 0;
private void FixedUpdate()
{
if (i >= steps)
{
body1.jointForce = new ArticulationReducedSpace(0);
body1.jointVelocity = new ArticulationReducedSpace(0);
body2.jointForce = new ArticulationReducedSpace(0);
body1.jointVelocity = new ArticulationReducedSpace(0);
return;
}
Debug.Log(q_acc_desired[0, i]);
List<float> gravityForce = new List<float>();
List<float> coriolisCentrifugal = new List<float>();
//List<float> driveForces = new List<float>();
body1.GetJointGravityForces(gravityForce);
body1.GetJointCoriolisCentrifugalForces(coriolisCentrifugal);
//body1.GetDriveForces(driveForces);
float accelerationForce1 = body1.GetJointForcesForAcceleration(new ArticulationReducedSpace(q_acc_desired[0, i]))[0];
float force1 = gravityForce[0] + coriolisCentrifugal[0] + accelerationForce1;
force1 = accelerationForce1;
body1.jointForce = new ArticulationReducedSpace(force1);
float accelerationForce2 = body2.GetJointForcesForAcceleration(new ArticulationReducedSpace(q_acc_desired[1, i]))[0];
float force2 = gravityForce[1] + coriolisCentrifugal[1] + accelerationForce2;
force2 = accelerationForce2;
body2.jointForce = new ArticulationReducedSpace(force2);
i++;
}
}
```

Any help would be grateful!