# QP solver error with nonlinear ls cost and nonlinear constraints

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.

The full code:

#!/usr/bin/env python3
# coding: utf-8

from pprint import pformat, pprint
from typing import Callable, Tuple, Union

import numpy as np
from matplotlib import pyplot as plt
from scipy import linalg as spl

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
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(
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)

solver_stats = {}
print("solver_stats:")
for field in [
"statistics",
"time_tot",
"time_lin",
"time_sim",
"time_sim_la",
"time_qp",
"time_qp_solver_call",
"time_reg",
"sqp_iter",
"residuals",
"qp_iter",
"alpha",
]:
try:
solver_stats[field] = solver.get_stats(field)
except Exception as e:
solver_stats[field] = None
pprint(solver_stats)

def get_solution(
) -> 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(
):
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(
):
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)

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())


The unit velocity requirement might be a use case for algebraic constraints and implicit dynamics, but I’m not sure how to implement it.

The problem formulation suggests that f_impl maps to n_x + n_z dimensions, where n_z is the number of algebraic state variables.
However, I don’t see what my algebraic state variable is, or how I can constrain it.