InverseDynamics behaviors do not coincide with Bullet.

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:

Code (CSharp):

1. using System.Collections;
2. using System.Collections.Generic;
3. using UnityEngine;
4. using RFUniverse.Controllers;
5. using Robotflow.RFUniverse.SideChannels;
6. using System.Linq;
7. public class NewBehaviourScript : MonoBehaviour
8. {
9.     public ArticulationBody body1;
10.     public ArticulationBody body2;
11.     int steps;
12.     float[,] q_pos_desired;
13.     float[,] q_vel_desired;
14.     float[,] q_acc_desired;
15.     private void Awake()
16.     {
17.         float delta_t = 0.001f;
18.         float start = 0;
19.         float end = 1;
20.         Time.fixedDeltaTime = delta_t;
21.         steps = (int)((end - start) / delta_t);
22.         float[] t = new float[steps];
23.         q_pos_desired = new float[2, steps];
24.         q_vel_desired = new float[2, steps];
25.         q_acc_desired = new float[2, steps];
26.         for (int s = 0; s < steps; s++)
27.         {
28.             t[s] = start + s * delta_t;
29.             q_pos_desired[0, s] = 1f / (2f * Mathf.PI) * Mathf.Sin(2f * Mathf.PI * t[s]) - t[s];
30.             q_pos_desired[1, s] = -1f / (2f * Mathf.PI) * (Mathf.Cos(2f * Mathf.PI * t[s]) - 1f);
31.             q_vel_desired[0, s] = Mathf.Cos(2f * Mathf.PI * t[s]) - 1f;
32.             q_vel_desired[1, s] = Mathf.Sin(2f * Mathf.PI * t[s]);
33.             q_acc_desired[0, s] = -2f * Mathf.PI * Mathf.Sin(2f * Mathf.PI * t[s]);
34.             q_acc_desired[1, s] = 2f * Mathf.PI * Mathf.Cos(2f * Mathf.PI * t[s]);
35.         }
36.     }
37.     int i = 0;
38.     private void FixedUpdate()
39.     {
40.         if (i >= steps)
41.         {
42.             body1.jointForce = new ArticulationReducedSpace(0);
43.             body1.jointVelocity = new ArticulationReducedSpace(0);
44.             body2.jointForce = new ArticulationReducedSpace(0);
45.             body1.jointVelocity = new ArticulationReducedSpace(0);
46.             return;
47.         }
48.         Debug.Log(q_acc_desired[0, i]);
49.         List<float> gravityForce = new List<float>();
50.         List<float> coriolisCentrifugal = new List<float>();
51.         //List<float> driveForces = new List<float>();
52.         body1.GetJointGravityForces(gravityForce);
53.         body1.GetJointCoriolisCentrifugalForces(coriolisCentrifugal);
54.         //body1.GetDriveForces(driveForces);
55.         float accelerationForce1 = body1.GetJointForcesForAcceleration(new ArticulationReducedSpace(q_acc_desired[0, i]))[0];
56.         float force1 = gravityForce[0] + coriolisCentrifugal[0] + accelerationForce1;
57.         force1 = accelerationForce1;
58.         body1.jointForce = new ArticulationReducedSpace(force1);
59.         float accelerationForce2 = body2.GetJointForcesForAcceleration(new ArticulationReducedSpace(q_acc_desired[1, i]))[0];
60.         float force2 = gravityForce[1] + coriolisCentrifugal[1] + accelerationForce2;
61.         force2 = accelerationForce2;
62.         body2.jointForce = new ArticulationReducedSpace(force2);
63.         i++;
64.     }
65. }

Any help would be grateful!

 

Hi, thanks for your reply!

I wonder if there's a suggested practice for force/torque control?
I saw the documentation here (Unity-Robotics-Hub/urdf_appendix.md at main · Unity-Technologies/Unity-Robotics-Hub (github.com)) mentions "Note: Best practices for velocity and torque control will be published in the future."
But it never publishes ever since.

 

I think I figured out the problem partially. Previously there was a strange jitter/shaking on the robot when I tried to do force control, but it becomes much smaller after I removed the scripts added by URDF importer. More specifically, when I removed the inertia tensor and inertia tensor rotation, the shaking behavior becomes much smaller.

 

Hi, now I'm using
float force1 = gravityForce[0] + coriolisCentrifugal[0] + accelerationForce; as the force to apply on the joint, and set jointForce as this force. I'm wondering is this is the correct way to do force control. Also, I observed that
body1.SetJointForces(accelerationForce1); and
body1.jointForce = new ArticulationReducedSpace(force1); have different behaviors. Which one should I use? It will be really helpful if you can provide a sample Force/Torque control project. Thanks!

 

Hi, thanks for your reply! I'm using 2022.2.0b1, also tried 2022.1.3f1