Search Unity

  1. Unity 6 Preview is now available. To find out what's new, have a look at our Unity 6 Preview blog post.
    Dismiss Notice
  2. Unity is excited to announce that we will be collaborating with TheXPlace for a summer game jam from June 13 - June 19. Learn more.
    Dismiss Notice

InverseDynamics behaviors do not coincide with Bullet.

Discussion in 'Physics' started by RobotFlow, Aug 31, 2022.

  1. RobotFlow

    RobotFlow

    Joined:
    Aug 11, 2020
    Posts:
    26
    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!
     
  2. JuozasK

    JuozasK

    Unity Technologies

    Joined:
    Mar 23, 2017
    Posts:
    84
    Hey!

    Thanks for taking an interest in Inverse Dynamics!

    I can't speak to how the pyBullet example functions, but I would not automatically expect these two systems to provide exact results. Factors like iteration count, or when does the force get applied, can change how either system reacts to the initial same setup.

    I would suggest reducing the amount of variables and starting from a smaller example, maybe comparing smaller steps, smaller robots and checking where the discrepancy occurs.

    You can also try playing around with Physics Project settings like Solver type or iteration count.
     
  3. RobotFlow

    RobotFlow

    Joined:
    Aug 11, 2020
    Posts:
    26
    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.
     
  4. RobotFlow

    RobotFlow

    Joined:
    Aug 11, 2020
    Posts:
    26
    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.
     
  5. RobotFlow

    RobotFlow

    Joined:
    Aug 11, 2020
    Posts:
    26
    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!
     
  6. JuozasK

    JuozasK

    Unity Technologies

    Joined:
    Mar 23, 2017
    Posts:
    84
    Yeah, sometimes the inertial values that come with the URDF are not correct or they may affect the simulation adversely. It's not unexpected to have them removed in certain cases.

    body1.SetJointForces will apply the forces to the entire chain. So for example your `accelerationForce1` should be a list that has all the forces for all the Degrees of Freedom on the whole chain.
    https://docs.unity3d.com/ScriptReference/ArticulationBody.SetJointForces.html

    While you can call it from any body in the chain, it doesn't really matter which one you do it from. Usually its simplest to call it on root.

    body1.jointForce will set the joint force for the referenced body specifically.

    Also, which Unity version are you using? There was a bug with indexing in earlier versions which might also cause some issues.
     
  7. RobotFlow

    RobotFlow

    Joined:
    Aug 11, 2020
    Posts:
    26
    Hi, thanks for your reply! I'm using 2022.2.0b1, also tried 2022.1.3f1
     
  8. JuozasK

    JuozasK

    Unity Technologies

    Joined:
    Mar 23, 2017
    Posts:
    84
    The fix landed in 2022.1.21f1 of the 2022.1 stream so I would recommend updating to that version.

    It only affects the batch methods like SetJointForces or SetJointPositions etc.
    If you try to set the ArticulationBody.jointForce per each body manually, that should work as expected.