Debugging 2000 -> Problem solution failed (solver error) #301
Comments
|
There should be an error code. Does the error happen when you call the solver? I tried to run your code, but I'm not sure how to call the function |
|
If you're getting an error 2000, this means that the solver failed, while trying to solve the problem. This typically means that a NaN was encountered. Unless you have a reason to use the TCP interface, you can use the direct interface. Try to enable the preconditioning and see if it helps. |
|
I will try to use the direct interface to debug although I do find there is a significant performance difference. TCP interface is faster that the direct interface. |
|
I could come with a sample file that you can run. The problem can be solved by IPT (included in the source code) but gives an solver error using panoc. I tried relaxing a few thresholds but no luck so far. Instructions on running the code
The python program will first try to solve using IPOPT, and then would generate the problem for panoc and error out with a solver error. import math
import random
import csv
import casadi as cs
import opengen as og
import numpy as np
# IPOPT Options
opts = {}
opts["ipopt.print_level"] = 0
opts["ipopt.sb"] = "yes"
opts["print_time"] = 0
def runga_kutta_4th(f, xk, uk, dt):
k1 = f(xk, uk)
k2 = f(xk + dt/2*k1, uk)
k3 = f(xk + dt/2*k2, uk)
k4 = f(xk + dt*k3, uk)
return dt/6*(k1+2*k2+2*k3+k4)
def generate_nlp_problem(n_states, n_inputs, n_desired, n_horizon, Ts, path_length, x_lut, y_lut):
initial_state = cs.MX.sym("x0", n_states)
desired_state = cs.MX.sym("xd", n_desired)
c1, c2 = -0.65, 0.65
r1, r2 = 0.975, 1.0
obstacle = (10.050, 3.705140)
Q = cs.diagcat(100.0, 10000000.0, 1000.0)
R = cs.diagcat(50, 50, 50.0)
l_f = 0.75
l_r = 0.75
l = 1.5
states_lb = np.array([-10000000.0, -10000000.0, -np.pi, -30, -0.1, -3])
states_ub = np.array([10000000.0, 10000000.0, np.pi, 30, path_length, 100000.0])
inputs_lb = np.array([-1, -0.7, -2])
inputs_ub = np.array([+10, 0.7, 2])
x, u = cs.MX.sym("x", n_states), cs.MX.sym("u", n_inputs)
p_x, p_y, p_yaw, v, p_s, p_vs = cs.vertsplit(x)
a, delta, a_s = cs.vertsplit(u)
beta = cs.atan(l_r / (l_f + l_r) * cs.tan(delta))
dynamics = cs.vertcat(
v * cs.cos(beta + p_yaw),
v * cs.sin(beta + p_yaw),
v * cs.tan(delta) * cs.cos(beta) / (l_f + l_r),
a,
p_vs,
a_s
)
f = cs.Function("f", [x, u], [dynamics])
X, U = cs.MX.sym("X", n_states, n_horizon), cs.MX.sym("U", n_inputs, n_horizon)
X0X = cs.horzcat(initial_state, X)
objective = 0
dynamics_constr = []
collision_constr = []
for k in range(0, n_horizon - 1):
xk, uk = X0X[:, k], U[:, k]
xkp1 = X0X[:, k + 1]
x_s = x_lut(xk[4])
y_s = y_lut(xk[4])
dy_x = x_lut.jacobian()(xk[4], x_s)
dy_y = y_lut.jacobian()(xk[4], y_s)
yaw = cs.atan2(dy_y, dy_x)
# Contouring Error
contour_error = cs.sin(yaw)*(xk[0] - x_s) - cs.cos(yaw)*(xk[1] - y_s)
# Lag Error
lag_error = -cs.cos(yaw)*(xk[0] - x_s) - cs.sin(yaw)*(xk[1] - y_s)
# Velocity Error
velocity_error = xk[3] - desired_state[0]
err = cs.vertcat(contour_error, lag_error, velocity_error)
if k == 0:
a_delta = uk[0] - desired_state[1]
delta_delta = uk[1] - desired_state[2]
a_s_delta = uk[2] - desired_state[3]
u_delta = cs.vertcat(a_delta, delta_delta, a_s_delta)
else:
u_delta = uk - u_previous
objective += err.T @ Q @ err + u_delta.T @ R @ u_delta
next_euler = (xk + runga_kutta_4th(f, xk, uk, Ts))
dynamics_constr = cs.vertcat(dynamics_constr, xkp1 - next_euler)
collision = r1**2 - (((xk[0] + c1*cs.cos(xk[2])) - obstacle[0])**2 + ((xk[1] + c1*cs.sin(xk[2])) + obstacle[1])**2)
collision_constr = cs.vertcat(collision_constr, collision)
collision = r2**2 - (((xk[0] + c2*cs.cos(xk[2])) - obstacle[0])**2 + ((xk[1] + c2*cs.sin(xk[2])) - obstacle[1])**2)
collision_constr = cs.vertcat(collision_constr, collision)
u_previous = uk
xN = X0X[:, -1]
x_s = x_lut(xN[4])
y_s = y_lut(xN[4])
dy_x = x_lut.jacobian()(xN[4], x_s)
dy_y = y_lut.jacobian()(xN[4], y_s)
yaw = cs.atan2(dy_y, dy_x)
# Contouring Error
contour_error = (cs.sin(yaw)*(xN[0] - x_s) - cs.cos(yaw)*(xN[1] - y_s))
# Lag Error
lag_error = (-cs.cos(yaw)*(xN[0] - x_s) - cs.sin(yaw)*(xN[1] - y_s))
# Velocity Error
velocity_error = xN[3] - desired_state[0]
err = cs.vertcat(contour_error, lag_error, velocity_error)
objective += 10 * err.T @ Q @ err # Terminal cost
dynamics_constr_lb = np.zeros(((n_horizon - 1) * n_states,))
dynamics_constr_ub = np.zeros(((n_horizon - 1) * n_states,))
collision_constr_lb = -np.inf * np.ones(((n_horizon - 1) * 2,))
collision_constr_ub= np.zeros(((n_horizon - 1) * 2,))
nlp = {
"f": objective,
"g": cs.vertcat(dynamics_constr, collision_constr),
"x": cs.vertcat(
X.reshape((n_horizon * n_states, 1)), U.reshape((n_horizon * n_inputs, 1))
),
"p": cs.vertcat(initial_state, desired_state),
}
bounds = {
"lbx": np.concatenate((np.tile(states_lb, n_horizon), np.tile(inputs_lb, n_horizon))),
"ubx": np.concatenate((np.tile(states_ub, n_horizon), np.tile(inputs_ub, n_horizon))),
"lbg": np.concatenate((dynamics_constr_lb, collision_constr_lb)),
"ubg": np.concatenate((dynamics_constr_ub, collision_constr_ub)),
}
return nlp, bounds
def generate_panoc_problem(n_states, n_inputs, n_desired, n_horizon, Ts, path_length, x_lut, y_lut):
parameters = cs.MX.sym('z', n_states + n_desired)
initial_state = parameters[0:n_states]
desired_state = parameters[n_states:]
Q = cs.diagcat(100.0, 10000000.0, 1000.0)
R = cs.diagcat(50, 50, 50.0)
l_f = 0.75
l_r = 0.75
l = 1.5
states_lb = [-10000000.0, -10000000.0, -np.pi, -30, -0.1, -3]
states_ub = [10000000.0, 10000000.0, np.pi, 30, path_length, 100000.0]
inputs_lb = [-1, -0.7, -2]
inputs_ub = [+10, 0.7, 2]
x, u = cs.MX.sym("x", n_states), cs.MX.sym("u", n_inputs)
p_x, p_y, p_yaw, v, p_s, p_vs = cs.vertsplit(x)
a, delta, a_s = cs.vertsplit(u)
beta = cs.atan(l_r / (l_f + l_r) * cs.tan(delta))
dynamics = cs.vertcat(
v * cs.cos(beta + p_yaw),
v * cs.sin(beta + p_yaw),
v * cs.tan(delta) * cs.cos(beta) / (l_f + l_r),
a,
p_vs,
a_s
)
f = cs.Function("f", [x, u], [dynamics])
variables = cs.MX.sym("vars", n_states*n_horizon + n_inputs*n_horizon)
X = variables[0:n_states*n_horizon]
U = variables[n_states*n_horizon:]
X0X = cs.vertcat(initial_state, X)
objective = 0
dynamics_constr = []
for k in range(0, n_horizon - 1):
xk, uk = X0X[k*n_states:k*n_states + n_states], U[k*n_inputs:k*n_inputs + n_inputs]
xkp1 = X0X[(k+1)*n_states:(k + 1)*n_states + n_states]
x_s = x_lut(xk[4])
y_s = y_lut(xk[4])
dy_x = x_lut.jacobian()(xk[4], x_s)
dy_y = y_lut.jacobian()(xk[4], y_s)
yaw = cs.atan2(dy_y, dy_x)
# Contouring Error
contour_error = cs.sin(yaw)*(xk[0] - x_s) - cs.cos(yaw)*(xk[1] - y_s)
# Lag Error
lag_error = -cs.cos(yaw)*(xk[0] - x_s) - cs.sin(yaw)*(xk[1] - y_s)
# Velocity Error
velocity_error = xk[3] - desired_state[0]
err = cs.vertcat(contour_error, lag_error, velocity_error)
objective += err.T @ Q @ err + uk.T @ R @ uk
next_euler = (xk + runga_kutta_4th(f, xk, uk, Ts))
dynamics_constr = cs.vertcat(dynamics_constr, xkp1 - next_euler)
xN = X0X[-n_states:]
x_s = x_lut(xN[4])
y_s = y_lut(xN[4])
dy_x = x_lut.jacobian()(xN[4], x_s)
dy_y = y_lut.jacobian()(xN[4], y_s)
yaw = cs.atan2(dy_y, dy_x)
# Contouring Error
contour_error = (cs.sin(yaw)*(xN[0] - x_s) - cs.cos(yaw)*(xN[1] - y_s))
# Lag Error
lag_error = (-cs.cos(yaw)*(xN[0] - x_s) - cs.sin(yaw)*(xN[1] - y_s))
# Velocity Error
velocity_error = xN[3] - desired_state[0]
err = cs.vertcat(contour_error, lag_error, velocity_error)
objective += 10 * err.T @ Q @ err # Terminal cost
#c = dynamics_constr #cs.vertcat(dynamics_constr, collision_constr)
#set_c = og.constraints.Zero()
optimization_variables = variables
optimization_parameters = parameters
xmin = states_lb * n_horizon
umin = inputs_lb * n_horizon
xmax = states_ub * n_horizon
umax = inputs_ub * n_horizon
bounds = og.constraints.Rectangle(xmin + umin, xmax + umax)
problem = og.builder.Problem(optimization_variables,
optimization_parameters,
objective) \
.with_constraints(bounds) \
.with_penalty_constraints(dynamics_constr)
tcp_config = og.config.TcpServerConfiguration('0.0.0.0', 9555)
build_config = og.config.BuildConfiguration()\
.with_build_directory("optimizers")\
.with_build_mode("release")\
.with_tcp_interface_config(tcp_config)
meta = og.config.OptimizerMeta()\
.with_optimizer_name("mpcc")
solver_config = og.config.SolverConfiguration()\
.with_tolerance(1e-5)\
.with_preconditioning(True)
builder = og.builder.OpEnOptimizerBuilder(problem,
meta,
build_config,
solver_config)\
.with_verbosity_level(1)
builder.build()
def main(x_lut, y_lut, path_length):
n_states = 6
n_desired = 4
n_inputs = 3
Ts = 0.1
n_horizon = 15
s = 0.0
v_s = 0.0
param = [0.051675815135240555, 3.7084553241729736, -5.326321862684935e-07, 0.00047293867110315835, 0.0016758168110367162, 0.0, 1.5, 0.0, 0.0, 0.0]
acceleration, steering, a_s = 0.0, 0.0, 0.0
# Generate the mpcc problem and solve using IPT
nlp, bounds = generate_nlp_problem(n_states, n_inputs, n_desired, n_horizon, Ts, path_length, x_lut, y_lut)
solver = cs.nlpsol('solver', 'ipopt', nlp, opts)
solution = solver(lbx=bounds['lbx'], ubx=bounds['ubx'], lbg=bounds['lbg'], ubg=bounds['ubg'], p=param)
results = solution['x']
print(results[n_states * n_horizon: n_states*n_horizon + n_inputs])
# Solve using PANOC
generate_panoc_problem(n_states, n_inputs, n_desired, n_horizon, Ts, path_length, x_lut, y_lut)
mpcc_mng = og.tcp.OptimizerTcpManager('optimizers/mpcc')
mpcc_mng.start()
solution = mpcc_mng.call(p=param)
results = solution['solution']
print(results[n_states * n_horizon: n_states*n_horizon + n_inputs])
mng.kill()
if __name__ == '__main__':
# Load the track and convert it into a spline
waypoints_file = '/home/aditya/carla_track.txt'
with open(waypoints_file) as waypoints_file_handle:
waypoints = list(csv.reader(waypoints_file_handle, delimiter=',',
quoting=csv.QUOTE_NONNUMERIC))
waypoints_np = np.array(waypoints)
x_data = [w[0]*10**-2 for w in waypoints]
y_data = [w[1]*10**-2 for w in waypoints]
#y_data = savgol_filter(y_data, 51, 3)
arr = np.zeros(len(x_data))
for i in range(1, len(x_data)):
dx = x_data[i] - x_data[i-1]
dy = y_data[i] - y_data[i-1]
arr[i] = math.hypot(dx, dy)
xy_data = arr
sum = 0.0
arr = np.zeros(len(xy_data))
for i in range(len(xy_data)):
arr[i] = sum + xy_data[i]
sum += xy_data[i]
x_lut = cs.interpolant('LUT', 'bspline', [arr], x_data)
y_lut = cs.interpolant('LUT', 'bspline', [arr], y_data)
path_length = arr[-1]
main(x_lut, y_lut, path_length)The csv file |
|
@alphaville thoughts? |
|
@truncs Sorry for the delay. We're looking at it and we'll try to get back to you by next week. |
|
@alphaville Thanks, much appreciated! |
|
We looked into this and the very first iteration, the algorithm is at the point: and the gradient at that point is [0.046676354372876996, -2.4798313596954067, 2.5714315739064483e-5, 0.0015466545478518734,
NaN, 6.612805104738676e-5, 0.01062739470113399, -1.4560830970755991, 2.0913201202445258e-7,
-0.0004350111800550722, NaN, 1.526672569389828e-14, 0.03095445865853089, 1.9115102157289758e-10,
-3.0761292035423143e-27, -0.00024931715252972196, NaN, 1.526672569389828e-14, 0.03095445865853089,
1.9115102157289758e-10, -3.0761292035423143e-27, -0.00024931715252972196, NaN, 1.526672569389828e-14,
0.03095445865853089, 1.9115102157289758e-10, -3.0761292035423143e-27, -0.00024931715252972196, NaN,
1.526672569389828e-14, 0.03095445865853089, 1.9115102157289758e-10, -3.0761292035423143e-27,
-0.00024931715252972196, NaN, 1.526672569389828e-14, 0.03095445865853089, 1.9115102157289758e-10,
-3.0761292035423143e-27, -0.00024931715252972196, NaN, 1.526672569389828e-14, 0.03095445865853089,
1.9115102157289758e-10, -3.0761292035423143e-27, -0.00024931715252972196, NaN, 1.526672569389828e-14,
0.03095445865853089, 1.9115102157289758e-10, -3.0761292035423143e-27, -0.00024931715252972196, NaN,
1.526672569389828e-14, 0.03095445865853089, 1.9115102157289758e-10, -3.0761292035423143e-27,
-0.00024931715252972196, NaN, 1.526672569389828e-14, 0.03095445865853089, 1.9115102157289758e-10,
-3.0761292035423143e-27, -0.00024931715252972196, NaN, 1.526672569389828e-14, 0.03095445865853089,
1.9115102157289758e-10, -3.0761292035423143e-27, -0.00024931715252972196, NaN, 1.526672569389828e-14,
0.05795881227575218, 1.6676009610635937e-5, 7.742864152452035e-11, 0.0025186290932355413, NaN,
-0.0027004353617072928, -0.02701110470577827, -1.6675818459624492e-5, -4.500725704677709e-18,
-6.751088418853e-5, 0.027004353617072926, -1.453982979565589e-13, 3.0954458657030894,
1.9115100657289757e-8, 0.0, -0.0024992261385344185, NaN, 0.0, 0.00018145405361482672,
9.226254817911728e-5, 0.0001438818602013502, 0.00012023743083737167, 1.26786767718842e-5,
3.289952730641701e-6, 3.375545733305741e-8, 8.333347263556714e-18, 1.531149941368414e-14,
3.375545733305741e-8, 8.333347263556714e-18, 1.531149941368414e-14, 3.375545733305741e-8,
8.333347263556714e-18, 1.531149941368414e-14, 3.375545733305741e-8, 8.333347263556714e-18,
1.531149941368414e-14, 3.375545733305741e-8, 8.333347263556714e-18, 1.531149941368414e-14,
3.375545733305741e-8, 8.333347263556714e-18, 1.531149941368414e-14, 3.375545733305741e-8,
8.333347263556714e-18, 1.531149941368414e-14, 3.375545733305741e-8, 8.333347263556714e-18,
1.531149941368414e-14, 3.375545733305741e-8, 8.333347263556714e-18, 1.531149941368414e-14,
3.375545733305741e-8, 8.333347263556714e-18, 1.531149941368414e-14, 3.375545733305741e-8,
8.333347263556714e-18, 1.531149941368414e-14, 0.00014180661194775285, 3.87144489336943e-11,
-0.00013502176807081646, 0.0, 0.0, 0.0]where we see there are lots of |
|
For what it's worth, I'm noticing (actually, @smokinmirror noticed) that it's at the 5th coordinate of the state where the gradient of the cost is |
|
So I modified the file to remove all the extra bits and just have one iteration of the MPC. I do get values for the 5th variable and the states also look much different as compared to the states you mentioned. Attaching the code samples and the output for it. import math
import random
import csv
import casadi as cs
import opengen as og
import numpy as np
def runga_kutta_4th(f, xk, uk, dt):
k1 = f(xk, uk)
k2 = f(xk + dt/2*k1, uk)
k3 = f(xk + dt/2*k2, uk)
k4 = f(xk + dt*k3, uk)
return dt/6*(k1+2*k2+2*k3+k4)
def generate_panoc_problem(n_states, n_inputs, n_desired, n_horizon, Ts, path_length, x_lut, y_lut):
parameters = cs.MX.sym('z', n_states + n_desired)
initial_state = parameters[0:n_states]
desired_state = parameters[n_states:]
Q = cs.diagcat(100.0, 10000000.0, 1000.0)
R = cs.diagcat(50, 50, 50.0)
l_f = 0.75
l_r = 0.75
l = 1.5
states_lb = [-10000000.0, -10000000.0, -np.pi, -30, -0.1, -3]
states_ub = [10000000.0, 10000000.0, np.pi, 30, path_length, 100000.0]
inputs_lb = [-1, -0.7, -2]
inputs_ub = [+10, 0.7, 2]
x, u = cs.MX.sym("x", n_states), cs.MX.sym("u", n_inputs)
p_x, p_y, p_yaw, v, p_s, p_vs = cs.vertsplit(x)
a, delta, a_s = cs.vertsplit(u)
beta = cs.atan(l_r / (l_f + l_r) * cs.tan(delta))
dynamics = cs.vertcat(
v * cs.cos(beta + p_yaw),
v * cs.sin(beta + p_yaw),
v * cs.tan(delta) * cs.cos(beta) / (l_f + l_r),
a,
p_vs,
a_s
)
f = cs.Function("f", [x, u], [dynamics])
variables = cs.MX.sym("vars", n_states + n_inputs)
X = variables[0:n_states]
U = variables[n_states:]
X0X = cs.vertcat(initial_state, X)
objective = 0
dynamics_constr = []
xk, uk = X0X[:n_states], U[:]
xkp1 = X0X[n_states:]
x_s = x_lut(xk[4])
y_s = y_lut(xk[4])
dy_x = x_lut.jacobian()(xk[4], x_s)
dy_y = y_lut.jacobian()(xk[4], y_s)
yaw = cs.atan2(dy_y, dy_x)
# Contouring Error
contour_error = cs.sin(yaw)*(xk[0] - x_s) - cs.cos(yaw)*(xk[1] - y_s)
# Lag Error
lag_error = -cs.cos(yaw)*(xk[0] - x_s) - cs.sin(yaw)*(xk[1] - y_s)
# Velocity Error
velocity_error = xk[3] - desired_state[0]
err = cs.vertcat(contour_error, lag_error, velocity_error)
objective += err.T @ Q @ err + uk.T @ R @ uk
next_euler = (xk + runga_kutta_4th(f, xk, uk, Ts))
dynamics_constr = cs.vertcat(dynamics_constr, xkp1 - next_euler)
optimization_variables = variables
optimization_parameters = parameters
xmin = states_lb
umin = inputs_lb
xmax = states_ub
umax = inputs_ub
bounds = og.constraints.Rectangle(xmin + umin, xmax + umax)
problem = og.builder.Problem(optimization_variables,
optimization_parameters,
objective) \
.with_constraints(bounds) \
.with_penalty_constraints(dynamics_constr)
tcp_config = og.config.TcpServerConfiguration('0.0.0.0', 9555)
build_config = og.config.BuildConfiguration()\
.with_build_directory("optimizers")\
.with_build_mode("release")\
.with_tcp_interface_config(tcp_config)
meta = og.config.OptimizerMeta()\
.with_optimizer_name("mpcc_test2")
solver_config = og.config.SolverConfiguration()\
.with_tolerance(1e-5)\
.with_preconditioning(True)
builder = og.builder.OpEnOptimizerBuilder(problem,
meta,
build_config,
solver_config)\
.with_verbosity_level(1)
builder.build()
def main(x_lut, y_lut, path_length):
n_states = 6
n_desired = 4
n_inputs = 3
Ts = 0.1
n_horizon = 15
s = 0.0
v_s = 0.0
param = [0.051675815135240555, 3.7084553241729736, -5.326321862684935e-07, 0.00047293867110315835, 0.0016758168110367162, 0.0, 1.5, 0.0, 0.0, 0.0]
acceleration, steering, a_s = 0.0, 0.0, 0.0
# Solve using PANOC
generate_panoc_problem(n_states, n_inputs, n_desired, n_horizon, Ts, path_length, x_lut, y_lut)
mpcc_mng = og.tcp.OptimizerTcpManager('optimizers/mpcc_test2')
mpcc_mng.start()
solution = mpcc_mng.call(p=param)
results = solution['solution']
print(results[:])
mpcc_mng.kill()
if __name__ == '__main__':
# Load the track and convert it into a spline
waypoints_file = '/home/aditya/carla_track.txt'
with open(waypoints_file) as waypoints_file_handle:
waypoints = list(csv.reader(waypoints_file_handle, delimiter=',',
quoting=csv.QUOTE_NONNUMERIC))
waypoints_np = np.array(waypoints)
x_data = [w[0]*10**-2 for w in waypoints]
y_data = [w[1]*10**-2 for w in waypoints]
#y_data = savgol_filter(y_data, 51, 3)
arr = np.zeros(len(x_data))
for i in range(1, len(x_data)):
dx = x_data[i] - x_data[i-1]
dy = y_data[i] - y_data[i-1]
arr[i] = math.hypot(dx, dy)
xy_data = arr
sum = 0.0
arr = np.zeros(len(xy_data))
for i in range(len(xy_data)):
arr[i] = sum + xy_data[i]
sum += xy_data[i]
x_lut = cs.interpolant('LUT', 'bspline', [arr], x_data)
y_lut = cs.interpolant('LUT', 'bspline', [arr], y_data)
path_length = arr[-1]
main(x_lut, y_lut, path_length)Output |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
I am trying to formulate a MPCC style control system but while running it I get a problem solution failed. What does the error mean? Any thoughts oh how can I debug it?
See #301 (comment) for reproducing the error.
Expected behavior
The problem should be solved? Note that the same problem works okay with IPOPT.
System information:
rustup show?rustc -V?Additional context
Add any other context about the problem here.
The text was updated successfully, but these errors were encountered: