# Final Solution not Converging

Hi

I am trying to write an acados numerical solver for Kinematic Model of a mobile robot in Python.
The task is pretty straight forward, I have 4 state variables [x,y,v,theta] and 2 control variables [a,w] (acceleration and angular velocity) and the robot needs start at (0,0) and finish at (5,5), initial and final velocity and orientation angle should be 0.

Solver:

def desired_trajectory(N):
# Define parameters
initial_position = (0, 0)
final_position = (5, 5)

# Linear interpolation for positions
x = np.linspace(initial_position[0], final_position[0], N+1)
y = np.linspace(initial_position[1], final_position[1], N+1)

print(x[N], y[N])

# Calculate v and theta for each pair of x and y
v = 0.3 * np.ones(31)
theta = np.arctan2(y, x)
v[0] = 0
v[N] = 0
theta[N] = 0

# Combine into p
p = np.vstack((x, y, v, theta)).T

a = np.zeros(len(x))  # Acceleration
w = np.zeros(len(x))  # Angular velocity

# Combine control variables into matrix r
r = np.vstack((a, w)).T

return p, r

def create_ocp_solver():
"""
Create Acados solver for trajectory optimization.
"""

# Set up the optimization problem
model = mobile_robot_model()
ocp.model = model

# --------------------PARAMETERS--------------
# constants
nx = model.x.size()[0]
nu = model.u.size()[0]
N = 30  # Prediction horizon (works for N =200)

# Setting initial conditions
ocp.dims.N = N
ocp.dims.nx = nx
ocp.dims.nu = nu

# Set initial condition for the robot
ocp.constraints.x0 = np.array([0, 0, 0, 0])

# ---------------------CONSTRAINTS------------------
# Define constraints on states and control inputs
ocp.constraints.idxbu = np.array([0, 1])  # indices 0 & 1 of u
ocp.constraints.idxbx = np.array([0, 1, 2, 3])  # indices 0...3 of x
ocp.constraints.lbu = np.array([-0.1, -0.3])  # Lower bounds on control inputs
ocp.constraints.ubu = np.array([0.1, 0.3])  # Upper bounds on control inputs
ocp.constraints.lbx = np.array([-100, -100, 0, -10])  # Lower bounds on states
ocp.constraints.ubx = np.array([100, 100, 1, 10])  # Upper bounds on states
# ---------------------COSTS--------------------------
# Set up the cost function
ocp.cost.cost_type = "NONLINEAR_LS"
ocp.cost.cost_type_e = "NONLINEAR_LS"

X = ocp.model.x
U = ocp.model.u

ocp.model.cost_y_expr = ca.vertcat(X, U)
ocp.model.cost_y_expr_e = X

ocp.cost.yref = np.zeros(nx + nu)
ocp.cost.yref_e = np.zeros(nx)

W_x = np.array([5, 5, 0.05, 0.005, 0.0001, 0.0001])
W = np.diag(W_x)
W_xe = np.array([300, 300, 0.01, 0.01])
W_e = np.diag(W_xe)

ocp.cost.W = W  # State weights
ocp.cost.W_e = W_e  # Terminal state weights

# ---------------------SOLVER-------------------------
ocp.solver_options.tf = 25
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.qp_solver_cond_N = 10
ocp.solver_options.nlp_solver_type = 'SQP'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.levenberg_marquardt = 3.0
ocp.solver_options.nlp_solver_max_iter = 15
ocp.solver_options.qp_solver_iter_max = 100
ocp.solver_options.nlp_solver_tol_stat = 1e2
ocp.solver_options.nlp_solver_tol_eq = 1e-1

I tried playing around with different configuration for the solve but nothing seems to work properly. For higher ocp.solver_options.tf, final solution is closer to the optimal solution but ideally it should work for ocp.solver_options.tf = 17 since max velocity is 0.3.

Driver code:

# Create optimal control problem solver
ocp, solver = create_ocp_solver()

nx = ocp.model.x.size()[0]
nu = ocp.model.u.size()[0]
N = ocp.dims.N

# Define target and obstacle positions
x_target, y_target = 5, 5
x_obst1, y_obst1 = 3.5, 3.5
x_obst2, y_obst2 = 1.1, 0.6
v_max = 0.3

# ----------------INITIALIZE-------------------
# Set initial state and parameter values

# Get desired trajectory
P, R = desired_trajectory(N)

# Initialize Optimal Trajectory
x_opt = np.zeros((ocp.dims.N + 1, nx))
u_opt = np.zeros((ocp.dims.N, nu))

# Set yref and initial state for each stage in the prediction horizon
for i in range(N):
yref = np.concatenate((P[i], R[i]))
solver.set(i, 'x', np.transpose(yref[:4]))
x = solver.get(i, "x")
print(x[0], ", ", x[1], ",", x[2], ",", x[3])
solver.set(i, 'u', np.array([0.0, 0.0]))
solver.set(i, "yref", yref)
solver.set(0, 'lbx', np.array([0, 0, 0, 0]))
solver.set(0, 'ubx', np.array([0, 0, 0, 0]))
# Set terminal state and terminal cost
solver.set(N, 'x', np.transpose(P[N]))
ocp.cost.yref_e = P[N]

start = timeit.default_timer()

# ---------------------SOLVE-----------------

# Solve optimal control problem
status = solver.solve()
if status != 0:
raise Exception("Solver failed!")
time_record = timeit.default_timer() - start

for i in range(ocp.dims.N+1):
x_opt[i, :] = solver.get(i, "x")
x = solver.get(i, "x")
print(x[0], ", ", x[1], ",", x[2], ",", x[3])
if i < ocp.dims.N:
u_opt[i, :] = solver.get(i, "u")

Output:

Update:

For future reference in case someone has a similar problem.
I managed to fix the issue by setting the terminal cost properly.
i.e. I had to replace this line in the drive code

ocp.cost.yref_e = P[N]

with this line:

solver.set(N, 'yref', P[-1])

So now my final solution is mostly convergent: