Hi,
I’m wondering what I’m doing wrong when using nonlinear constraints and nonlinear ls cost. I’m getting the following error:
QP solver returned error status 3 in SQP iteration 1, QP iteration 7.
Statistics:
iter res_stat res_eq res_ineq res_comp qp_stat qp_iter alpha
0 1.000000e-03 1.000000e-03 1.000000e+10 0.000000e+00 0 0 0.000000e+00
1 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 3 7 0.000000e+00
I’m using the Python interface on Python 3.8.
My goal is to create a solver for a generic curve.
The curve should be drawn from cartesian positions (in 2d), that have a constant “speed”, and a maximum curvature.
To achieve this I have created a model with the states x and y position, x and y unit velocity, and a speed that is constant, but that should be up to the solver to set.
I want the unit speed requirement to be held up by nonlinear path constraints. Additionally, I calculate curvature, and I set a maximum curvature requirement as path constraints. The full code is in the bottom of this post.
I’m wondering if I’m not setting all the right fields in the ocp/cost/model structs, but it’s hard to debug, so I’m hoping someone here has a clue.
Thanks very much in advance!
The full code:
#!/usr/bin/env python3
# coding: utf-8
from pprint import pformat, pprint
from typing import Callable, Tuple, Union
import acados_template as ac
import casadi as ca
import numpy as np
from matplotlib import pyplot as plt
from scipy import linalg as spl
ocp = ac.AcadosOcp()
model = ocp.model
cost = ocp.cost
constraints = ocp.constraints
dims = ocp.dims
solver_options = ocp.solver_options
# States
s_position_x = ca.SX.sym("px")
s_position_y = ca.SX.sym("py")
s_unit_velocity_x = ca.SX.sym("uvx")
s_unit_velocity_y = ca.SX.sym("uvy")
s_speed = ca.SX.sym("U")
# Inputs
i_acceleration_x = ca.SX.sym("ax")
i_acceleration_y = ca.SX.sym("ay")
# Calculated sizes
c_velocity_x = s_speed * s_unit_velocity_x
c_velocity_y = s_speed * s_unit_velocity_y
c_curvature = (
c_velocity_x * i_acceleration_y - c_velocity_y * i_acceleration_x
) / ((c_velocity_x**2 + c_velocity_y**2) ** (3.0 / 2.0))
c_unit_speed_squared = s_unit_velocity_x**2 + s_unit_velocity_y**2
# Dynamics
d_position_x_dot = c_velocity_x
d_position_y_dot = c_velocity_y
d_unit_velocity_x_dot = i_acceleration_x / s_speed
d_unit_velocity_y_dot = i_acceleration_y / s_speed
d_speed_dot = 0.0
# Concatenation
states = ca.vertcat(
s_position_x,
s_position_y,
s_unit_velocity_x,
s_unit_velocity_y,
s_speed,
)
inputs = ca.vertcat(i_acceleration_x, i_acceleration_y)
states_dot = ca.vertcat(
d_position_x_dot,
d_position_y_dot,
d_unit_velocity_x_dot,
d_unit_velocity_y_dot,
d_speed_dot,
)
# Assign to model
model_name = "a_curve"
model.name = model_name
model.x = states
model.u = inputs
model.f_expl_expr = states_dot
model.p = []
# Cost
cost.cost_type = "NONLINEAR_LS"
model.cost_y_expr = ca.vertcat(s_speed, c_curvature)
cost.yref = np.array((0.0, 0.0))
cost.W = np.diag((1.0, 1.0))
debug_lower_speed_limit = 0.1
# States
# Constraints (along trajectory)
constraints.idxbx = np.array((2, 3, 4)) # unit vel x, unit vel y, speed
constraints.lbx = np.array(
(
-1.0, # unit vel x
-1.0, # unit vel y
debug_lower_speed_limit, # speed
)
)
constraints.ubx = np.array(
(
1.0, # unit vel x
1.0, # unit vel y
1.0e10, # speed
)
)
# Initial constraints
constraints.idxbx_0 = np.array((0, 1, 2, 3, 4))
constraints.lbx_0 = np.array(
(
0.0, # pos x
0.0, # pos y
1.0, # unit vel x
0.0, # unit vel x
debug_lower_speed_limit, # speed (range)
)
)
constraints.ubx_0 = np.array(
(
0.0, # pos x
0.0, # pos y
1.0, # unit vel x
0.0, # unit vel x
1.0e10, # speed (range)
)
)
# Final constraints
constraints.idxbx_e = np.array((0, 1, 2, 3, 4))
constraints.lbx_e = np.array(
(
100.0, # pos x
100.0, # pos y
0.0, # unit vel x
1.0, # unit vel y
debug_lower_speed_limit, # speed (range)
)
)
constraints.ubx_e = np.array(
(
100.0, # pos x
100.0, # pos y
0.0, # unit vel x
1.0, # unit vel x
1.0e10, # speed (range)
)
)
# constraints.idxbu = np.array((0,1))
# constraints.lbu = np.array((-1.e15,-1.e15))
# constraints.ubu = np.array((1.e15,1.e15))
# Path constraints
turn_radius_min = 20.0
curvature_max = 1 / turn_radius_min
model.con_h_expr = ca.vertcat(c_curvature, c_unit_speed_squared)
constraints.lh = np.array((-curvature_max, 1.0))
constraints.uh = np.array((curvature_max, 1.0))
dims.N = 100
solver_options.tf = (
1.0 # Final time 1, requires high speed, but speed is not really speed
)
solver_options.nlp_solver_type = "SQP"
solver_options.nlp_solver_max_iter = 1000
ocp.code_export_directory = "./generated_" + model_name
ocp_solver = ac.AcadosOcpSolver(ocp, json_file=model_name + ".json")
# Set least squares cost reference
def set_yref(
solver: ac.AcadosOcpSolver,
f: Callable[[float], Union[float, np.ndarray]],
t0,
) -> None:
delta_t = solver.solver_options["Tsim"]
n = solver.N
for i in range(n):
t = t0 + i * delta_t
yref = f(t)
solver.cost_set(i, "yref", yref)
def f_yref(t: float) -> np.ndarray:
return np.array((0.0, 0.0))
set_yref(ocp_solver, f_yref, 0.0)
def print_stats(solver: ac.AcadosOcpSolver):
solver_stats = {}
print("solver_stats:")
for field in [
"statistics",
"time_tot",
"time_lin",
"time_sim",
"time_sim_ad",
"time_sim_la",
"time_qp",
"time_qp_solver_call",
"time_reg",
"sqp_iter",
"residuals",
"qp_iter",
"alpha",
]:
# noinspection PyBroadException
try:
solver_stats[field] = solver.get_stats(field)
except Exception as e:
solver_stats[field] = None
pprint(solver_stats)
def get_solution(
solver: ac.AcadosOcpSolver,
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
delta_t = solver.solver_options["Tsim"]
n = solver.N
nx = len(solver.get(0, "x"))
nu = len(solver.get(0, "u"))
times = np.linspace(0, delta_t * (n - 1), n)
states = np.full((nx, n), np.nan)
inputs = np.full((nu, n), np.nan)
for idx in range(n):
# print(idx)
states.T[idx] = solver.get(idx, "x")
inputs.T[idx] = solver.get(idx, "u")
return times, states, inputs
def set_initial_state(
solver: ac.AcadosOcpSolver, x: float, y: float, head: float
):
unit_velocity_x = np.cos(head)
unit_velocity_y = np.sin(head)
lbx = np.array(
(x, y, unit_velocity_x, unit_velocity_y, debug_lower_speed_limit)
)
ubx = np.array((x, y, unit_velocity_x, unit_velocity_y, 1.0e10))
solver.constraints_set(0, "lbx", lbx)
solver.constraints_set(0, "ubx", ubx)
def set_final_state(
solver: ac.AcadosOcpSolver, x: float, y: float, head: float
):
unit_velocity_x = np.cos(head)
unit_velocity_y = np.sin(head)
lbx = np.array(
(x, y, unit_velocity_x, unit_velocity_y, debug_lower_speed_limit)
)
ubx = np.array((x, y, unit_velocity_x, unit_velocity_y, 1.0e10))
solver.constraints_set(solver.N, "lbx", lbx)
solver.constraints_set(solver.N, "ubx", ubx)
set_initial_state(ocp_solver, 0.0, 0.0, float(np.radians(0.0)))
set_final_state(ocp_solver, 100.0, 100.0, float(np.radians(90.0)))
solver_status = ocp_solver.solve()
if solver_status != 0:
ocp_solver.print_statistics()
raise RuntimeError(f"ERROR: solve() returned {solver_status}.")
print_stats(ocp_solver)
sol_times, sol_states, sol_inputs = get_solution(ocp_solver)
# sol_times
sol_x, sol_y, sol_theta, sol_delta, sol_v = sol_states
# sol_states
sol_phi, sol_a = sol_inputs
# sol_inputs
_, ax = plt.subplots()
ax.plot(sol_y, sol_x)
ax.axis("equal")
plt.show()
_, ax = plt.subplots(5, 1, sharex="all", figsize=(7, 12))
ax[0].plot(sol_times, np.degrees(sol_theta), label=r"$\theta$")
ax[0].legend()
ax[1].plot(sol_times, np.degrees(sol_delta), label=r"$\delta$")
ax[1].legend()
ax[2].plot(sol_times, sol_v, label=r"$v$")
ax[2].legend()
ax[3].plot(sol_times, np.degrees(sol_phi), label=r"$\phi$")
ax[3].legend()
ax[4].plot(sol_times, sol_a, label=r"$a$")
ax[4].legend()
ax[-1].set_xlabel(r"$t$")
plt.show()
pprint(ocp_solver.get_cost())