alphaville / optimization-engine

Nonconvex embedded optimization: code generation for fast real-time optimization

Home Page:https://alphaville.github.io/optimization-engine/

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

Slow iteration time

truncs opened this issue · comments

Describe the bug

I have a toy MPC problem to go from the start to a destination while obeying the dynamics. Full code is below. While running the sample seems like the per iteration time is in the other of >100msecs and the constraints of the obstacle is being violated.

To Reproduce

import time
import matplotlib.pyplot as plt
from datetime import timedelta
import os
import sys
import casadi as cs
import numpy as np
import opengen as og


Ts = 0.05  # Sampling time
N_hor = 18  # Horizon length
N_sim = 80 # Length of the simulation

multipleshooting = False
R_obstacle = 2


def _generate_problem_ms(
    Ts,
    N_hor,
    R_obstacle,
    n_states,
    n_inputs,
    f,
    Q,
    R,
    states_lb,
    states_ub,
    inputs_lb,
    inputs_ub,
):
    n_horizon = N_hor
    parameters = cs.MX.sym('z', n_states + n_states)

    initial_state = parameters[0:n_states]
    desired_state = parameters[n_states:]

    variables = cs.MX.sym("vars", n_states*N_hor + n_inputs*N_hor)
    X = variables[0:n_states*N_hor]
    U = variables[n_states*N_hor:]

    X0X = cs.vertcat(initial_state, X)
    objective = 0
    dynamics_constr = []
    collision_constr = []

    # Forward Euler
    for k in range(N_hor):
        xk, uk = X0X[k*n_states:k*n_states + n_states], U[k*n_inputs:k*n_inputs + n_inputs]
        xkp1 = X0X[(k+1)*n_states:(k + 1)*n_states + n_states]

        err = xk - desired_state
        objective += err.T @ Q @ err + uk.T @ R @ uk
        next_euler = xk + Ts * f(xk, uk)
        dynamics_constr = cs.vertcat(dynamics_constr, xkp1 - next_euler)
        if k > 0:
            collision_constr = cs.vertcat(
                collision_constr, xk[0:2].T @ xk[0:2]
            )  # ball around (0,0)

    xN = X0X[-n_states:]
    err = xN - desired_state
    objective += 10 * err.T @ Q @ err  # Terminal cost
    collision_constr = cs.vertcat(collision_constr, xN[0:2].T @ xN[0:2])

    constraints = cs.vertcat(dynamics_constr, collision_constr)

    dynamics_constr_lb = [0.0] * N_hor * n_states
    dynamics_constr_ub = [0.0] * N_hor * n_states

    collision_constr_lb = [R_obstacle ** 2] * N_hor
    collision_constr_ub = [+np.inf] * N_hor

    c_bounds = og.constraints.Rectangle(
        dynamics_constr_lb + collision_constr_lb,
        dynamics_constr_ub + collision_constr_ub
    )

    xmin = states_lb * n_horizon
    umin = inputs_lb * n_horizon
    xmax = states_ub * n_horizon
    umax = inputs_ub * n_horizon

    bounds = og.constraints.Rectangle(xmin + umin, xmax + umax)

    #c = dynamics_constr #cs.vertcat(dynamics_constr, collision_constr)
    #set_c = og.constraints.Zero()

    optimization_variables = variables
    optimization_parameters = parameters

    problem = og.builder.Problem(optimization_variables,
                optimization_parameters,
                objective) \
                .with_aug_lagrangian_constraints(constraints, c_bounds) \
                .with_constraints(bounds)

    first_input_idx = N_hor * n_states

    build_config = og.config.BuildConfiguration()\
                    .with_build_directory("optimizers")\
                    .with_build_mode("debug")\
                    .with_build_python_bindings()

    meta = og.config.OptimizerMeta()\
            .with_optimizer_name("bicycle")
    solver_config = og.config.SolverConfiguration()\
                    .with_tolerance(1e-4)

    builder = og.builder.OpEnOptimizerBuilder(problem,
                meta,
                build_config,
                solver_config)\
                .with_verbosity_level(1)
    builder.build()

    return f, n_states, n_inputs, first_input_idx


