Acados multiphase python with different constraint functions per phase

Hi
I am trying to use the acados python interface to formulate a multiphase optimal control problem where in each phase, there is a different set of nonlinear constraints. I figured I’d mention that the constraints have a different dimension in each phase as well. I pasted a simple double integrator version of the problem I’m trying to solve below, where in the second phase, the double integrator’s velocity is constrained to be nonpositive.

Here is a function that exports the double integrator model, in a file called “acados_double_integrator_model.py”.

``````from acados_template import AcadosModel
from casadi import SX, vertcat, sin, cos

model_name = 'double_integrator_disc_dyn'

# set up states & controls
q = SX.sym('q', dim_q)
v = SX.sym('v', dim_q)

x = vertcat(q, v)

u = SX.sym('u', dim_q)

vnext = v + u*dt
qnext = q + vnext*dt

model.disc_dyn_expr = vertcat(qnext, vnext)
model.x = x
model.u = u
model.name = model_name

return model
``````

The script that imports the above function is called “double_integrator_multiphase_example.py”, pasted below:

``````from acados_template import AcadosOcp, AcadosOcpSolver, AcadosMultiphaseOcp
import numpy as np

inf = 1e15

def main():
# Horizon definition
N = 20 # Number of time intervals
Tf = 1.0 # Duration
dt = Tf/N # Time step

# Dimension of double integrator configuration and velocity
nq = 1
nv = 1

# State and control dimensions
nx = nq + nv
nu = nv

# Initial state
x0 = np.concatenate((np.ones(nq), 0.25*np.ones(nv)))

# Cost matrices
Q_mat = np.diag(np.concatenate((np.ones(nq), 10*np.ones(nv))))
R_mat = np.diag(10*np.ones(nu))

# Number of intervals in each phase
N_list = [10, 10]
# Multiphase ocp

#### PHASE 0: velocity can take any value ####

# Dynamics
phase_idx = 0

ocp.dims.N = N_list[phase_idx]

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

# Quadratic state and control cost

# Control limits
Fmax = 1
ocp.constraints.lbu = -Fmax*np.ones(nu)
ocp.constraints.ubu = Fmax*np.ones(nu)
ocp.constraints.idxbu = np.arange(nu)

# Initial state constraint
ocp.constraints.x0 = x0

multiphase_ocp.set_phase(ocp, phase_idx)

#### PHASE 1: velocity must be nonpositive ####
phase_idx += 1

# Dynamics

# Nonlinear constraint: velocity must be nonpositive (yes, this is actually
# a linear constraint, but we're enforcing it using the acados nonlinear constraint interface)
h_lb = -inf*np.ones(nv)
h_ub = np.zeros(nv)

ocp.dims.N = N_list[phase_idx]

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

# Quadratic state and control cost

# Control limits
Fmax = 1
ocp.constraints.lbu = -Fmax*np.ones(nu)
ocp.constraints.ubu = Fmax*np.ones(nu)
ocp.constraints.idxbu = np.arange(nu)

# Set nonlinear constraint sizes and bounds
ocp.dims.nh = h_expr.shape[0]
ocp.constraints.lh = h_lb
ocp.constraints.uh = h_ub

multiphase_ocp.set_phase(ocp, phase_idx)

# Set options
multiphase_ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
multiphase_ocp.solver_options.qp_solver_cond_N = N
multiphase_ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
multiphase_ocp.solver_options.nlp_solver_type = 'SQP'
multiphase_ocp.solver_options.tf = Tf
multiphase_ocp.solver_options.nlp_solver_tol_eq = 1e-4
multiphase_ocp.solver_options.nlp_solver_tol_ineq = 1e-4
multiphase_ocp.mocp_opts.integrator_type = ['DISCRETE', 'DISCRETE']

status = ocp_solver.solve()
ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics")

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

When I run the script (i.e. using

``````python3 double_integrator_multiphase_example.py
``````

in a terminal, I get the following errors:

``````acados_solver_multiphase_ocp.c: In function ‘multiphase_ocp_acados_create_5_set_nlp_in’:
acados_solver_multiphase_ocp.c:769:49: error: ‘multiphase_ocp_solver_capsule’ {aka ‘struct multiphase_ocp_solver_capsule’} has no member named ‘nl_constr_h_fun_jac’; did you mean ‘nl_constr_h_fun_jac_1’?
769 |                                       &capsule->nl_constr_h_fun_jac[i-1]);
|                                                 ^~~~~~~~~~~~~~~~~~~
|                                                 nl_constr_h_fun_jac_1
acados_solver_multiphase_ocp.c:771:49: error: ‘multiphase_ocp_solver_capsule’ {aka ‘struct multiphase_ocp_solver_capsule’} has no member named ‘nl_constr_h_fun’; did you mean ‘nl_constr_h_fun_1’?
771 |                                       &capsule->nl_constr_h_fun[i-1]);
|                                                 ^~~~~~~~~~~~~~~
|                                                 nl_constr_h_fun_1

``````

I tried manually changing line 769 in acados_solver_multiphase_ocp.c to say `capsule->nl_constr_h_fun_jac_1` instead of `capsule->nl_constr_h_fun_jac`. Similarly, I changed line 771 in acados_solver_multiphase_ocp.c to say `capsule->nl_constr_h_fun_1` instead of `capsule->nl_constr_h_fun`. Then I ran `make` in the c_generated_code directory, and tried running the generated main_multiphase_ocp binary file, i.e. using `./main_multiphase_ocp`. Doing this gave me a `Segmentation fault (core dumped)`.

Ideas on what to do here?

Hi,
thanks for setting up the minimal example.
I fixed the issue here: Multiphase nonlinear constraints by FreyJo · Pull Request #1023 · acados/acados · GitHub
I also added the example to acados to have a test for this.
Is that alright for you?

Best,
Jonathan

1 Like