search

alphaville / optimization-engine

Nonconvex embedded optimization: code generation for fast real-time optimization + ROS support
https://alphaville.github.io/optimization-engine/
Other
512 stars 53 forks source link

Debugging 2000 -> Problem solution failed (solver error) #301

Open truncs opened 2 years ago

truncs commented 2 years ago

I am trying to formulate a MPCC style control system but while running it I get a problem solution failed. What does the error mean? Any thoughts oh how can I debug it?

See https://github.com/alphaville/optimization-engine/issues/301#issuecomment-1310830381 for reproducing the error.

Expected behavior

The problem should be solved? Note that the same problem works okay with IPOPT.

System information:

:warning: Please, provide the following information:

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.
alphaville commented 2 years ago

There should be an error code. Does the error happen when you call the solver?

I tried to run your code, but I'm not sure how to call the function generate_problem. What are the various arguments?

alphaville commented 2 years ago

If you're getting an error 2000, this means that the solver failed, while trying to solve the problem. This typically means that a NaN was encountered. Unless you have a reason to use the TCP interface, you can use the direct interface.

Try to enable the preconditioning and see if it helps.

truncs commented 2 years ago

I will try to use the direct interface to debug although I do find there is a significant performance difference. TCP interface is faster that the direct interface. open_runtimes_tcp

truncs commented 1 year ago

I could come with a sample file that you can run. The problem can be solved by IPT (included in the source code) but gives an solver error using panoc. I tried relaxing a few thresholds but no luck so far. Instructions on running the code

  1. Download/Copy-Paste the csv file
  2. Download/Copy-Paste the py file
  3. Change the csv path in the python file
  4. Run the python file

The python program will first try to solve using IPOPT, and then would generate the problem for panoc and error out with a solver error.

import math
import random

import csv
import casadi as cs
import opengen as og
import numpy as np

# IPOPT Options
opts = {}
opts["ipopt.print_level"] = 0
opts["ipopt.sb"] = "yes"
opts["print_time"] = 0

def runga_kutta_4th(f, xk, uk, dt):
    k1 = f(xk, uk)
    k2 = f(xk + dt/2*k1, uk)
    k3 = f(xk + dt/2*k2, uk)
    k4 = f(xk + dt*k3, uk)
    return dt/6*(k1+2*k2+2*k3+k4)

def generate_nlp_problem(n_states, n_inputs, n_desired, n_horizon, Ts, path_length, x_lut, y_lut):
    initial_state = cs.MX.sym("x0", n_states)
    desired_state = cs.MX.sym("xd", n_desired)

    c1, c2 = -0.65, 0.65
    r1, r2 = 0.975, 1.0

    obstacle = (10.050, 3.705140)

    Q = cs.diagcat(100.0, 10000000.0, 1000.0)
    R = cs.diagcat(50, 50, 50.0)
    l_f = 0.75
    l_r = 0.75
    l = 1.5

    states_lb = np.array([-10000000.0, -10000000.0, -np.pi, -30, -0.1, -3])
    states_ub = np.array([10000000.0, 10000000.0, np.pi, 30, path_length, 100000.0])

    inputs_lb = np.array([-1, -0.7, -2])
    inputs_ub = np.array([+10, 0.7, 2])

    x, u = cs.MX.sym("x", n_states), cs.MX.sym("u", n_inputs)
    p_x, p_y, p_yaw, v, p_s, p_vs = cs.vertsplit(x)
    a, delta, a_s = cs.vertsplit(u)

    beta = cs.atan(l_r / (l_f + l_r) * cs.tan(delta))
    dynamics = cs.vertcat(
        v * cs.cos(beta + p_yaw),
        v * cs.sin(beta + p_yaw),
        v * cs.tan(delta) * cs.cos(beta) / (l_f + l_r),
        a,
        p_vs,
        a_s
    )
    f = cs.Function("f", [x, u], [dynamics])

    X, U = cs.MX.sym("X", n_states, n_horizon), cs.MX.sym("U", n_inputs, n_horizon)
    X0X = cs.horzcat(initial_state, X)

    objective = 0

    dynamics_constr = []
    collision_constr = []

    for k in range(0, n_horizon - 1):
        xk, uk = X0X[:, k], U[:, k]
        xkp1 = X0X[:, k + 1]

        x_s = x_lut(xk[4])
        y_s = y_lut(xk[4])

        dy_x = x_lut.jacobian()(xk[4], x_s)
        dy_y = y_lut.jacobian()(xk[4], y_s)
        yaw = cs.atan2(dy_y, dy_x)

        # Contouring Error
        contour_error = cs.sin(yaw)*(xk[0] - x_s) - cs.cos(yaw)*(xk[1] - y_s)

        # Lag Error
        lag_error = -cs.cos(yaw)*(xk[0] - x_s) - cs.sin(yaw)*(xk[1] - y_s)

        # Velocity Error
        velocity_error = xk[3] - desired_state[0]

        err = cs.vertcat(contour_error, lag_error, velocity_error)

        if k == 0:
            a_delta = uk[0] - desired_state[1]
            delta_delta = uk[1] - desired_state[2]
            a_s_delta = uk[2] - desired_state[3]
            u_delta = cs.vertcat(a_delta, delta_delta, a_s_delta)
        else:
            u_delta = uk - u_previous

        objective += err.T @ Q @ err + u_delta.T @ R @ u_delta
        next_euler = (xk + runga_kutta_4th(f, xk, uk, Ts))
        dynamics_constr = cs.vertcat(dynamics_constr, xkp1 - next_euler)

        collision = r1**2 - (((xk[0] + c1*cs.cos(xk[2])) - obstacle[0])**2 + ((xk[1] + c1*cs.sin(xk[2])) + obstacle[1])**2)
        collision_constr = cs.vertcat(collision_constr, collision)
        collision = r2**2 - (((xk[0] + c2*cs.cos(xk[2])) - obstacle[0])**2 + ((xk[1] + c2*cs.sin(xk[2])) - obstacle[1])**2)
        collision_constr = cs.vertcat(collision_constr, collision)

        u_previous = uk

    xN = X0X[:, -1]
    x_s = x_lut(xN[4])
    y_s = y_lut(xN[4])
    dy_x = x_lut.jacobian()(xN[4], x_s)
    dy_y = y_lut.jacobian()(xN[4], y_s)
    yaw = cs.atan2(dy_y, dy_x)

    # Contouring Error
    contour_error = (cs.sin(yaw)*(xN[0] - x_s) - cs.cos(yaw)*(xN[1] - y_s))

    # Lag Error
    lag_error = (-cs.cos(yaw)*(xN[0] - x_s) - cs.sin(yaw)*(xN[1] - y_s))

    # Velocity Error
    velocity_error = xN[3] - desired_state[0]

    err = cs.vertcat(contour_error, lag_error, velocity_error)

    objective += 10 * err.T @ Q @ err  # Terminal cost

    dynamics_constr_lb = np.zeros(((n_horizon - 1) * n_states,))
    dynamics_constr_ub = np.zeros(((n_horizon - 1) * n_states,))

    collision_constr_lb = -np.inf * np.ones(((n_horizon - 1) * 2,))
    collision_constr_ub= np.zeros(((n_horizon - 1) * 2,))

    nlp = {
        "f": objective,
        "g": cs.vertcat(dynamics_constr, collision_constr),
        "x": cs.vertcat(
            X.reshape((n_horizon * n_states, 1)), U.reshape((n_horizon * n_inputs, 1))
        ),
        "p": cs.vertcat(initial_state, desired_state),
    }

    bounds = {
        "lbx": np.concatenate((np.tile(states_lb, n_horizon), np.tile(inputs_lb, n_horizon))),
        "ubx": np.concatenate((np.tile(states_ub, n_horizon), np.tile(inputs_ub, n_horizon))),
        "lbg": np.concatenate((dynamics_constr_lb, collision_constr_lb)),
        "ubg": np.concatenate((dynamics_constr_ub, collision_constr_ub)),
    }

    return nlp, bounds

def generate_panoc_problem(n_states, n_inputs, n_desired, n_horizon, Ts, path_length, x_lut, y_lut):

    parameters = cs.MX.sym('z', n_states + n_desired)

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

    Q = cs.diagcat(100.0, 10000000.0, 1000.0)
    R = cs.diagcat(50, 50, 50.0)
    l_f = 0.75
    l_r = 0.75
    l = 1.5

    states_lb = [-10000000.0, -10000000.0, -np.pi, -30, -0.1, -3]
    states_ub = [10000000.0, 10000000.0, np.pi, 30, path_length, 100000.0]

    inputs_lb = [-1, -0.7, -2]
    inputs_ub = [+10, 0.7, 2]

    x, u = cs.MX.sym("x", n_states), cs.MX.sym("u", n_inputs)
    p_x, p_y, p_yaw, v, p_s, p_vs = cs.vertsplit(x)
    a, delta, a_s = cs.vertsplit(u)

    beta = cs.atan(l_r / (l_f + l_r) * cs.tan(delta))
    dynamics = cs.vertcat(
        v * cs.cos(beta + p_yaw),
        v * cs.sin(beta + p_yaw),
        v * cs.tan(delta) * cs.cos(beta) / (l_f + l_r),
        a,
        p_vs,
        a_s
    )
    f = cs.Function("f", [x, u], [dynamics])

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

    objective = 0

    dynamics_constr = []

    for k in range(0, n_horizon - 1):
        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]

        x_s = x_lut(xk[4])
        y_s = y_lut(xk[4])

        dy_x = x_lut.jacobian()(xk[4], x_s)
        dy_y = y_lut.jacobian()(xk[4], y_s)
        yaw = cs.atan2(dy_y, dy_x)

        # Contouring Error
        contour_error = cs.sin(yaw)*(xk[0] - x_s) - cs.cos(yaw)*(xk[1] - y_s)

        # Lag Error
        lag_error = -cs.cos(yaw)*(xk[0] - x_s) - cs.sin(yaw)*(xk[1] - y_s)

        # Velocity Error
        velocity_error = xk[3] - desired_state[0]

        err = cs.vertcat(contour_error, lag_error, velocity_error)

        objective += err.T @ Q @ err + uk.T @ R @ uk
        next_euler = (xk + runga_kutta_4th(f, xk, uk, Ts))
        dynamics_constr = cs.vertcat(dynamics_constr, xkp1 - next_euler)

    xN = X0X[-n_states:]
    x_s = x_lut(xN[4])
    y_s = y_lut(xN[4])
    dy_x = x_lut.jacobian()(xN[4], x_s)
    dy_y = y_lut.jacobian()(xN[4], y_s)
    yaw = cs.atan2(dy_y, dy_x)

    # Contouring Error
    contour_error = (cs.sin(yaw)*(xN[0] - x_s) - cs.cos(yaw)*(xN[1] - y_s))

    # Lag Error
    lag_error = (-cs.cos(yaw)*(xN[0] - x_s) - cs.sin(yaw)*(xN[1] - y_s))

    # Velocity Error
    velocity_error = xN[3] - desired_state[0]

    err = cs.vertcat(contour_error, lag_error, velocity_error)

    objective += 10 * err.T @ Q @ err  # Terminal cost

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

    optimization_variables = variables
    optimization_parameters = parameters

    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)

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

    tcp_config = og.config.TcpServerConfiguration('0.0.0.0', 9555)

    build_config = og.config.BuildConfiguration()\
                    .with_build_directory("optimizers")\
                    .with_build_mode("release")\
                    .with_tcp_interface_config(tcp_config)

    meta = og.config.OptimizerMeta()\
            .with_optimizer_name("mpcc")
    solver_config = og.config.SolverConfiguration()\
                    .with_tolerance(1e-5)\
                    .with_preconditioning(True)

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