def generate_problem(Ts, N_hor, R_obstacle, multipleshooting):

    n_states = 4
    n_inputs = 2

    # state weights matrix
    Q = cs.diagcat(100, 100, 10, 10)
    # controls weights matrix
    R = cs.diagcat(1e-1, 1e-5)

    # physical constants
    l = 0.5
    l_r = l / 2
    l_f = l / 2

    # state and input constraints
    states_lb = [-10, -10, -np.pi / 3, -3]
    states_ub = [+10, +10, +np.pi / 3, +3]

    inputs_lb = [-5, -np.pi / 4]
    inputs_ub = [+5, +np.pi / 4]

    x, u = cs.SX.sym("x", n_states), cs.SX.sym("u", n_inputs)
    p_x, p_y, ψ, v = cs.vertsplit(x)
    a, δ_f = cs.vertsplit(u)
    β = cs.atan(l_r / (l_f + l_r) * cs.tan(δ_f))
    continuous_dynamics = cs.vertcat(
        v * cs.cos(ψ + β),
        v * cs.sin(ψ + β),
        v / l_r * cs.sin(β),
        a,
    )
    f = cs.Function("f", [x, u], [continuous_dynamics])

    gen = _generate_problem_ms 
    return gen(
        Ts,
        N_hor,
        R_obstacle,
        n_states,
        n_inputs,
        f,
        Q,
        R,
        states_lb,
        states_ub,
        inputs_lb,
        inputs_ub,
    )

f, n_states, n_inputs, first_input_idx = generate_problem(
    Ts, N_hor, R_obstacle, multipleshooting
)

sys.path.insert(1, './optimizers/bicycle')
import bicycle
solver = bicycle.solver()

state = np.array([-5.0, 0.0, 0.0, 0.0])
dest = np.array([5.0, 0.1, 0, 0])


xs = np.zeros((N_sim, n_states))
times = np.zeros((N_sim,))
for k in range(N_sim):
    p = state.tolist() + dest.tolist()
    t0 = time.perf_counter()
    solution = solver.run(p=p)
    t = time.perf_counter() - t0
    times[k] = t
    xs[k] = state
    print(
        solution.exit_status,
        solution.solve_time_ms,
        solution.num_outer_iterations,
        solution.num_inner_iterations,
    )
    #import ipdb; ipdb.set_trace()
    input = solution.solution[first_input_idx : first_input_idx + n_inputs]
    state += (Ts * f(state, input)).toarray().squeeze()
    print('State: ' + str(state))

#%% Plot

fig_trajectory, ax = plt.subplots(1, 1)
c = plt.Circle((0, 0), R_obstacle)
ax.set_aspect(1)
ax.add_artist(c)
ax.plot(xs[:, 0], xs[:, 1], "r.-")
ax.set_title('Trajectory')

fig_time, ax = plt.subplots(1, 1)
ax.plot(times)
ax.set_title("Run time")
ax.set_xlabel("MPC time step")
ax.set_ylabel("Run time [s]")

plt.show()

Output

[INFO] --- Initialising builder: 'bicycle'
[INFO] Checking user parameters
[INFO] Creating necessary folders
[INFO] Copying icasadi interface to target directory
[INFO] Generating icasadi's Cargo.toml
[INFO] Generating Cargo.toml for target optimizer
[INFO] Generating icasadi Rust library file
[INFO] Defining CasADi functions and generating C code
[INFO] Defining function psi(u, xi, p) and its gradient
[INFO] Function psi and its gradient (C code)
[INFO] Defining function F1(u, p)
[INFO] Mapping F1 (C code)
[INFO] Defining function F2(u, p)
[INFO] Mapping F2 (C code)
[INFO] Defining preconditioning functions
[INFO] Generating casadi_memory.h
[INFO] Generating intercafe.c (C interface)
[INFO] Generating main code for target optimizer (lib.rs)
[INFO] Generating build.rs for target optimizer
[INFO] Generating YAML configuration file
[INFO] Building optimizer
[INFO] Generating Python bindings
[INFO] Generating code for Python bindings
[INFO] Generating code for Python bindings in /home/aditya/workspace/gist/python/optimizers/bicycle/python_bindings_bicycle
[INFO] Building Python bindings
[INFO] To use the Python bindings do:

       import sys
       sys.path.insert(1, "/home/aditya/workspace/gist/python/optimizers/bicycle")
       import bicycle
       solver = bicycle.solver()
NotConvergedIterations 800.576932 10 2336
State: [-5.    0.    0.    0.25]
 NotConvergedIterations 923.244461 10 1755
