Closed truncs closed 1 year ago
I was able to solve the constraints being not honored part by using penalty_constraints but it is still slow. Updated cods below
import time
import matplotlib.pyplot as plt
from datetime import timedelta
import os
import sys
import casadi as cs
import numpy as np
import opengen as og
Ts = 0.05 # Sampling time
N_hor = 18 # Horizon length
N_sim = 80 # Length of the simulation
multipleshooting = False
R_obstacle = 2
def _generate_problem_ms(
Ts,
N_hor,
R_obstacle,
n_states,
n_inputs,
f,
Q,
R,
states_lb,
states_ub,
inputs_lb,
inputs_ub,
):
n_horizon = N_hor
parameters = cs.MX.sym('z', n_states + n_states)
initial_state = parameters[0:n_states]
desired_state = parameters[n_states:]
variables = cs.MX.sym("vars", n_states*N_hor + n_inputs*N_hor)
X = variables[0:n_states*N_hor]
U = variables[n_states*N_hor:]
X0X = cs.vertcat(initial_state, X)
objective = 0
dynamics_constr = []
collision_constr = []
# Forward Euler
for k in range(N_hor):
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]
err = xk - desired_state
objective += err.T @ Q @ err + uk.T @ R @ uk
next_euler = xk + Ts * f(xk, uk)
dynamics_constr = cs.vertcat(dynamics_constr, xkp1 - next_euler)
if k > 0:
collision_constr = cs.vertcat(
collision_constr, cs.fmax(R_obstacle**2 - xk[0:2].T @ xk[0:2], 0)
) # ball around (0,0)
xN = X0X[-n_states:]
err = xN - desired_state
objective += 10 * err.T @ Q @ err # Terminal cost
collision_constr = cs.vertcat(collision_constr, cs.fmax(R_obstacle**2 - xk[0:2].T @ xk[0:2], 0))
constraints = cs.vertcat(dynamics_constr, collision_constr)
dynamics_constr_lb = [0.0] * N_hor * n_states
dynamics_constr_ub = [0.0] * N_hor * n_states
collision_constr_lb = [R_obstacle ** 2] * N_hor
collision_constr_ub = [+np.inf] * N_hor
c_bounds = og.constraints.Rectangle(
dynamics_constr_lb + collision_constr_lb,
dynamics_constr_ub + collision_constr_ub
)
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)
#c = dynamics_constr #cs.vertcat(dynamics_constr, collision_constr)
#set_c = og.constraints.Zero()
optimization_variables = variables
optimization_parameters = parameters
problem = og.builder.Problem(optimization_variables,
optimization_parameters,
objective) \
.with_penalty_constraints(constraints) \
.with_constraints(bounds)
first_input_idx = N_hor * n_states
build_config = og.config.BuildConfiguration()\
.with_build_directory("optimizers")\
.with_build_mode("debug")\
.with_build_python_bindings()
meta = og.config.OptimizerMeta()\
.with_optimizer_name("bicycle")
solver_config = og.config.SolverConfiguration()\
.with_tolerance(1e-4)
builder = og.builder.OpEnOptimizerBuilder(problem,
meta,
build_config,
solver_config)\
.with_verbosity_level(1)
builder.build()
return f, n_states, n_inputs, first_input_idx
def generate_problem(Ts, N_hor, R_obstacle, multipleshooting):
n_states = 4
n_inputs = 2
# state weights matrix
Q = cs.diagcat(100, 100, 10, 10)
# controls weights matrix
R = cs.diagcat(1e-1, 1e-5)
# physical constants
l = 0.5
l_r = l / 2
l_f = l / 2
# state and input constraints
states_lb = [-10, -10, -np.pi / 3, -3]
states_ub = [+10, +10, +np.pi / 3, +3]
inputs_lb = [-5, -np.pi / 4]
inputs_ub = [+5, +np.pi / 4]
x, u = cs.SX.sym("x", n_states), cs.SX.sym("u", n_inputs)
p_x, p_y, ψ, v = cs.vertsplit(x)
a, δ_f = cs.vertsplit(u)
β = cs.atan(l_r / (l_f + l_r) * cs.tan(δ_f))
continuous_dynamics = cs.vertcat(
v * cs.cos(ψ + β),
v * cs.sin(ψ + β),
v / l_r * cs.sin(β),
a,
)
f = cs.Function("f", [x, u], [continuous_dynamics])
gen = _generate_problem_ms
return gen(
Ts,
N_hor,
R_obstacle,
n_states,
n_inputs,
f,
Q,
R,
states_lb,
states_ub,
inputs_lb,
inputs_ub,
)
f, n_states, n_inputs, first_input_idx = generate_problem(
Ts, N_hor, R_obstacle, multipleshooting
)
sys.path.insert(1, './optimizers/bicycle')
import bicycle
solver = bicycle.solver()
state = np.array([-5.0, 0.0, 0.0, 0.0])
dest = np.array([5.0, 0.1, 0, 0])
xs = np.zeros((N_sim, n_states))
times = np.zeros((N_sim,))
for k in range(N_sim):
p = state.tolist() + dest.tolist()
t0 = time.perf_counter()
solution = solver.run(p=p)
t = time.perf_counter() - t0
times[k] = t
xs[k] = state
print(
solution.exit_status,
solution.solve_time_ms,
solution.num_outer_iterations,
solution.num_inner_iterations,
)
#import ipdb; ipdb.set_trace()
input = solution.solution[first_input_idx : first_input_idx + n_inputs]
state += (Ts * f(state, input)).toarray().squeeze()
print('State: ' + str(state))
#%% Plot
fig_trajectory, ax = plt.subplots(1, 1)
c = plt.Circle((0, 0), R_obstacle)
ax.set_aspect(1)
ax.add_artist(c)
ax.plot(xs[:, 0], xs[:, 1], "r.-")
ax.set_title('Trajectory')
fig_time, ax = plt.subplots(1, 1)
ax.plot(times)
ax.set_title("Run time")
ax.set_xlabel("MPC time step")
ax.set_ylabel("Run time [s]")
plt.show()
Closing this, found that with the release build it is really fast, on the order 60msecs.
@truncs Aditya, you can also try to use with_preconditioning(True)
. This may speed things up further.
@alphaville Amazing. Getting runtimes of 10 - 15 msecs. Thank you!
Describe the bug
I have a toy MPC problem to go from the start to a destination while obeying the dynamics. Full code is below. While running the sample seems like the per iteration time is in the other of >100msecs and the constraints of the obstacle is being violated.
To Reproduce
Output
Run the self contained sample above.
Expected behavior
Per iteration should have been way faster and the constraints honored.
System information:
rustup show
?stable-x86_64-unknown-linux-gnu (default) rustc 1.64.0 (a55dd71d5 2022-09-19)
rustc 1.64.0 (a55dd71d5 2022-09-19)
python --version Python 3.8.14