def main(x_lut, y_lut, path_length):

    n_states = 6
    n_desired = 4
    n_inputs = 3
    Ts = 0.1
    n_horizon = 15
    s = 0.0
    v_s = 0.0

    param = [0.051675815135240555, 3.7084553241729736, -5.326321862684935e-07, 0.00047293867110315835, 0.0016758168110367162, 0.0, 1.5, 0.0, 0.0, 0.0]
    acceleration, steering, a_s = 0.0, 0.0, 0.0

    # Generate the mpcc problem and solve using IPT
    nlp, bounds = generate_nlp_problem(n_states, n_inputs, n_desired, n_horizon, Ts, path_length, x_lut, y_lut)
    solver = cs.nlpsol('solver', 'ipopt', nlp, opts)
    solution = solver(lbx=bounds['lbx'], ubx=bounds['ubx'], lbg=bounds['lbg'], ubg=bounds['ubg'], p=param)
    results = solution['x']
    print(results[n_states * n_horizon: n_states*n_horizon + n_inputs])

    # Solve using PANOC
    generate_panoc_problem(n_states, n_inputs, n_desired, n_horizon, Ts, path_length, x_lut, y_lut)
    mpcc_mng = og.tcp.OptimizerTcpManager('optimizers/mpcc')
    mpcc_mng.start()
    solution = mpcc_mng.call(p=param)
    results = solution['solution']
    print(results[n_states * n_horizon: n_states*n_horizon + n_inputs])
    mng.kill()

if __name__ == '__main__':
    # Load the track and convert it into a spline
    waypoints_file = '/home/aditya/carla_track.txt'

    with open(waypoints_file) as waypoints_file_handle:
        waypoints = list(csv.reader(waypoints_file_handle, delimiter=',',
                        quoting=csv.QUOTE_NONNUMERIC))
        waypoints_np = np.array(waypoints)

    x_data = [w[0]*10**-2 for w in waypoints]
    y_data = [w[1]*10**-2 for w in waypoints]

    #y_data = savgol_filter(y_data, 51, 3)

    arr = np.zeros(len(x_data))

    for i in range(1, len(x_data)):
        dx = x_data[i] - x_data[i-1]
        dy = y_data[i] - y_data[i-1]
        arr[i] = math.hypot(dx, dy)

    xy_data = arr 

    sum = 0.0
    arr = np.zeros(len(xy_data))

    for i in range(len(xy_data)):
        arr[i] = sum + xy_data[i]
        sum += xy_data[i]

    x_lut = cs.interpolant('LUT', 'bspline', [arr], x_data)
    y_lut = cs.interpolant('LUT', 'bspline', [arr], y_data)
    path_length = arr[-1]

    main(x_lut, y_lut, path_length)

The csv file 

6.0, 370.5140
105.0, 370.5140
205.0, 370.5140
305.0, 370.5140
405.0, 370.5140
505.0, 370.5140
605.0, 370.5140
705.0, 370.5140
805.0, 370.5140
905.0, 370.5140
1005.0, 370.5140
1105.0, 370.5140
1205.0, 370.5140
1305.0, 370.5140
1405.0, 370.5140
1505.0, 370.5140
1605.0, 370.5140
1705.0, 370.5140
1805.0, 370.5140
1905.0, 370.5140
2005.0, 370.5140
2105.0, 370.5140
2205.0, 370.5140
2305.0, 370.5140
2405.0, 370.5140
2505.0, 370.5140
2605.0, 370.5140
2705.0, 370.5140
2805.0, 370.5140
2905.0, 370.5140
3005.0, 370.5140
3105.0, 370.5140
3205.0, 370.5140
3305.0, 370.5140
3405.0, 370.5140
3505.0, 370.5140
3605.0, 370.5140
3705.0, 370.5140
3805.0, 370.5140
3905.0, 370.5140
4005.0, 370.5140
4105.0, 370.5140
4205.0, 370.5140
4305.0, 370.5140
4405.0, 370.5140
4505.0, 370.5140
4605.0, 370.5140
4705.0, 370.5140
4805.0, 370.5140
4905.0, 370.5140
5005.0, 370.5140
5105.0, 370.5140
5205.0, 370.5140
5305.0, 370.5140
5405.0, 370.5140
5505.0, 370.5140
5605.0, 370.5140
5705.0, 370.5140
5805.0, 370.5140
5905.0, 370.5140
6005.0, 370.5140
6105.0, 370.5140
6200.0, 369.4
6300.0, 368.4
6400.0, 366.9
6500.0, 364.9
6600.0, 362.4
6700.0, 359.4
6800.0, 355.8
6900.0, 351.5
7000.0, 346.5
7100.0, 341.0
7200.0, 334.4
7300.0, 327.0
7400.0, 318.7
7500.0, 309.3
7600.0, 298.5
7700.0, 286.6
7751.0, 280.1
7800.0, 272.9
7850.0, 265.7
7916.0, 255.1
7975.0, 245.1
8000.0, 240.2
8054.0, 230.1
8102.0, 220.2
8149.0, 210.1
8191.0, 200.0
8231.0, 190.0
8268.0, 180.1
8304.0, 170.0
8337.0, 160.0
8369.0, 150.0
8399.0, 140.0
8427.0, 130.0
8453.0, 120.0
8479.0, 110.0
8503.0, 100.0
8525.0, 90.0
8546.0, 80.0
8567.0, 70.0
8585.0, 60.0
8600.0, 50.0
8621.0, 40.0
8637.0, 30.0
8652.0, 20.0
8667.0, 10.0
8680.0, 0.0
8692.0, -10.0
8700.0, -20.0
8715.0, -30.0
8726.0, -40.0
8736.0, -50.0
8745.0, -60.0
8754.0, -70.0
8760.0, -80.0
8768.0, -90.0
8775.0, -100.0
8781.0, -110.0
8788.0, -120.0
8792.0, -130.0
8796.0, -140.0
8800.0, -150.0
8804.0, -160.0
8804.0, -170.0
8804.0, -180.0
8804.0, -190.0
8804.0, -200.0
8804.0, -210.0
8804.0, -220.0
8804.0, -230.0
8804.0, -240.0
8804.0, -250.0
8804.0, -260.0
8804.0, -360.0
8804.0, -460.0
8804.0, -560.0
8804.0, -660.0
8804.0, -760.0
8804.0, -860.0
8804.0, -960.0
8804.0, -1060.0
8804.0, -1160.0
8804.0, -1260.0
8804.0, -1360.0
8804.0, -1460.0
8804.0, -1560.0
8804.0, -1660.0
8804.0, -1760.0
8804.0, -1860.0
8804.0, -1960.0
8804.0, -2060.0
truncs commented 1 year ago