State: [-4.98750071e+00  1.33447889e-04  5.33791557e-04  5.00000000e-01]
NotConvergedIterations 753.422069 10 1705
State: [-4.96250209e+00  3.95972893e-04  1.53051521e-03  7.50000000e-01]
NotConvergedIterations 1405.286654 10 2426
State: [-4.92500589e+00  9.29464099e-04  3.43492358e-03  1.00000000e+00]
NotConvergedIterations 1815.95558 10 2279
State: [-4.87500844e+00  1.43463878e-03  4.76866209e-03  1.25000000e+00]
NotConvergedIterations 1744.04235 10 2138
State: [-4.81251097e+00  1.99671773e-03  5.82483952e-03  1.50000000e+00]
NotConvergedIterations 1368.724023 10 1823
State: [-4.73751513e+00  2.78688960e-03  7.23812838e-03  1.75000000e+00]
NotConvergedIterations 1382.540392 10 1892
State: [-4.65002008e+00  3.71801435e-03  8.42935045e-03  2.00000000e+00]
NotConvergedIterations 1320.123336 10 1742
State: [-4.55002574  0.00478161  0.00931208  2.25      ]
NotConvergedIterations 366.162178 10 941
State: [-4.43753089  0.00585782  0.00942655  2.5       ]
NotConvergedIterations 2077.663025 10 2628
State: [-4.31253282  0.00655287  0.00749349  2.75      ]
NotConvergedIterations 406.652056 10 1206
State: [-4.17516376  0.01255229  0.02737302  2.9999935 ]
NotConvergedIterations 1063.27191 10 2260
State: [-4.02547108e+00  2.96056065e-03 -2.73676234e-02  3.00028176e+00]
NotConvergedIterations 1064.91184 10 2298
State: [-3.8770586   0.02482292  0.07629382  3.00002287]
NotConvergedIterations 586.353629 10 1685
State: [-3.72850589  0.00402779 -0.05193537  3.00006707]
NotConvergedIterations 990.466358 10 2008
State: [-3.58125486  0.03263094  0.09289937  3.00001418]
NotConvergedIterations 1538.88352 10 2562
State: [-3.43524716e+00 -1.74873727e-03 -9.82044519e-02  3.00000000e+00]
NotConvergedIterations 885.187163 10 1674
State: [-3.28976292  0.03477985  0.10426289  2.9999637 ]
NotConvergedIterations 1417.026037 10 2060
State: [-3.14413475e+00 -1.16304073e-03 -9.93524230e-02  3.00000877e+00]
NotConvergedIterations 491.362972 10 1449
State: [-3.00098139  0.04363926  0.13577001  2.99988343]
NotConvergedIterations 484.128367 10 1406
State: [-2.85896874 -0.00463764 -0.13244801  2.99994976]
NotConvergedIterations 505.852358 10 1389
State: [-2.71712318  0.04413819  0.13587565  2.99963152]
NotConvergedIterations 1252.362531 10 2144
State: [-2.57512632 -0.00414604 -0.13241955  3.00003704]
NotConvergedIterations 877.585522 10 1796
State: [-2.43327802  0.04463525  0.13591192  3.00001228]
NotConvergedIterations 387.091249 10 1177
State: [-2.29126139 -0.00364996 -0.13241733  3.00000005]
NotConvergedIterations 400.878633 10 1167
State: [-2.14941494  0.04513105  0.13591083  2.99959161]
NotConvergedIterations 508.804357 10 1485
State: [-2.00741828 -0.00314755 -0.1323808   3.00020317]
NotConvergedIterations 1202.722654 10 2333
State: [-1.86556401  0.04564194  0.13596553  2.99187946]
NotConvergedIterations 749.762893 10 2015
State: [-1.72392980e+00 -2.50477538e-03 -1.31636305e-01  2.98649244e+00]
NotConvergedIterations 663.798148 10 1999
State: [-1.58275999  0.04616686  0.1354837   2.98483896]
NotConvergedIterations 791.530276 10 2349
State: [-1.44148223e+00 -1.93463429e-03 -1.31488413e-01  2.96019540e+00]
NotConvergedIterations 787.918254 10 2286
State: [-1.30156261  0.04632913  0.13327951  2.9968154 ]
NotConvergedIterations 1123.395589 10 3303
State: [-1.15982477e+00 -2.27790730e-03 -1.34763805e-01  2.87602037e+00]
NotConvergedIterations 733.542864 10 2159
State: [-1.02373099  0.04416793  0.12247528  2.96861904]
NotConvergedIterations 806.992206 10 2410
State: [-0.88385515 -0.00549589 -0.14304608  2.86239133]
NotConvergedIterations 764.634283 10 2270
State: [-0.74802809  0.03960645  0.11297398  2.95781065]
NotConvergedIterations 1030.371484 10 3051
State: [-0.60913795 -0.01119846 -0.15158065  2.851468  ]
NotConvergedIterations 983.76996 10 2857
State: [-0.47345071  0.03257534  0.10346241  2.77087831]
NotConvergedIterations 892.737243 10 2651
State: [-0.34379694 -0.01625412 -0.14437249  2.70816828]
NotConvergedIterations 827.027232 10 2459
State: [-0.21523163  0.02624765  0.09785345  2.76021184]
NotConvergedIterations 1104.807077 10 3179
State: [-0.08635182 -0.0231175  -0.1490274   2.99690514]
NotConvergedIterations 952.347392 10 2757
State: [0.05613813 0.02325292 0.11902394 3.00075724]
NotConvergedIterations 967.360842 10 2527
State: [ 0.19735416 -0.02743625 -0.14937195  3.00403831]
NotConvergedIterations 1053.398535 10 3114
State: [ 0.34673575 -0.04311282 -0.12245785  3.0021026 ]
NotConvergedIterations 1051.560627 10 3120
State: [ 0.49531508 -0.06446077 -0.13461313  3.00274609]
NotConvergedIterations 883.726138 10 2627
State: [ 0.64382776 -0.08648787 -0.1421987   2.97508013]
NotConvergedIterations 792.449016 10 2354
State: [ 0.79088218 -0.10891001 -0.14761983  3.00534127]
NotConvergedIterations 922.714166 10 2747
State: [ 0.93979003 -0.12907537 -0.13979598  2.99045759]
NotConvergedIterations 871.443076 10 2610
State: [ 1.087975   -0.14903297 -0.13625457  2.99020465]
NotConvergedIterations 763.4208 10 2267
State: [ 1.23669247 -0.16440908 -0.11638581  2.99228927]
NotConvergedIterations 946.037891 10 2787
State: [ 1.3856732  -0.17816513 -0.10183726  2.98743045]
NotConvergedIterations 590.015271 10 1791
State: [ 1.53409252 -0.1950043  -0.10849093  2.99839465]
NotConvergedIterations 818.22886 10 2468
State: [ 1.68283298 -0.21377134 -0.11869635  2.98877279]
NotConvergedIterations 852.416081 10 2564
State: [ 1.83131581 -0.2306461  -0.11538839  2.99496591]
NotConvergedIterations 638.350512 10 1943
State: [ 1.98105901 -0.22941084 -0.04151895  2.99857885]
NotConvergedIterations 556.796054 10 1683
State: [ 2.13006888 -0.21283538  0.04946558  2.99968008]
NotConvergedIterations 305.1892 10 950
State: [ 2.27915424 -0.1964416   0.08547411  3.00012737]
NotConvergedIterations 911.458133 10 1534
State: [ 2.42826145 -0.18004191  0.09991625  2.99972332]
NotConvergedIterations 387.887667 10 1182
State: [ 2.57736516 -0.16379584  0.10508399  3.00021083]
NotConvergedIterations 376.297712 10 1045
State: [ 2.7264739  -0.14737193  0.10785675  2.9996894 ]
NotConvergedIterations 712.013448 10 1467
State: [ 2.87556999 -0.13107165  0.10847954  2.99967888]
NotConvergedIterations 497.461488 10 1082
State: [ 3.02464048 -0.11454377  0.10964486  3.00035815]
NotConvergedIterations 398.429545 10 1126
State: [ 3.17372043 -0.09779441  0.11098749  2.99767155]
NotConvergedIterations 458.067478 10 1362
State: [ 3.32267771 -0.08115657  0.11113548  2.99624818]
NotConvergedIterations 581.447708 10 1638
State: [ 3.47168769 -0.0656716   0.10658833  2.99744456]
NotConvergedIterations 560.799398 10 1701
State: [ 3.62057275 -0.04849828  0.11153425  2.9250377 ]
NotConvergedIterations 457.857649 10 1411
State: [ 3.76586693 -0.03178854  0.11327111  2.80433429]
NotConvergedIterations 542.461492 10 1641
State: [ 3.905411   -0.01807071  0.1047007   2.83830649]
NotConvergedIterations 1071.90147 10 1891
State: [ 4.04629410e+00 -9.85283636e-04  1.13773643e-01  2.60199510e+00]
NotConvergedIterations 433.697027 10 1361
State: [4.17542274 0.01488104 0.11818957 2.42326072]
NotConvergedIterations 511.168909 10 1494
State: [4.29571993 0.02934015 0.11888337 2.17427203]
NotConvergedIterations 411.213897 10 1285
State: [4.40380838 0.04098215 0.11384399 1.92575291]
NotConvergedIterations 424.084041 10 1243
State: [4.49946719 0.05196859 0.11403859 1.68797744]
NotConvergedIterations 449.872311 10 1382
State: [4.5833825  0.06099027 0.11169548 1.46574383]
NotConvergedIterations 473.946268 10 1410
State: [4.65627752 0.06856178 0.1092923  1.22475309]
NotConvergedIterations 384.576425 10 1176
State: [4.71725176 0.07423569 0.1052495  1.11116941]
NotConvergedIterations 512.841552 10 1353
State: [4.77151287 0.08617197 0.12992875 0.90972509]
NotConvergedIterations 589.811603 10 1809
State: [4.8169978  0.08651929 0.10773356 0.8244252 ]
NotConvergedIterations 442.043169 10 1300
State: [4.85782463 0.0922081  0.1127972  0.58158044]
NotConvergedIterations 408.263158 10 1247
State: [4.88688491 0.09116421 0.09556429 0.51051137]

