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!