Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improvements over acados MPC formulation #183

Open
wants to merge 9 commits into
base: main
Choose a base branch
from
Open

Improvements over acados MPC formulation #183

Show file tree
Hide file tree
Changes from 8 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
205dcc4
improve acados mpc
aghezz1 Dec 4, 2024
044d812
fix call to casadi integrators to avoid warning for deprecation
aghezz1 Dec 4, 2024
bb47497
incluse t_wall to stats report
aghezz1 Dec 4, 2024
a70f8e6
get t_wall from acados mpc
aghezz1 Dec 4, 2024
0eb0b29
improving plots
aghezz1 Dec 5, 2024
08e293d
improving plots 2
aghezz1 Dec 5, 2024
6702a4d
add warmstart_type field for acados, avoid multiple compilation of ac…
aghezz1 Dec 5, 2024
264afd5
fixing t_wall metrics for other controllers and mc
aghezz1 Dec 9, 2024
cfe0159
renaming warm starting strategy
aghezz1 Dec 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions examples/mpc/config_overrides/quadrotor_3D/mpc_acados_quadrotor_3D_tracking.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,4 +25,5 @@ algo_config:
randomize_prior_prop: False
prior_prop_rand_info: null
warmstart: True
warmstart_type: "heuristic" # "heuristic", "lqr", "ipopt"
use_RTI: False
22 changes: 13 additions & 9 deletions examples/mpc/mpc_experiment.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,11 +64,11 @@ def run(gui=False, plot=True, n_episodes=1, n_steps=None, save_data=False):


def post_analysis(state_stack, input_stack, env):
'''Plots the input and states to determine iLQR's success.
'''Plots the input and states to determine MPC's success.
aghezz1 marked this conversation as resolved.
Show resolved Hide resolved

Args:
state_stack (ndarray): The list of observations of iLQR in the latest run.
input_stack (ndarray): The list of inputs of iLQR in the latest run.
state_stack (ndarray): The list of observations of MPC in the latest run.
input_stack (ndarray): The list of inputs of MPC in the latest run.
'''
model = env.symbolic
stepsize = model.dt
Expand All @@ -81,29 +81,33 @@ def post_analysis(state_stack, input_stack, env):
reference = np.tile(reference.reshape(1, model.nx), (plot_length, 1))

# Plot states
fig, axs = plt.subplots(model.nx)
fig, axs = plt.subplots(model.nx, figsize=(6, 9), sharex=True)
for k in range(model.nx):
axs[k].plot(times, np.array(state_stack).transpose()[k, 0:plot_length], label='actual')
axs[k].plot(times, reference.transpose()[k, 0:plot_length], color='r', label='desired')
axs[k].set(ylabel=env.STATE_LABELS[k] + f'\n[{env.STATE_UNITS[k]}]')
axs[k].yaxis.set_major_formatter(FormatStrFormatter('%.1f'))
if k != model.nx - 1:
axs[k].set_xticks([])
axs[k].set_xlim(times[0], times[-1])
axs[0].set_title('State Trajectories')
axs[-1].legend(ncol=3, bbox_transform=fig.transFigure, bbox_to_anchor=(1, 0), loc='lower right')
axs[-1].set(xlabel='time (sec)')

fig.tight_layout()
# Plot inputs
_, axs = plt.subplots(model.nu)
fig, axs = plt.subplots(model.nu, sharex=True)
if model.nu == 1:
axs = [axs]
for k in range(model.nu):
axs[k].plot(times, np.array(input_stack).transpose()[k, 0:plot_length])
axs[k].set(ylabel=f'input {k}')
axs[k].set(ylabel=env.ACTION_LABELS[k] + f'\n[{env.ACTION_UNITS[k]}]')
axs[k].yaxis.set_major_formatter(FormatStrFormatter('%.1f'))
axs[k].yaxis.set_major_formatter(FormatStrFormatter('%.2f'))
axs[k].set_ylim(0.8*env.physical_action_bounds[0][k], 1.2*env.physical_action_bounds[1][k])
axs[k].axhline(env.physical_action_bounds[0][k], color="r", alpha=0.5, linestyle="--")
axs[k].axhline(env.physical_action_bounds[1][k], color="r", alpha=0.5, linestyle="--")
axs[k].set_xlim(times[0], times[-1])
axs[0].set_title('Input Trajectories')
axs[-1].set(xlabel='time (sec)')
fig.tight_layout()

plt.show()

Expand Down
5 changes: 4 additions & 1 deletion safe_control_gym/controllers/mpc/mpc.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -216,14 +216,17 @@ def compute_initial_guess(self, init_state, goal_states=None):
u = self.lqr_gain @ (x_guess[:, i] - goal_states[:, i]) + np.atleast_2d(self.model.U_EQ)[0, :].T
u_guess[:, i] = u
x_guess[:, i + 1, None] = self.dynamics_func(x0=x_guess[:, i], p=u)['xf'].toarray()
elif self.compute_initial_guess_method == 'heuristic':
aghezz1 marked this conversation as resolved.
Show resolved Hide resolved
x_guess = goal_states
u_guess = np.repeat(np.atleast_2d(self.U_EQ), self.T, axis=0).T
else:
raise Exception('Initial guess method not implemented.')

self.x_prev = x_guess
self.u_prev = u_guess

# set the solver back
self.setup_optimizer(solver=self.solver)
self.setup_optimizer(solver=self.solver) # TODO why? this method is also called from mpc_acados! at least move to line 209!
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't see setup_optimizer being called in mpc_acados. It calls setup_acados_optimizer, but this function is for casadi. I am a bit confused about this whole section lol.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

With my comment I wanted to highlight that when you call compute_initial_guess() from mpc_acados.py you end up calling compute_initial_guess()from mpc.py and also you run line 229 which creates an instance of a casadi nlpsolver that is not needed in mpcacados.py.
Line 229 is important only in case you are using mpc.py, in order to overwrite the solver at line 188.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hmmm I see your point, we're setting up casadi for no reason. We should fix this, will talk to the group

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This was added by me. This idea of this is allowing solver swiching for mpc. For example, do an initial guess (self.init_solver) with ipopt then switch to a faster sqp solver (self.solver). I have tested with cartpole previously and this can improve the overall runtime and accuracy. But indeed acados will not use this. I think we can keep this if there is no clear interference with other code.

One problem I notice is that this will add several milliseconds to the runtime. So the init step runtime needs a special treatment (maybe simply remove the first step).

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would move it to line 209, you are interested to have a robust solver for the warmstart only.
So you switch to ipopt solver when ipopt is the chosen warmstart type.
Otherwise you go on using the NLP solver you started with.
For instance, if you choose sqpmethod with casadi, and lqr as warmstart type you don't have to create/change solver for the initialization.


return x_guess, u_guess

Expand Down
Loading