Transfered naive ML-CasADi to L4Casadi.naive

.github/workflows/ci.yaml

@@ -16,7 +16,7 @@ jobs:
     - name: Run mypy
       run: |
         pip install mypy
-        mypy . --ignore-missing-imports
+        mypy . --ignore-missing-imports --exclude examples
     - name: Run flake8
       run: |
         pip install flake8
@@ -44,12 +44,12 @@ jobs:
     - name: Install Python
       uses: actions/setup-python@v4
       with:
-        python-version: 3.x
+        python-version: '>=3.9 <3.12'
 
     - name: Install L4CasADi
       run: |
         python -m pip install --upgrade pip
-        pip install torch>=2.0 --index-url https://download.pytorch.org/whl/cpu  # Ensure CPU torch version
+        pip install torch --index-url https://download.pytorch.org/whl/cpu  # Ensure CPU torch version
         pip install -r requirements_build.txt
         pip install . -v --no-build-isolation 
 
@@ -77,13 +77,13 @@ jobs:
           distro: ubuntu_latest
           install: |
             apt-get update
-            apt-get install -y --no-install-recommends python3 python3-pip python-is-python3
+            apt-get install -y --no-install-recommends python3.9 python3-pip python-is-python3
             pip install -U pip
             apt-get install -y build-essential
 
           run: |
             python -m pip install --upgrade pip
-            pip install torch>=2.0 --index-url https://download.pytorch.org/whl/cpu  # Ensure CPU torch version
+            pip install torch --index-url https://download.pytorch.org/whl/cpu  # Ensure CPU torch version
             pip install -r requirements_build.txt
             pip install . -v --no-build-isolation 
             # pip install pytest

README.md

@@ -86,6 +86,17 @@ Further examples:
 
 ---
 
+## Naive L4CasADi
+
+For small models the overhead of context switches between PyTorch and CasADi can be significant even in pure C++.
+Thus, L4CasADi provides a `NaiveL4CasADiModule` which will directly re-create the PyTorch computational graph in C++
+and copies the weights. This, however is limited to a small subset of PyTorch operations - only `MultiLayerPerceptron`
+models and CPU inference are supported.
+
+https://github.com/Tim-Salzmann/l4casadi/blob/59876b190941c8d43286b6eb3d710add6f14a1d2/examples/naive/readme.py#L5-L9
+
+---
+
 ## Batch Dimension
 
 If your PyTorch model expects a batch dimension as first dimension (which most models do) you should pass
@@ -117,9 +128,13 @@ https://github.com/Tim-Salzmann/l4casadi/blob/421de6ef408267eed0fd2519248b2152b6
 
 ---
 ## Real-time L4CasADi