@alphaville thoughts?

alphaville commented 1 year ago

@truncs Sorry for the delay. We're looking at it and we'll try to get back to you by next week.

truncs commented 1 year ago

@alphaville Thanks, much appreciated!

alphaville commented 1 year ago

We looked into this and the very first iteration, the algorithm is at the point:

u = [0.032540166451917776, 1.0014558585309914, -1.4383287905910368e-7, 0.00017414572012260812, -0.018120131695473597, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 0.18572675194199906, 0.0001146906039436235, 1e-12, 0.0004643168808530375, -0.1, 1e-12, -8.260619139023693e-5, -2.3681830224775235e-5, -2.2627164881463407e-6, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12]

and the gradient at that point is 

[0.046676354372876996, -2.4798313596954067, 2.5714315739064483e-5, 0.0015466545478518734, 
NaN, 6.612805104738676e-5, 0.01062739470113399, -1.4560830970755991, 2.0913201202445258e-7, 
-0.0004350111800550722, NaN, 1.526672569389828e-14, 0.03095445865853089, 1.9115102157289758e-10, 
-3.0761292035423143e-27, -0.00024931715252972196, NaN, 1.526672569389828e-14, 0.03095445865853089, 
1.9115102157289758e-10, -3.0761292035423143e-27, -0.00024931715252972196, NaN, 1.526672569389828e-14, 
0.03095445865853089, 1.9115102157289758e-10, -3.0761292035423143e-27, -0.00024931715252972196, NaN, 
1.526672569389828e-14, 0.03095445865853089, 1.9115102157289758e-10, -3.0761292035423143e-27, 
-0.00024931715252972196, NaN, 1.526672569389828e-14, 0.03095445865853089, 1.9115102157289758e-10, 
-3.0761292035423143e-27, -0.00024931715252972196, NaN, 1.526672569389828e-14, 0.03095445865853089, 
1.9115102157289758e-10, -3.0761292035423143e-27, -0.00024931715252972196, NaN, 1.526672569389828e-14, 
0.03095445865853089, 1.9115102157289758e-10, -3.0761292035423143e-27, -0.00024931715252972196, NaN, 
1.526672569389828e-14, 0.03095445865853089, 1.9115102157289758e-10, -3.0761292035423143e-27, 
-0.00024931715252972196, NaN, 1.526672569389828e-14, 0.03095445865853089, 1.9115102157289758e-10, 
-3.0761292035423143e-27, -0.00024931715252972196, NaN, 1.526672569389828e-14, 0.03095445865853089, 
1.9115102157289758e-10, -3.0761292035423143e-27, -0.00024931715252972196, NaN, 1.526672569389828e-14, 
0.05795881227575218, 1.6676009610635937e-5, 7.742864152452035e-11, 0.0025186290932355413, NaN, 
-0.0027004353617072928, -0.02701110470577827, -1.6675818459624492e-5, -4.500725704677709e-18, 
-6.751088418853e-5, 0.027004353617072926, -1.453982979565589e-13, 3.0954458657030894, 
1.9115100657289757e-8, 0.0, -0.0024992261385344185, NaN, 0.0, 0.00018145405361482672, 
9.226254817911728e-5, 0.0001438818602013502, 0.00012023743083737167, 1.26786767718842e-5, 
3.289952730641701e-6, 3.375545733305741e-8, 8.333347263556714e-18, 1.531149941368414e-14, 
3.375545733305741e-8, 8.333347263556714e-18, 1.531149941368414e-14, 3.375545733305741e-8, 
8.333347263556714e-18, 1.531149941368414e-14, 3.375545733305741e-8, 8.333347263556714e-18, 
1.531149941368414e-14, 3.375545733305741e-8, 8.333347263556714e-18, 1.531149941368414e-14, 
3.375545733305741e-8, 8.333347263556714e-18, 1.531149941368414e-14, 3.375545733305741e-8, 
8.333347263556714e-18, 1.531149941368414e-14, 3.375545733305741e-8, 8.333347263556714e-18, 
1.531149941368414e-14, 3.375545733305741e-8, 8.333347263556714e-18, 1.531149941368414e-14, 
3.375545733305741e-8, 8.333347263556714e-18, 1.531149941368414e-14, 3.375545733305741e-8, 
8.333347263556714e-18, 1.531149941368414e-14, 0.00014180661194775285, 3.87144489336943e-11, 
-0.00013502176807081646, 0.0, 0.0, 0.0]

