Free final time optimization in acados

User Questions
1

Hi,

I am looking help for free final time optimization in acados. My use case is little complex so I have a simple double integrator system example with me both of which are giving me same error. I am using python interface.

import numpy as np
import matplotlib.pyplot as plt
import scipy.linalg
from acados_template import AcadosOcp, AcadosOcpSolver
from acados_template import AcadosModel
import casadi as ca

def export_double_integrator_model():
    """
    Export double integrator model
    """
    model_name = 'double_integrator'
    
    # States
    x = ca.MX.sym('x')  # position
    v = ca.MX.sym('v')  # velocity
    T = ca.MX.sym('T')  # free final time
    x = ca.vertcat(x, v, T)
    
    x_dot = ca.MX.sym('xdot', 3)  # state derivative
    
    # Controls
    u = ca.MX.sym('u')  # acceleration
    
    # Dynamics
    x_dot_expr = ca.vertcat(v, u, 0)
    f_expr = x_dot_expr * T
    
    # Model
    model = AcadosModel()
    model.name = model_name
    model.x = x
    model.u = u
    model.xdot = x_dot
    model.f_expl_expr = f_expr  # continuous time dynamics
    
    return model

def create_ocp_solver():
    # Create model
    model = export_double_integrator_model()
    
    # Create OCP
    ocp = AcadosOcp()
    
    # Set model
    ocp.model = model
    
    # Parameters
    N = 50
    
    # Set time horizon
    ocp.dims.N = N
    ocp.solver_options.tf = 1
    
    # Set cost
    ocp.cost.cost_type = 'EXTERNAL'
    ocp.cost.cost_type_e = 'EXTERNAL'
    
    # In acados, for minimum time, we can simply put the cost objective as 1.0 (the time step)
    # and then scale the dynamics by T
    x = ocp.model.x
    u = ocp.model.u
    
    Q = np.diag([0.0, 0.0, 1.0])
    ocp.model.cost_expr_ext_cost = ca.mtimes([x.T, Q, x]) + u**2
    ocp.model.cost_expr_ext_cost_e = ca.mtimes([x.T, Q, x])
    
    # Initial state
    x0 = np.array([0.0, 0.0, 0])
    ocp.constraints.x0 = x0
    
    # Terminal constraint
    ocp.constraints.idxbx = np.array([0, 1, 2])
    ocp.constraints.lbx = np.array([-15, -5, 0])
    ocp.constraints.ubx = np.array([15.0, 5.0, 20])
    
    # Terminal constraint # Final state
    ocp.constraints.idxbx_e = np.array([0, 1, 2])
    xf_0 = np.array([10.0, 0.0, 0])
    xf_f = np.array([10.0, 0.0, 20.0])
    ocp.constraints.lbx_e = xf_0
    ocp.constraints.ubx_e = xf_f
    
    # Control constraints
    u_max = 1.0
    ocp.constraints.idxbu = np.array([0])
    ocp.constraints.lbu = np.array([-u_max])
    ocp.constraints.ubu = np.array([u_max])
    
    # Solver options
    ocp.solver_options.integrator_type = 'ERK'
    ocp.solver_options.nlp_solver_type = 'SQP_RTI'  # or 'SQP'
    ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
    ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
    ocp.solver_options.print_level = 5
    
    # Create solver
    acados_ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
    
    return acados_ocp_solver, N

def main():
    # Create OCP solver
    solver, N = create_ocp_solver()
    
    # Initial guess for control
    u_init = np.zeros((N, 1))
    x_init = np.zeros((N+1, 3))
    
    # Linearly interpolate position for initial guess
    x_init[:, 0] = np.linspace(0.0, 10.0, N+1)
    x_init[:, 2] = 5
    
    # Initialize solver
    for i in range(N):
        solver.set(i, 'x', x_init[i])
        solver.set(i, 'u', u_init[i])
    solver.set(N, 'x', x_init[N])
    
    # Solve OCP
    status = solver.solve()
    
    # Get solution
    x_opt = np.zeros((N+1, 3))
    u_opt = np.zeros((N, 1))
    
    for i in range(N):
        x_opt[i] = solver.get(i, 'x')
        u_opt[i] = solver.get(i, 'u')
    x_opt[N] = solver.get(N, 'x')
    
    print(f"Solver status: {status}")
    print(f"Optimal states \n{x_opt}")
    print(f"Optimal controls \n{u_opt}")

if __name__ == '__main__':
    main()

The error which I am getting is status 4. I am not sure how to fix it. I have a same code in casadi which I solve using ipopt and it works perfectly.

Thanks in advance!

2

Hi :waving_hand:

the lower bound on T should be strictly larger than zero. For T=0 the integration will likely fail.
Also please use SQP instead of SQP_RTI (which solves only a single QP approximation of the problem).
As the Hessian of your problem is not positive definite I would also recommend to add a Levenberg marquardt term or try a regularization method, e.g. GERSHGORIN_LEVENBERG_MARQUARDT.

Hope this is helpful!

Best, Katrin

3

Hi Kaethe,

Thank you for you reply.

I tried the changes you recommended but I am still getting status 4 error but this time it gives me some values before giving error.
What I have noticed is that initial guess is affecting the optimization a lot which is expected, but for some of the cases my dynamics are breaking, which I have not idea why it is happening. And every time, the optimal time value is coming to be 1.

I have attached my code below.

Thank you once again for your help.

Regards,
Yash

import matplotlib.pyplot as plt
import scipy.linalg
from acados_template import AcadosOcp, AcadosOcpSolver
from acados_template import AcadosModel
import casadi as ca

def export_double_integrator_model():
    """
    Export double integrator model
    """
    model_name = 'double_integrator'
    
    # States
    x = ca.MX.sym('x')  # position
    v = ca.MX.sym('v')  # velocity
    T = ca.MX.sym('T')  # free final time
    x = ca.vertcat(x, v, T)
    
    x_dot = ca.MX.sym('xdot', 3)  # state derivative
    
    # Controls
    u = ca.MX.sym('u')  # acceleration
    
    # Dynamics
    x_dot_expr = ca.vertcat(v, u, 0)
    f_expr = x_dot_expr * (T-1)
    
    # Model
    model = AcadosModel()
    model.name = model_name
    model.x = x
    model.u = u
    model.xdot = x_dot
    model.f_expl_expr = f_expr  # continuous time dynamics
    
    return model

def create_ocp_solver():
    # Create model
    model = export_double_integrator_model()
    
    # Create OCP
    ocp = AcadosOcp()
    
    # Set model
    ocp.model = model
    
    # Parameters
    N = 50
    
    # Set time horizon
    ocp.dims.N = N
    ocp.solver_options.tf = 1
    
    # Set cost
    ocp.cost.cost_type = 'EXTERNAL'
    ocp.cost.cost_type_e = 'EXTERNAL'
    
    # In acados, for minimum time, we can simply put the cost objective as 1.0 (the time step)
    # and then scale the dynamics by T
    x = ocp.model.x
    u = ocp.model.u
    
    Q = np.diag([0.0, 0.0, 1])
    ocp.model.cost_expr_ext_cost = ca.mtimes([x.T, Q, x]) +  200*u**2
    ocp.model.cost_expr_ext_cost_e = ca.mtimes([x.T, Q, x])
    
    # Initial state
    initial_time = 0.1
    x0 = np.array([0.0, 0.0, 1])  # CHANGE 1: Set initial T to 1.0 instead of 0
    ocp.constraints.x0 = x0
    
    # Terminal constraint
    ocp.constraints.idxbx = np.array([0, 1, 2])
    ocp.constraints.lbx = np.array([-15, -5, 1])  # CHANGE 2: Lower bound on T is 0.1 > 0
    ocp.constraints.ubx = np.array([15.0, 5.0, 200.0])
    
    # Terminal constraint # Final state
    ocp.constraints.idxbx_e = np.array([0, 1, 2])
    xf_0 = np.array([10.0, 0.0, 1])  # CHANGE 3: Lower bound on final T is 0.1 > 0
    xf_f = np.array([10.0, 0.0, 100])
    ocp.constraints.lbx_e = xf_0
    ocp.constraints.ubx_e = xf_f
    
    # Control constraints
    u_max = 1.0
    ocp.constraints.idxbu = np.array([0])
    ocp.constraints.lbu = np.array([-u_max])
    ocp.constraints.ubu = np.array([u_max])
    
    # Solver options
    ocp.solver_options.integrator_type = 'ERK'
    ocp.solver_options.nlp_solver_type = 'SQP'  # CHANGE 4: Use 'SQP' instead of 'SQP_RTI'
    ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
    ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
    ocp.solver_options.regularize_method = 'GERSHGORIN_LEVENBERG_MARQUARDT'  # CHANGE 5: Add regularization
    ocp.solver_options.levenberg_marquardt = 1e-2  # CHANGE 6: Set regularization parameter
    ocp.solver_options.print_level = 5
    
    # Create solver
    acados_ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
    
    return acados_ocp_solver, N

def main():
    # Create OCP solver
    solver, N = create_ocp_solver()
    
    # Initial guess for control
    u_init = np.zeros((N, 1))
    x_init = np.zeros((N+1, 3))
    
    # Linearly interpolate position for initial guess
    x_init[:, 0] = np.linspace(0.0, 10.0, N+1)
    x_init[:, 1] = 1
    x_init[:, 2] = 10  # Initial guess for T is 5
    
    # Initialize solver
    for i in range(N):
        solver.set(i, 'x', x_init[i])
        solver.set(i, 'u', u_init[i])
    solver.set(N, 'x', x_init[N])
    
    # Solve OCP
    status = solver.solve()
    
    # Get solution
    x_opt = np.zeros((N+1, 3))
    u_opt = np.zeros((N, 1))
    
    for i in range(N):
        x_opt[i] = solver.get(i, 'x')
        u_opt[i] = solver.get(i, 'u')
    x_opt[N] = solver.get(N, 'x')
    
    print(f"Solver status: {status}")
    print(f"Optimal states \n{x_opt}")
    print(f"Optimal controls \n{u_opt}")
    
    # Display the optimal time
    print(f"Optimal final time T: {x_opt[N, 2]}")
    
    # Plot results
    plt.figure(figsize=(12, 8))
    
    # Position
    plt.subplot(3, 1, 1)
    plt.plot(np.linspace(0, x_opt[N, 2], N+1), x_opt[:, 0])
    plt.ylabel('Position')
    plt.grid(True)
    
    # Velocity
    plt.subplot(3, 1, 2)
    plt.plot(np.linspace(0, x_opt[N, 2], N+1), x_opt[:, 1])
    plt.ylabel('Velocity')
    plt.grid(True)
    
    # Control
    plt.subplot(3, 1, 3)
    plt.step(np.linspace(0, x_opt[N, 2], N), u_opt)
    plt.xlabel('Time')
    plt.ylabel('Control (acceleration)')
    plt.grid(True)
    
    plt.tight_layout()
    plt.savefig('double_integrator_minimum_time.png')
    plt.show()

if __name__ == '__main__':
    main()
4

Hi Yash,

you made a mistake defining the constraints. You cannot use constraints.x0 as the initial value of T should be free (by setting constraints.x0 you define an equality constraint for x0).
I would also recommend to use an implicit integrator whenever using a time transformation.

Regarding regularization: If you use regularize_method = 'GERSHGORIN_LEVENBERG_MARQUARDT', there is no need to add an extra Levenberg Marquardt term.