-Real-time L4Casadi (former [ML-CasADi](https://github.com/TUM-AAS/ml-casadi)) is the underlying framework powering
-[Real-time Neural-MPC](https://arxiv.org/pdf/2203.07747). It replaces complex models with local Taylor approximations
-to enable real-time optimization procedures. More information [here](l4casadi/realtime).
+Real-time L4Casadi (former `Approximated` approach in [ML-CasADi](https://github.com/TUM-AAS/ml-casadi)) is the underlying framework powering
+[Real-time Neural-MPC](https://arxiv.org/pdf/2203.07747). It replaces complex models with local Taylor approximations.
+For certain optimization procedures (such as MPC with multiple shooting nodes) this can lead to improved optimization times.
+However, `Real-time L4Casadi`, comes with many restrictions (only Python, no C(++) code generation, ...) and is therefore not
+a one-to-one replacement for `L4Casadi`. Rather it is a complementary framework for certain special use cases.
+
+More information [here](l4casadi/realtime).
 
 https://github.com/Tim-Salzmann/l4casadi/blob/62219f61375af4c4133167f1d8b138d90f678c32/l4casadi/realtime/examples/readme.py#L32-L43
 

examples/naive/readme.py

@@ -0,0 +1,18 @@
+import casadi as cs
+import l4casadi as l4c
+
+
+naive_mlp = l4c.naive.MultiLayerPerceptron(2, 128, 1, 2, 'Tanh')
+l4c_model = l4c.L4CasADi(naive_mlp, model_expects_batch_dim=True)
+
+x_sym = cs.MX.sym('x', 2, 1)
+y_sym = l4c_model(x_sym)
+f = cs.Function('y', [x_sym], [y_sym])
+df = cs.Function('dy', [x_sym], [cs.jacobian(y_sym, x_sym)])
+ddf = cs.Function('ddy', [x_sym], [cs.hessian(y_sym, x_sym)[0]])
+
+x = cs.DM([[0.], [2.]])
+print(l4c_model(x))
+print(f(x))
+print(df(x))
+print(ddf(x))

examples/realtime/mpc_mlp_example.py

@@ -0,0 +1,244 @@
+import casadi as cs
+import numpy as np
+import torch
+import l4casadi as l4c
+from acados_template import AcadosSimSolver, AcadosOcpSolver, AcadosSim, AcadosOcp, AcadosModel
+import time
+import os
+
+os.environ['KMP_DUPLICATE_LIB_OK'] = 'True'
+
+
+class PyTorchModel(torch.nn.Module):
+    def __init__(self):
+        super().__init__()
+
+        self.input_layer = torch.nn.Linear(2, 512)
+
+        hidden_layers = []
+        for i in range(5):
+            hidden_layers.append(torch.nn.Linear(512, 512))
+
+        self.hidden_layer = torch.nn.ModuleList(hidden_layers)
+        self.out_layer = torch.nn.Linear(512, 2)
+
+        # Model is not trained -- setting output to zero
+        with torch.no_grad():
+            self.out_layer.bias.fill_(0.)
+            self.out_layer.weight.fill_(0.)
+
+    def forward(self, x):
+        x = self.input_layer(x)
+        for layer in self.hidden_layer:
+            x = torch.tanh(layer(x))
+        x = self.out_layer(x)
+        return x
+
+
+class DoubleIntegratorWithLearnedDynamics:
+    def __init__(self, learned_dyn):
+        self.learned_dyn = learned_dyn
+
+    def model(self):
+        s = cs.MX.sym('s', 1)
+        s_dot = cs.MX.sym('s_dot', 1)
+        s_dot_dot = cs.MX.sym('s_dot_dot', 1)
+        u = cs.MX.sym('u', 1)
+        x = cs.vertcat(s, s_dot)
+        x_dot = cs.vertcat(s_dot, s_dot_dot)
+
+        res_model = self.learned_dyn(x)
+        p = self.learned_dyn.get_sym_params()
+        parameter_values = self.learned_dyn.get_params(np.array([0, 0]))
+
+        f_expl = cs.vertcat(
+            s_dot,
+            u
+        ) + res_model
+
+        x_start = np.zeros((2))
+
+        # store to struct
+        model = cs.types.SimpleNamespace()
+        model.x = x
+        model.xdot = x_dot
+        model.u = u
+        model.z = cs.vertcat([])
+        model.p = p
+        model.parameter_values = parameter_values
+        model.f_expl = f_expl
+        model.x_start = x_start
+        model.constraints = cs.vertcat([])
+        model.name = "wr"
+
+        return model
+
+
+class MPC:
+    def __init__(self, model, N):
+        self.N = N
+        self.model = model
+
+    @property
+    def simulator(self):
+        return AcadosSimSolver(self.sim())
+
+    @property
+    def solver(self):
+        return AcadosOcpSolver(self.ocp())
+
+    def sim(self):
+        model = self.model
+
+        t_horizon = 1.
+        N = self.N
+
+        # Get model
+        model_ac = self.acados_model(model=model)
+        model_ac.p = model.p
+
+        # Dimensions
+        nx = 2
+        nu = 1
+        ny = 1
+
+        # Create OCP object to formulate the optimization
+        sim = AcadosSim()
+        sim.model = model_ac
+        sim.dims.N = N
+        sim.dims.nx = nx
+        sim.dims.nu = nu
+        sim.dims.ny = ny
+        sim.solver_options.tf = t_horizon
+
+        # Solver options
+        sim.solver_options.Tsim = 1./ 10.
+        sim.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
+        sim.solver_options.hessian_approx = 'GAUSS_NEWTON'
+        sim.solver_options.integrator_type = 'ERK'
+        # ocp.solver_options.print_level = 0
+        sim.solver_options.nlp_solver_type = 'SQP_RTI'
+
+        return sim
+
+    def ocp(self):
+        model = self.model
+
+        t_horizon = 1.
+        N = self.N
+
+        # Get model
+        model_ac = self.acados_model(model=model)
+        model_ac.p = model.p
+
+        # Dimensions
+        nx = 2
+        nu = 1
+        ny = 1
+
+        # Create OCP object to formulate the optimization
+        ocp = AcadosOcp()
+        ocp.model = model_ac
+        ocp.dims.N = N
+        ocp.dims.nx = nx
+        ocp.dims.nu = nu
+        ocp.dims.ny = ny
+        ocp.solver_options.tf = t_horizon
+
+        # Initialize cost function
+        ocp.cost.cost_type = 'LINEAR_LS'
+        ocp.cost.cost_type_e = 'LINEAR_LS'
+
+        ocp.cost.W = np.array([[1.]])
+
+        ocp.cost.Vx = np.zeros((ny, nx))
+        ocp.cost.Vx[0, 0] = 1.
+        ocp.cost.Vu = np.zeros((ny, nu))
+        ocp.cost.Vz = np.array([[]])
+        ocp.cost.Vx_e = np.zeros((ny, nx))
+        ocp.cost.W_e = np.array([[0.]])
+        ocp.cost.yref_e = np.array([0.])
+
+        # Initial reference trajectory (will be overwritten)
+        ocp.cost.yref = np.zeros(1)
+
+        # Initial state (will be overwritten)
+        ocp.constraints.x0 = model.x_start
+
+        # Set constraints
+        a_max = 10
+        ocp.constraints.lbu = np.array([-a_max])
+        ocp.constraints.ubu = np.array([a_max])
+        ocp.constraints.idxbu = np.array([0])
+
+        # Solver options
+        ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
+        ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
+        ocp.solver_options.integrator_type = 'ERK'
+        ocp.solver_options.nlp_solver_type = 'SQP_RTI'
+
+        ocp.parameter_values = model.parameter_values
+
+        return ocp
+
+    def acados_model(self, model):
+        model_ac = AcadosModel()
+        model_ac.f_impl_expr = model.xdot - model.f_expl
+        model_ac.f_expl_expr = model.f_expl
+        model_ac.x = model.x
+        model_ac.xdot = model.xdot
+        model_ac.u = model.u
+        model_ac.name = model.name
+        return model_ac
+
+
+def run():
+    N = 10
+
+    learned_dyn_model = l4c.realtime.RealTimeL4CasADi(PyTorchModel(), approximation_order=1)
+
+    model = DoubleIntegratorWithLearnedDynamics(learned_dyn_model)
+    solver = MPC(model=model.model(), N=N).solver
+
+    print('Warming up model...')
+    x_l = []
+    for i in range(N):
+        x_l.append(solver.get(i, "x"))
+    for i in range(20):
+        learned_dyn_model.get_params(np.stack(x_l, axis=0))
+    print('Warmed up!')
+
+    x = []
+    x_ref = []
+    ts = 1. / N
+    xt = np.array([1., 0.])
+    opt_times = []
+
+    for i in range(50):
+        now = time.time()
+        t = np.linspace(i * ts, i * ts + 1., 10)
+        yref = np.sin(0.5 * t + np.pi / 2)
+        x_ref.append(yref[0])
+        for t, ref in enumerate(yref):
+            solver.set(t, "yref", ref)
+        solver.set(0, "lbx", xt)
+        solver.set(0, "ubx", xt)
+        solver.solve()
+        xt = solver.get(1, "x")
+        x.append(xt)
+
+        x_l = []
+        for i in range(N):
+            x_l.append(solver.get(i, "x"))
+        params = learned_dyn_model.get_params(np.stack(x_l, axis=0))
+        for i in range(N):
+            solver.set(i, "p", params[i])
+
+        elapsed = time.time() - now
+        opt_times.append(elapsed)
+
+    print(f'Mean iteration time: {1000*np.mean(opt_times):.1f}ms -- {1/np.mean(opt_times):.0f}Hz)')
+
+
+if __name__ == '__main__':
+    run()

l4casadi/realtime/examples/readme.py → examples/realtime/readme.py

@@ -29,7 +29,7 @@ def forward(self, x):
 
 size_in = 2
 size_out = 1
-l4c_model = l4c.RealTimeL4CasADi(pyTorch_model, approximation_order=1)  # approximation_order=2
+l4c_model = l4c.realtime.RealTimeL4CasADi(pyTorch_model, approximation_order=1)  # approximation_order=2
 
 x_sym = cs.MX.sym('x', 2, 1)
 y_sym = l4c_model(x_sym)

l4casadi/__init__.py

@@ -2,7 +2,9 @@
 import ctypes
 
 from .l4casadi import L4CasADi, dynamic_lib_file_ending
-from .realtime import RealTimeL4CasADi
+
+from . import naive
+from . import realtime
 
 
 file_dir = files('l4casadi')