Run the self contained sample above.

Expected behavior

Per iteration should have been way faster and the constraints honored.

System information:

  • System/Platform: AMD Threadripper 1950X
  • OS: Ubuntu
  • What is the output of rustup show?
Default host: x86_64-unknown-linux-gnu
rustup home:  /home/aditya/.rustup

stable-x86_64-unknown-linux-gnu (default)
rustc 1.64.0 (a55dd71d5 2022-09-19)
  • What is the output of rustc -V?
rustc 1.64.0 (a55dd71d5 2022-09-19)
  • Python/MATLAB version if relevant:
python --version
Python 3.8.14

Additional context

Add any other context about the problem here.

I was able to solve the constraints being not honored part by using penalty_constraints but it is still slow. Updated cods below

import time
import matplotlib.pyplot as plt
from datetime import timedelta
import os
import sys
import casadi as cs
import numpy as np
import opengen as og


Ts = 0.05  # Sampling time
N_hor = 18  # Horizon length
N_sim = 80 # Length of the simulation

multipleshooting = False
R_obstacle = 2


def _generate_problem_ms(
    Ts,
    N_hor,
    R_obstacle,
    n_states,
    n_inputs,
    f,
    Q,
    R,
    states_lb,
    states_ub,
    inputs_lb,
    inputs_ub,
):
    n_horizon = N_hor
    parameters = cs.MX.sym('z', n_states + n_states)

    initial_state = parameters[0:n_states]
    desired_state = parameters[n_states:]

    variables = cs.MX.sym("vars", n_states*N_hor + n_inputs*N_hor)
    X = variables[0:n_states*N_hor]
    U = variables[n_states*N_hor:]

    X0X = cs.vertcat(initial_state, X)
    objective = 0
    dynamics_constr = []
    collision_constr = []

    # Forward Euler
    for k in range(N_hor):
        xk, uk = X0X[k*n_states:k*n_states + n_states], U[k*n_inputs:k*n_inputs + n_inputs]
        xkp1 = X0X[(k+1)*n_states:(k + 1)*n_states + n_states]

        err = xk - desired_state
        objective += err.T @ Q @ err + uk.T @ R @ uk
        next_euler = xk + Ts * f(xk, uk)
        dynamics_constr = cs.vertcat(dynamics_constr, xkp1 - next_euler)
        if k > 0:
            collision_constr = cs.vertcat(
                collision_constr, cs.fmax(R_obstacle**2 - xk[0:2].T @ xk[0:2], 0)
            )  # ball around (0,0)

    xN = X0X[-n_states:]
    err = xN - desired_state
    objective += 10 * err.T @ Q @ err  # Terminal cost
    collision_constr = cs.vertcat(collision_constr, cs.fmax(R_obstacle**2 - xk[0:2].T @ xk[0:2], 0))

    constraints = cs.vertcat(dynamics_constr, collision_constr)

    dynamics_constr_lb = [0.0] * N_hor * n_states
    dynamics_constr_ub = [0.0] * N_hor * n_states

    collision_constr_lb = [R_obstacle ** 2] * N_hor
    collision_constr_ub = [+np.inf] * N_hor

    c_bounds = og.constraints.Rectangle(
        dynamics_constr_lb + collision_constr_lb,
        dynamics_constr_ub + collision_constr_ub
    )


    xmin = states_lb * n_horizon
    umin = inputs_lb * n_horizon
    xmax = states_ub * n_horizon
    umax = inputs_ub * n_horizon

    bounds = og.constraints.Rectangle(xmin + umin, xmax + umax)

    #c = dynamics_constr #cs.vertcat(dynamics_constr, collision_constr)
    #set_c = og.constraints.Zero()

    optimization_variables = variables
    optimization_parameters = parameters

    problem = og.builder.Problem(optimization_variables,
                optimization_parameters,
                objective) \
                .with_penalty_constraints(constraints) \
                .with_constraints(bounds)

    first_input_idx = N_hor * n_states

    build_config = og.config.BuildConfiguration()\
                    .with_build_directory("optimizers")\
                    .with_build_mode("debug")\
                    .with_build_python_bindings()

    meta = og.config.OptimizerMeta()\
            .with_optimizer_name("bicycle")
    solver_config = og.config.SolverConfiguration()\
                    .with_tolerance(1e-4)

    builder = og.builder.OpEnOptimizerBuilder(problem,
                meta,
                build_config,
                solver_config)\
                .with_verbosity_level(1)
    builder.build()

    return f, n_states, n_inputs, first_input_idx