where we see there are lots of NaNs, so I suspect there's something wrong with the gradient of the cost function. Perhaps it's not differentiatiable around the starting point. Is there a minimal working example?

alphaville commented 1 year ago

For what it's worth, I'm noticing (actually, @smokinmirror noticed) that it's at the 5th coordinate of the state where the gradient of the cost is NaN.

truncs commented 1 year ago

So I modified the file to remove all the extra bits and just have one iteration of the MPC. I do get values for the 5th variable and the states also look much different as compared to the states you mentioned. Attaching the code samples and the output for it.

import math
import random

import csv
import casadi as cs
import opengen as og
import numpy as np

def runga_kutta_4th(f, xk, uk, dt):
    k1 = f(xk, uk)
    k2 = f(xk + dt/2*k1, uk)
    k3 = f(xk + dt/2*k2, uk)
    k4 = f(xk + dt*k3, uk)
    return dt/6*(k1+2*k2+2*k3+k4)

def generate_panoc_problem(n_states, n_inputs, n_desired, n_horizon, Ts, path_length, x_lut, y_lut):

    parameters = cs.MX.sym('z', n_states + n_desired)

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

    Q = cs.diagcat(100.0, 10000000.0, 1000.0)
    R = cs.diagcat(50, 50, 50.0)
    l_f = 0.75
    l_r = 0.75
    l = 1.5

    states_lb = [-10000000.0, -10000000.0, -np.pi, -30, -0.1, -3]
    states_ub = [10000000.0, 10000000.0, np.pi, 30, path_length, 100000.0]

    inputs_lb = [-1, -0.7, -2]
    inputs_ub = [+10, 0.7, 2]

    x, u = cs.MX.sym("x", n_states), cs.MX.sym("u", n_inputs)
    p_x, p_y, p_yaw, v, p_s, p_vs = cs.vertsplit(x)
    a, delta, a_s = cs.vertsplit(u)

    beta = cs.atan(l_r / (l_f + l_r) * cs.tan(delta))
    dynamics = cs.vertcat(
        v * cs.cos(beta + p_yaw),
        v * cs.sin(beta + p_yaw),
        v * cs.tan(delta) * cs.cos(beta) / (l_f + l_r),
        a,
        p_vs,
        a_s
    )
    f = cs.Function("f", [x, u], [dynamics])

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

    objective = 0

    dynamics_constr = []

    xk, uk = X0X[:n_states], U[:]
    xkp1 = X0X[n_states:]

    x_s = x_lut(xk[4])
    y_s = y_lut(xk[4])

    dy_x = x_lut.jacobian()(xk[4], x_s)
    dy_y = y_lut.jacobian()(xk[4], y_s)
    yaw = cs.atan2(dy_y, dy_x)

    # Contouring Error
    contour_error = cs.sin(yaw)*(xk[0] - x_s) - cs.cos(yaw)*(xk[1] - y_s)

    # Lag Error
    lag_error = -cs.cos(yaw)*(xk[0] - x_s) - cs.sin(yaw)*(xk[1] - y_s)

    # Velocity Error
    velocity_error = xk[3] - desired_state[0]

    err = cs.vertcat(contour_error, lag_error, velocity_error)

    objective += err.T @ Q @ err + uk.T @ R @ uk
    next_euler = (xk + runga_kutta_4th(f, xk, uk, Ts))
    dynamics_constr = cs.vertcat(dynamics_constr, xkp1 - next_euler)

    optimization_variables = variables
    optimization_parameters = parameters

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

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

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

    tcp_config = og.config.TcpServerConfiguration('0.0.0.0', 9555)

    build_config = og.config.BuildConfiguration()\
                    .with_build_directory("optimizers")\
                    .with_build_mode("release")\
                    .with_tcp_interface_config(tcp_config)

    meta = og.config.OptimizerMeta()\
            .with_optimizer_name("mpcc_test2")
    solver_config = og.config.SolverConfiguration()\
                    .with_tolerance(1e-5)\
                    .with_preconditioning(True)

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

def main(x_lut, y_lut, path_length):

    n_states = 6
    n_desired = 4
    n_inputs = 3
    Ts = 0.1
    n_horizon = 15
    s = 0.0
    v_s = 0.0

    param = [0.051675815135240555, 3.7084553241729736, -5.326321862684935e-07, 0.00047293867110315835, 0.0016758168110367162, 0.0, 1.5, 0.0, 0.0, 0.0]
    acceleration, steering, a_s = 0.0, 0.0, 0.0

    # Solve using PANOC
    generate_panoc_problem(n_states, n_inputs, n_desired, n_horizon, Ts, path_length, x_lut, y_lut)
    mpcc_mng = og.tcp.OptimizerTcpManager('optimizers/mpcc_test2')
    mpcc_mng.start()
    solution = mpcc_mng.call(p=param)
    results = solution['solution']
    print(results[:])
    mpcc_mng.kill()

if __name__ == '__main__':
    # Load the track and convert it into a spline
    waypoints_file = '/home/aditya/carla_track.txt'

    with open(waypoints_file) as waypoints_file_handle:
        waypoints = list(csv.reader(waypoints_file_handle, delimiter=',',
                        quoting=csv.QUOTE_NONNUMERIC))
        waypoints_np = np.array(waypoints)

    x_data = [w[0]*10**-2 for w in waypoints]
    y_data = [w[1]*10**-2 for w in waypoints]

    #y_data = savgol_filter(y_data, 51, 3)

    arr = np.zeros(len(x_data))

    for i in range(1, len(x_data)):
        dx = x_data[i] - x_data[i-1]
        dy = y_data[i] - y_data[i-1]
        arr[i] = math.hypot(dx, dy)

    xy_data = arr 

    sum = 0.0
    arr = np.zeros(len(xy_data))

    for i in range(len(xy_data)):
        arr[i] = sum + xy_data[i]
        sum += xy_data[i]

    x_lut = cs.interpolant('LUT', 'bspline', [arr], x_data)
    y_lut = cs.interpolant('LUT', 'bspline', [arr], y_data)
    path_length = arr[-1]

    main(x_lut, y_lut, path_length)

Output

[0.051721711545130486, 3.708455507734047, -5.35082141500763e-07, 0.0004449378279139774, 0.0016757785034980444, -7.678264896759694e-07, -0.0002715292682926265, -7.76248128226494e-05, -7.445753145453775e-06]