The following code works for me:

def export_double_integrator_model():
    """
    Export double integrator model
    """
    model_name = 'double_integrator'

    # States
    x = ca.MX.sym('x')  # position
    v = ca.MX.sym('v')  # velocity
    T = ca.MX.sym('T')  # free final time
    x = ca.vertcat(x, v, T)

    x_dot = ca.MX.sym('xdot', 3)  # state derivative

    # Controls
    u = ca.MX.sym('u')  # acceleration

    # Dynamics
    x_dot_expr = ca.vertcat(v, u, 0)
    f_expr = x_dot_expr * (T-1)

    # Model
    model = AcadosModel()
    model.name = model_name
    model.x = x
    model.u = u
    model.xdot = x_dot
    model.f_expl_expr = f_expr  # continuous time dynamics
    model.f_impl_expr = f_expr - x_dot

    return model

def create_ocp_solver():
    # Create model
    model = export_double_integrator_model()

    # Create OCP
    ocp = AcadosOcp()

    # Set model
    ocp.model = model

    # Parameters
    N = 50

    # Set time horizon
    ocp.dims.N = N
    ocp.solver_options.tf = 1

    # Set cost
    ocp.cost.cost_type = 'EXTERNAL'
    ocp.cost.cost_type_e = 'EXTERNAL'

    # In acados, for minimum time, we can simply put the cost objective as 1.0 (the time step)
    # and then scale the dynamics by T
    x = ocp.model.x
    u = ocp.model.u

    Q = np.diag([0.0, 0.0, 1])
    ocp.model.cost_expr_ext_cost = ca.mtimes([x.T, Q, x]) +  200*u**2
    ocp.model.cost_expr_ext_cost_e = ca.mtimes([x.T, Q, x])

    # Initial state

    # initial constraint
    ocp.constraints.idxbx_0 = np.array([0, 1, 2])
    ocp.constraints.lbx_0 = np.array([0., 0., .1])  # CHANGE 2: Lower bound on T is 0.1 > 0
    ocp.constraints.ubx_0 = np.array([0., 0., 200.0])

    # path constraints
    ocp.constraints.idxbx = np.array([0, 1, 2])
    ocp.constraints.lbx = np.array([-15, -5, .1])  # CHANGE 2: Lower bound on T is 0.1 > 0
    ocp.constraints.ubx = np.array([15.0, 5.0, 200.0])

    # Terminal constraint # Final state
    ocp.constraints.idxbx_e = np.array([0, 1, 2])
    xf_0 = np.array([10.0, 0.0, 1])  # CHANGE 3: Lower bound on final T is 0.1 > 0
    xf_f = np.array([10.0, 0.0, 100])
    ocp.constraints.lbx_e = xf_0
    ocp.constraints.ubx_e = xf_f

    # Control constraints
    u_max = 1.0
    ocp.constraints.idxbu = np.array([0])
    ocp.constraints.lbu = np.array([-u_max])
    ocp.constraints.ubu = np.array([u_max])

    # Solver options
    ocp.solver_options.integrator_type = 'IRK'
    ocp.solver_options.nlp_solver_type = 'SQP'  # CHANGE 4: Use 'SQP' instead of 'SQP_RTI'
    ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
    ocp.solver_options.hessian_approx = 'EXACT'
    ocp.solver_options.regularize_method = 'GERSHGORIN_LEVENBERG_MARQUARDT'  # CHANGE 5: Add regularization
    # ocp.solver_options.levenberg_marquardt = 1e-2  # CHANGE 6: Set regularization parameter
    ocp.solver_options.print_level = 1
    ocp.solver_options.globalization = 'MERIT_BACKTRACKING'

    # Create solver
    acados_ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')

    return acados_ocp_solver, N
1 Like
5

Thanks Kaethe so much, I realize my mistake now. :slight_smile:

1 Like