def generate_problem(Ts, N_hor, R_obstacle, multipleshooting):

    n_states = 4
    n_inputs = 2

    # state weights matrix
    Q = cs.diagcat(100, 100, 10, 10)
    # controls weights matrix
    R = cs.diagcat(1e-1, 1e-5)

    # physical constants
    l = 0.5
    l_r = l / 2
    l_f = l / 2

    # state and input constraints
    states_lb = [-10, -10, -np.pi / 3, -3]
    states_ub = [+10, +10, +np.pi / 3, +3]

    inputs_lb = [-5, -np.pi / 4]
    inputs_ub = [+5, +np.pi / 4]

    x, u = cs.SX.sym("x", n_states), cs.SX.sym("u", n_inputs)
    p_x, p_y, ψ, v = cs.vertsplit(x)
    a, δ_f = cs.vertsplit(u)
    β = cs.atan(l_r / (l_f + l_r) * cs.tan(δ_f))
    continuous_dynamics = cs.vertcat(
        v * cs.cos(ψ + β),
        v * cs.sin(ψ + β),
        v / l_r * cs.sin(β),
        a,
    )
    f = cs.Function("f", [x, u], [continuous_dynamics])

    gen = _generate_problem_ms 
    return gen(
        Ts,
        N_hor,
        R_obstacle,
        n_states,
        n_inputs,
        f,
        Q,
        R,
        states_lb,
        states_ub,
        inputs_lb,
        inputs_ub,
    )

