[core][math] Add Lie-group utilities, SimulationContext & multi-controller architecture, SO(3) helpers, and simulation engine refactor

.gitignore

@@ -3,6 +3,7 @@ output/
 tmp/
 
 # --------------------------------------
+.pytest_cache
 
 # Byte-compiled / optimized / DLL files
 __pycache__/

examples/controllers/__init__.py

@@ -1 +1,2 @@
+from .dummy_high_lvl import *
 from .oscillator import *

examples/controllers/dummy_high_lvl.py

@@ -0,0 +1,13 @@
+__all__ = ["DummyOscillatorHL"]
+
+import numpy as np
+from ssl_simulator import Controller
+
+class DummyOscillatorHL(Controller):
+    def __init__(self, context, oscillator_key):
+        super().__init__(context)
+
+        self.oscillator_key = oscillator_key
+
+    def compute_control(self, time, dt):
+        self.context.call_interface(self.oscillator_key, "_set_osc_omega", time*np.pi/10)

examples/controllers/oscillator.py

@@ -8,34 +8,41 @@
 from ssl_simulator import Controller
 
 class Oscillator(Controller):
-    def __init__(self, A, omega, speed):
-
+    def __init__(self, context, A, omega, speed):
+        super().__init__(context)
         # Controller settings
         self.A = A
         self.omega = omega
         self.speed = speed
 
-        # ---------------------------
+        # Controller variables
+        self.gamma = None
+
+        # ---------------------------        
         # Controller output variables
         self.control_vars = {
             "u": None,
         }
 
         # Controller variables to be tracked by logger
         self.tracked_vars = {
-            "A_gamma": self.A,
-            "omega_gamma": self.omega,
+            "A": self.A,
+            "omega": self.omega,
             "speed": self.speed,
-            "gamma": None,
+            "gamma": self.gamma,
         }
 
-        # Controller data
-        self.init_data()
+        # Controller interface for other controller to interact with it
+        self.register_interface(self._set_osc_omega)
 
-    def compute_control(self, time, state):
+    def _set_osc_omega(self, omega):
+        self.omega = omega
+
+    def compute_control(self, time, dt):
         """
         Follow y = gamma(t) = A * sin(w t) at constant speed ||v|| = s
         """
+        state = self.context.get_robot_state()
         N = state["p"].shape[0]
 
         if (self.A * self.omega > self.speed).any():
@@ -45,10 +52,9 @@ def compute_control(self, time, state):
         gamma_dot = self.A*self.omega * np.cos(self.omega * time) 
         x_dot = np.sqrt(self.speed**2 - gamma_dot**2)
 
-        self.tracked_vars["gamma"] = gamma
+        self.gamma = gamma
+        self.tracked_vars["gamma"] = self.gamma
 
         self.control_vars["u"] = np.zeros((N,2))
-        self.control_vars["u"][:,0] = x_dot * np.ones(N)
-        self.control_vars["u"][:,1] = gamma_dot * np.ones(N)
-
-        return self.control_vars
+        self.control_vars["u"][:,0] = x_dot
+        self.control_vars["u"][:,1] = gamma_dot