Comments (10)
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 generate_problem
. What are the various arguments?
from optimization-engine.
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.
from optimization-engine.
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.
from optimization-engine.
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
- Download/Copy-Paste the csv file
- Download/Copy-Paste the py file
- Change the csv path in the python file
- Run the python file
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
6.0, 370.5140
105.0, 370.5140
205.0, 370.5140
305.0, 370.5140
405.0, 370.5140
505.0, 370.5140
605.0, 370.5140
705.0, 370.5140
805.0, 370.5140
905.0, 370.5140
1005.0, 370.5140
1105.0, 370.5140
1205.0, 370.5140
1305.0, 370.5140
1405.0, 370.5140
1505.0, 370.5140
1605.0, 370.5140
1705.0, 370.5140
1805.0, 370.5140
1905.0, 370.5140
2005.0, 370.5140
2105.0, 370.5140
2205.0, 370.5140
2305.0, 370.5140
2405.0, 370.5140
2505.0, 370.5140
2605.0, 370.5140
2705.0, 370.5140
2805.0, 370.5140
2905.0, 370.5140
3005.0, 370.5140
3105.0, 370.5140
3205.0, 370.5140
3305.0, 370.5140
3405.0, 370.5140
3505.0, 370.5140
3605.0, 370.5140
3705.0, 370.5140
3805.0, 370.5140
3905.0, 370.5140
4005.0, 370.5140
4105.0, 370.5140
4205.0, 370.5140
4305.0, 370.5140
4405.0, 370.5140
4505.0, 370.5140
4605.0, 370.5140
4705.0, 370.5140
4805.0, 370.5140
4905.0, 370.5140
5005.0, 370.5140
5105.0, 370.5140
5205.0, 370.5140
5305.0, 370.5140
5405.0, 370.5140
5505.0, 370.5140
5605.0, 370.5140
5705.0, 370.5140
5805.0, 370.5140
5905.0, 370.5140
6005.0, 370.5140
6105.0, 370.5140
6200.0, 369.4
6300.0, 368.4
6400.0, 366.9
6500.0, 364.9
6600.0, 362.4
6700.0, 359.4
6800.0, 355.8
6900.0, 351.5
7000.0, 346.5
7100.0, 341.0
7200.0, 334.4
7300.0, 327.0
7400.0, 318.7
7500.0, 309.3
7600.0, 298.5
7700.0, 286.6
7751.0, 280.1
7800.0, 272.9
7850.0, 265.7
7916.0, 255.1
7975.0, 245.1
8000.0, 240.2
8054.0, 230.1
8102.0, 220.2
8149.0, 210.1
8191.0, 200.0
8231.0, 190.0
8268.0, 180.1
8304.0, 170.0
8337.0, 160.0
8369.0, 150.0
8399.0, 140.0
8427.0, 130.0
8453.0, 120.0
8479.0, 110.0
8503.0, 100.0
8525.0, 90.0
8546.0, 80.0
8567.0, 70.0
8585.0, 60.0
8600.0, 50.0
8621.0, 40.0
8637.0, 30.0
8652.0, 20.0
8667.0, 10.0
8680.0, 0.0
8692.0, -10.0
8700.0, -20.0
8715.0, -30.0
8726.0, -40.0
8736.0, -50.0
8745.0, -60.0
8754.0, -70.0
8760.0, -80.0
8768.0, -90.0
8775.0, -100.0
8781.0, -110.0
8788.0, -120.0
8792.0, -130.0
8796.0, -140.0
8800.0, -150.0
8804.0, -160.0
8804.0, -170.0
8804.0, -180.0
8804.0, -190.0
8804.0, -200.0
8804.0, -210.0
8804.0, -220.0
8804.0, -230.0
8804.0, -240.0
8804.0, -250.0
8804.0, -260.0
8804.0, -360.0
8804.0, -460.0
8804.0, -560.0
8804.0, -660.0
8804.0, -760.0
8804.0, -860.0
8804.0, -960.0
8804.0, -1060.0
8804.0, -1160.0
8804.0, -1260.0
8804.0, -1360.0
8804.0, -1460.0
8804.0, -1560.0
8804.0, -1660.0
8804.0, -1760.0
8804.0, -1860.0
8804.0, -1960.0
8804.0, -2060.0
from optimization-engine.
@alphaville thoughts?
from optimization-engine.
@truncs Sorry for the delay. We're looking at it and we'll try to get back to you by next week.
from optimization-engine.
@alphaville Thanks, much appreciated!
from optimization-engine.
We looked into this and the very first iteration, the algorithm is at the point:
u = [0.032540166451917776, 1.0014558585309914, -1.4383287905910368e-7, 0.00017414572012260812, -0.018120131695473597, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 0.018572675195099905, 1.146906129436235e-5, 1e-12, 4.643168898530375e-5, -0.018572675193102867, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 0.18572675194199906, 0.0001146906039436235, 1e-12, 0.0004643168808530375, -0.1, 1e-12, -8.260619139023693e-5, -2.3681830224775235e-5, -2.2627164881463407e-6, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12]
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 NaN
s, so I suspect there's something wrong with the gradient of the cost function. Perhaps it's not differentiatiable around the starting point. Is there a minimal working example?
from optimization-engine.
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 NaN
.
from optimization-engine.
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
[0.051721711545130486, 3.708455507734047, -5.35082141500763e-07, 0.0004449378279139774, 0.0016757785034980444, -7.678264896759694e-07, -0.0002715292682926265, -7.76248128226494e-05, -7.445753145453775e-06]
from optimization-engine.
Related Issues (20)
- Sphinx-proof docstrings
- ALM converging but not satisfying constraints HOT 3
- Design and imlpement OCPMaker
- Migrate to Non-Fork Repo HOT 3
- Print or log out parameter length when compiling the solver HOT 4
- Unable to run the MATLAB examples HOT 7
- An error "linking with link.exe failed: exit code: 1104" HOT 8
- An error when optimizing
- roslaunch husky_gazebo husky_empty_world.launch HOT 3
- Consider swapping to web-time from instant
- Always return best solution of inner problem
- Python support for Sphere and AffineSpace
- Additional constraints
- Small typo in python navigation example
- Code generation for Sphere2
- Warning when creating TCP server
- Jinja files to have .jinja extension
- Wrong documentation about Cartesian product constraint
- Infinite loop after evaluation of points out of the constraint set
- Obstacle avoidance in ECC 18
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from optimization-engine.