f, n_states, n_inputs, first_input_idx = generate_problem(
    Ts, N_hor, R_obstacle, multipleshooting
)

sys.path.insert(1, './optimizers/bicycle')
import bicycle
solver = bicycle.solver()

state = np.array([-5.0, 0.0, 0.0, 0.0])
dest = np.array([5.0, 0.1, 0, 0])


xs = np.zeros((N_sim, n_states))
times = np.zeros((N_sim,))
for k in range(N_sim):
    p = state.tolist() + dest.tolist()
    t0 = time.perf_counter()
    solution = solver.run(p=p)
    t = time.perf_counter() - t0
    times[k] = t
    xs[k] = state
    print(
        solution.exit_status,
        solution.solve_time_ms,
        solution.num_outer_iterations,
        solution.num_inner_iterations,
    )
    #import ipdb; ipdb.set_trace()
    input = solution.solution[first_input_idx : first_input_idx + n_inputs]
    state += (Ts * f(state, input)).toarray().squeeze()
    print('State: ' + str(state))

#%% Plot

fig_trajectory, ax = plt.subplots(1, 1)
c = plt.Circle((0, 0), R_obstacle)
ax.set_aspect(1)
ax.add_artist(c)
ax.plot(xs[:, 0], xs[:, 1], "r.-")
ax.set_title('Trajectory')

fig_time, ax = plt.subplots(1, 1)
ax.plot(times)
ax.set_title("Run time")
ax.set_xlabel("MPC time step")
ax.set_ylabel("Run time [s]")

plt.show()


Closing this, found that with the release build it is really fast, on the order 60msecs.

@truncs Aditya, you can also try to use with_preconditioning(True). This may speed things up further.

@alphaville Amazing. Getting runtimes of 10 - 15 msecs. Thank you!