Add Unit tests for OpenPLX integration

Author: FilipAlg

Diff for: Tests/Runtime/AGXUnityTests.asmdef

@@ -16,6 +16,11 @@
     "excludePlatforms": [],
     "allowUnsafeCode": false,
     "overrideReferences": false,
+    "precompiledReferences": [
+        "openplx.dll",
+        "agxDotNet.dll",
+        "agxMathDotNet.dll"
+    ],
     "autoReferenced": false,
     "versionDefines": [],
     "noEngineReferences": false

Diff for: Tests/Runtime/AuxiliaryScripts.meta

@@ -0,0 +1,104 @@
+using AGXUnity;
+using AGXUnity.IO.OpenPLX;
+using AGXUnity.Utils;
+using UnityEngine;
+
+namespace AGXUnityTesting.Runtime.Aux
+{
+
+  class PDController
+  {
+    public double KP { get; set; }
+    public double KD { get; set; }
+    public double Goal { get; set; }
+
+    public PDController( double kp, double kd, double goal )
+    {
+      KP = kp;
+      KD = kd;
+      Goal = goal;
+    }
+
+    public double Observe( double x, double xdot )
+    {
+      double error = Goal - x;
+      return KP * error - KD * xdot;
+    }
+  }
+
+  public class InvertedPendulumController : ScriptComponent
+  {
+    OpenPLXSignals signal = null;
+
+    InputTarget motor_input = null;
+    PDController cart;
+    PDController pole;
+
+
+    // Start is called before the first frame update
+    protected override bool Initialize()
+    {
+      signal = gameObject.GetInitializedComponent<OpenPLXSignals>();
+      motor_input = signal.FindInputTarget( "PendulumScene.motor_input" );
+
+      if ( motor_input == null )
+        Debug.LogWarning( "Could not find motor input" );
+
+      cart = new PDController( 11, 5, 0 );
+      pole = new PDController( 19, 5, 0 );
+
+      Simulation.Instance.StepCallbacks.PreStepForward += Pre;
+
+      hao = signal.FindOutputSource( "PendulumScene.hinge_angle_output" );
+      havo = signal.FindOutputSource( "PendulumScene.hinge_angular_velocity_output" );
+      poso = signal.FindOutputSource( "PendulumScene.cart_position_output" );
+      velo = signal.FindOutputSource( "PendulumScene.cart_velocity_output" );
+
+      return true;
+    }
+
+    OutputSource hao;
+    OutputSource havo;
+    OutputSource poso;
+    OutputSource velo;
+
+    double ha = 0.0f;
+    double hav = 0.0f;
+    double xpos = 0.0f;
+    double xvel = 0.0f;
+    double u_cart = 0.0f;
+    double u_pole = 0.0f;
+    double raw = 0.0f;
+
+    private void Pre()
+    {
+      try {
+        ha = hao.GetCachedValue<double>();
+        hav = havo.GetCachedValue<double>();
+        xpos = poso.GetCachedValue<Vector3>().x;
+        xvel = velo.GetCachedValue<Vector3>().x;
+      }
+      catch {
+        return;
+      }
+
+      u_cart = cart.Observe( xpos, xvel );
+      u_pole = pole.Observe( ha, hav );
+
+      raw = -u_cart - u_pole;
+
+      motor_input.SendSignal( Math.Clamp( raw, -1000, 1000 ) );
+    }
+
+    private void OnGUI()
+    {
+      GUILayout.Label( $"Angle: {ha}" );
+      GUILayout.Label( $"A. Vel: {hav}" );
+      GUILayout.Label( $"Pos: {xpos}" );
+      GUILayout.Label( $"Vel: {xvel}" );
+      GUILayout.Label( $"u_cart: {u_cart}" );
+      GUILayout.Label( $"u_pole: {u_pole}" );
+      GUILayout.Label( $"Raw: {raw}" );
+    }
+  }
+}

Diff for: Tests/Runtime/AuxiliaryScripts/InvertedPendulumController.cs

@@ -0,0 +1,56 @@
+using AGXUnity;
+using AGXUnity.IO.OpenPLX;
+using AGXUnity.Utils;
+using UnityEngine;
+
+namespace AGXUnityTesting.Runtime.Aux
+{
+  public class SimplerInvertedPendulum : ScriptComponent
+  {
+    OpenPLXSignals signal = null;
+
+    InputTarget motor_input = null;
+    PDController pole;
+
+    // Start is called before the first frame update
+    protected override bool Initialize()
+    {
+      signal = gameObject.GetInitializedComponent<OpenPLXSignals>();
+      motor_input = signal.FindInputTarget( "PendulumScene.motor_input" );
+
+      if ( motor_input == null )
+        Debug.LogWarning( "Could not find motor input" );
+
+      pole = new PDController( 5, 0.2f, 0 );
+
+      Simulation.Instance.StepCallbacks.SimulationPre += Pre;
+
+      hao = signal.FindOutputSource( "PendulumScene.hinge_angle_output" );
+      havo = signal.FindOutputSource( "PendulumScene.hinge_angular_velocity_output" );
+
+      return true;
+    }
+
+    OutputSource hao;
+    OutputSource havo;
+
+    double ha = 0.0f;
+    double hav = 0.0f;
+    double u_pole = 0.0f;
+
+    private void Pre()
+    {
+      try {
+        ha = hao.GetCachedValue<double>();
+        hav = havo.GetCachedValue<double>();
+      }
+      catch {
+        return;
+      }
+
+      u_pole = pole.Observe( ha, hav );
+
+      motor_input.SendSignal( u_pole );
+    }
+  }
+}

Diff for: Tests/Runtime/AuxiliaryScripts/InvertedPendulumController.cs.meta

@@ -0,0 +1,91 @@
+using NUnit.Framework;
+
+using Math = openplx.Math;
+
+namespace AGXUnityTesting.Runtime
+{
+  public class OperatorTests
+  {
+    [Test]
+    public void Vec3PlusTest()
+    {
+      var v1 = Math.Vec3.from_xyz(1, 2, 3);
+      var v2 = Math.Vec3.from_xyz(2, 3, 4);
+
+      var v3 = v1 + v2;
+
+      Assert.AreEqual( 3, v3.x() );
+      Assert.AreEqual( 5, v3.y() );
+      Assert.AreEqual( 7, v3.z() );
+    }
+
+    [Test]
+    public void Vec3DotTest()
+    {
+      var v1 = Math.Vec3.from_xyz(1, 2, 3);
+      var v2 = Math.Vec3.from_xyz(2, 3, 4);
+
+      var dot = v1 * v2;
+
+      Assert.AreEqual( 20, dot );
+    }
+
+    [Test]
+    public void Vec3MulTest()
+    {
+      var v1 = Math.Vec3.from_xyz(1, 2, 3);
+
+      var v2 = v1 * 3;
+
+      Assert.AreEqual( 3, v2.x() );
+      Assert.AreEqual( 6, v2.y() );
+      Assert.AreEqual( 9, v2.z() );
+
+      var v3 = 3 * v1;
+
+      Assert.AreEqual( 3, v3.x() );
+      Assert.AreEqual( 6, v3.y() );
+      Assert.AreEqual( 9, v3.z() );
+    }
+
+    [Test]
+    public void Vec3MinusTest()
+    {
+      var v1 = Math.Vec3.from_xyz(1, 2, 3);
+      var v2 = Math.Vec3.from_xyz(2, 3, 4);
+
+      var v3 = v1 - v2;
+
+      Assert.AreEqual( -1, v3.x() );
+      Assert.AreEqual( -1, v3.y() );
+      Assert.AreEqual( -1, v3.z() );
+    }
+
+    [Test]
+    public void Vec3DivTest()
+    {
+      var v1 = Math.Vec3.from_xyz(3, 6, 9);
+
+      var v2 = v1 / 3;
+
+      Assert.AreEqual( 1, v2.x() );
+      Assert.AreEqual( 2, v2.y() );
+      Assert.AreEqual( 3, v2.z() );
+    }
+
+    public void QuatTest()
+    {
+      var q1 = Math.Quat.from_to(Math.Vec3.X_AXIS(), Math.Vec3.Z_AXIS());
+      var q2 = Math.Quat.from_to(Math.Vec3.Z_AXIS(), Math.Vec3.Y_AXIS());
+
+      var q3 = q1 * q2;
+
+      var rotated = q3.rotate(Math.Vec3.X_AXIS());
+      var tar = Math.Vec3.Y_AXIS();
+
+      Assert.AreEqual( tar.x(), rotated.x() );
+      Assert.AreEqual( tar.y(), rotated.y() );
+      Assert.AreEqual( tar.z(), rotated.z() );
+    }
+  }
+}