Improvements over acados MPC formulation #183

aghezz1 · 2024-12-05T09:42:56Z

I had some problems in rebasing #181 and updating #182.
So I created this new PR to have a clean git history, sorry for the mess.

A quick comparison by running the tracking problem for the drone of a 3D lemniscate
Order: casadi/ipopt, acados/sqp, acados/sqp_rti
Considerable speed up of computation time (cf. t_wall metrics)

…ados solver and mc

Federico-PizarroBejarano

Looking good! Here are just some basic comments. I don't know much about acados so I will leave that review more for @MingxuanChe and @adamhall

examples/mpc/config_overrides/quadrotor_3D/mpc_acados_quadrotor_3D_tracking.yaml

examples/mpc/mpc_experiment.py

Federico-PizarroBejarano · 2024-12-06T17:02:42Z

safe_control_gym/experiments/base_experiment.py

@@ -409,10 +409,16 @@ def compute_metrics(self, data, verbose=False):
            'average_constraint_violation': np.asarray(self.get_episode_constraint_violation_steps()).mean(),
            'constraint_violation_std': np.asarray(self.get_episode_constraint_violation_steps()).std(),
            'constraint_violation': np.asarray(self.get_episode_constraint_violation_steps()) if len(self.get_episode_constraint_violation_steps()) > 1 else self.get_episode_constraint_violation_steps()[0],
+            'mean_t_wall_ms': np.asarray(self.get_t_wall()).mean()*1e3,


will this raise errors for the other controllers which do not have 't_wall' in their data?

Yes, thanks for pointing out. I propose a fix with a try/except.
Ultimately, i believe it would also be interesting to add the t_wall for the inference of neural networks.

In another branch what we did is we measure the inference time in the base_experiment.py class itself, so we don't need to change every controller. I think this is a cleaner solution. For now I propose we remove this and instead update the base experiment later to measure the time

Just a supplementary comment on the plan for the inference time metric. The inference time in the other branch is temporary. This metric should consider only the online optimization time and exclude all the variable logging etc. So in the end we will need to modify every controller to get this metric.

I also agree on removing this first from base_experiment.py but keeping the timing in the mpc_acados as some initial progess for unifying this metric.

For linear_mpc, if we add the option 'record_time': True when self.solver == 'qrqp', then we can get t_wall_total for that controller as well. I think its a really useful metric so maybe we can keep it in mpc, linear_mpc, and mpc_acados, but not in base_experiment.py. I think in the future we should look at how to get a good timing metric for all controllers though (that doesn't include logging time).

safe_control_gym/controllers/mpc/mpc_acados.py

Federico-PizarroBejarano · 2024-12-06T17:09:38Z

safe_control_gym/controllers/mpc/mpc.py

        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!


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.

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.

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

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).

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.

safe_control_gym/controllers/mpc/mpc.py

Federico-PizarroBejarano · 2024-12-09T18:52:26Z

BTW @aghezz1, what version of acados are you using? How often should we update our own acados?

aghezz1 · 2024-12-11T10:23:09Z

BTW @aghezz1, what version of acados are you using? How often should we update our own acados?

I am on master branch at the moment, as I am using an helper function for detecting constraints that was bugged in v0.4.2. Now the function is fixed but not yet included in an official version.
For what we are doing here any recent version of acados would work (>= v0.4.0) but I would update when v0.4.4 is released.

MingxuanChe · 2024-12-11T17:45:42Z

Just a heads-up: I tried release 0.4.3 and it complains the following bug. Building from the master branch works well.

File "/home/mingxuan/Repositories/scg_andrea/safe_control_gym/controllers/mpc/mpc_acados.py", line 194, in setup_acados_optimizer
    ocp = self.processing_acados_constraints_expression(ocp, h0_expr, h_expr, he_expr)
  File "/home/mingxuan/Repositories/scg_andrea/safe_control_gym/controllers/mpc/mpc_acados.py", line 337, in processing_acados_constraints_expression
    detect_constraint_structure(ocp.model, ocp.constraints, stage_type=s_type)
  File "/home/mingxuan/Repositories/acados/interfaces/acados_template/acados_template/mpc_utils.py", line 297, in detect_constraint_structure
    constraints.idxbx = J_to_idx(Jbx)
  File "/home/mingxuan/Repositories/acados/interfaces/acados_template/acados_template/mpc_utils.py", line 313, in J_to_idx
    raise ValueError(
ValueError: J_to_idx: Invalid J matrix. Exiting. Found more than one nonzero in row 1.

MingxuanChe

Thank you for the PR and I am happy to learn more from you. In general, this looks good and I find that your method for parsing the constraints is faster than my old implementation. I have some comments about some particular code.

safe_control_gym/controllers/mpc/mpc_acados.py

MingxuanChe · 2024-12-11T17:12:42Z

safe_control_gym/controllers/mpc/mpc_acados.py

@@ -344,31 +426,14 @@ def select_action(self,
        self.acados_ocp_solver.set(self.T, 'yref', y_ref_e)

        # solve the optimization problem
-        if self.use_RTI:


I want to learn more about why this is removed. I previously learned this phase switching from the examples, e.g. here and here, and I thought this was the standard way of using RTI. Also, when is this phase switching necessary?

Splitting RTI in two phases is useful in two cases:

you are really working on an embedded system where is important to minimize feedback delay. As you can do the preparation phase while the system is within a sampling time and as soon as you receive the new initial state you solve for the feedback phase. (You don't need to wait for the new initial state for doing linear algebra routines like condensing)

a more accurate profiling: for instance if you compare against a neural network controller you can compare the time spent in the feedback phase only. As the preparation phase can be during the previous sampling time. But again if you have very short sampling time the difference is smaller.

For more on RTI and it's implementation you can have a look to RTI paper

MingxuanChe · 2024-12-11T18:10:00Z

safe_control_gym/controllers/mpc/mpc_acados.py

@@ -260,38 +260,120 @@ def processing_acados_constraints_expression(self,
        ocp.constraints.ubu = self.env.constraints.input_constraints[0].upper_bounds
        ocp.constraints.idxbu = idxbu # active constraints dimension
        '''
-
-        ub = {'h': set_acados_constraint_bound(h_expr, 'ub', self.constraint_tol),


I briefly compared these two implementations, and I find yours is faster.
Updated version:

Before:

I'd like to learn more about what case 1 2 3 means. Also I didn't fully understand what detect_constraint_structure does. It seems that it doesn't return anything (or could you ask your colleague to add some docstring for this function.)

Apart from that, I wonder whether we can condense these codes more. For example I think gym has a fixed way to parse the constraint and we might always have lower bounds before upper bounds (here). So we might could safely remove the flag start_with_ub. Please correct me if I am wrong @adamhall .

Most of the code could avoided if the constraints keep their structures as simple bounds, linear, ... when you pass them to the environment. I know that rewriting everything as h(x, u) \leq 0 make many things simpler but also slower 😄
So to keep your code as it is, I think is good to preprocess the constraints if the solver you use exploits the constraint structure.

My implementation is a bit complicated but the rationale behind is simple:

acados prefers double-sided constraints as lh <= h(x, u) <= uh where the bounds are finite (not 1e10). Large numbers creates numerical difficulties.

Connected to 1. is therefore better to specify lh <= h(x, u) <= uhthan -inf <= -h(x, u) + lh <= 0 AND -inf <= h(x, u) - uh <=0

Some constraints structures like simple bounds or linear constraint are directly supported within acados. So if they are correctly declared, acados will not compile and build external functions.

Therefore in my implementation I do two steps:

I look into your constraints and check if the constraints is double-sided. Case 1., 2., 3., they simply look to different possible structure of the constraint Jacobian. Example: you would enter case 1 if you have constraints only on the state, case 2 if only on the control, case 3 if constraints are both on state and control.

I extract lower and upper bound from your generic expression h

I now have all the fields h, lh, uh for initial, path and terminal constraints. I populate the acados OCP object with these fields. Then, I call the acados helper function for detecting if this constraints are simple bounds or linear.

If such constraint structures are detected, the acados detecting constraints directly writes in your ocp object

This is very interesting! I didn't realize that the solvers care that much about how constraints are specified. I wonder if instead of having this block of code we modify or add something to constraints.py to handle different formulations. For example, the DefaultConstraint class is a child of BoudnedConstraint where we could add an option or function that creates them in the form of lb < h(x,u) < ub. On the contrary, this would only be useful in mpc_acados, so maybe it makes sense to do the parsing in mpc_acados, unless ipopt also benefits from having this constraint structure?

If we do keep the code likee this, I think some additional comments, like the ones in your comment above, might be helpful when we have to edit or debug in the future.

I think for IPOPT the benefit would the very minimal for the computation speed on small problems. But for large problems if the constraints are double-sided and you implement them as h(x, u) < 0 you basically double the number of constraints. So it would be nice indeed to preserve the constraints structure.

aghezz1 · 2024-12-12T09:35:39Z

@MingxuanChe regarding acados version, at the moment to use detect_constraints_structure you need to stay on master.
It will be included in the upcoming release.

BTW @aghezz1, what version of acados are you using? How often should we update our own acados?

I am on master branch at the moment, as I am using an helper function for detecting constraints that was bugged in v0.4.2. Now the function is fixed but not yet included in an official version. For what we are doing here any recent version of acados would work (>= v0.4.0) but I would update when v0.4.4 is released.

aghezz1 · 2024-12-11T10:26:09Z

safe_control_gym/experiments/base_experiment.py

+            return self.data['controller_data'][0]['t_wall'][0]
+        except:
+            return np.nan
+


In another branch what we did is we measure the inference time in the base_experiment.py class itself, so we don't need to change every controller. I think this is a cleaner solution. For now I propose we remove this and instead update the base experiment later to measure the time

I think this a valid option, what you think?

I think the problem with this is that there is a lot of logging that goes on inside some of the select_action functions. If we can separate out that logging, then I think the timings from base_experiment would make sense, but it might be necessary that all the algo's have their own timing routine as they all log a little differently?

I'm not sure I understood your comment..
I think would be cool to have the time spent in the solver, the python overhead in select_action should not be included.

aghezz1 · 2024-12-12T08:52:37Z

safe_control_gym/controllers/mpc/mpc.py

        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!


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.

safe_control_gym/controllers/mpc/mpc_acados.py

aghezz1 · 2024-12-12T09:06:06Z

safe_control_gym/controllers/mpc/mpc_acados.py

@@ -344,31 +426,14 @@ def select_action(self,
        self.acados_ocp_solver.set(self.T, 'yref', y_ref_e)

        # solve the optimization problem
-        if self.use_RTI:


Splitting RTI in two phases is useful in two cases:

you are really working on an embedded system where is important to minimize feedback delay. As you can do the preparation phase while the system is within a sampling time and as soon as you receive the new initial state you solve for the feedback phase. (You don't need to wait for the new initial state for doing linear algebra routines like condensing)

a more accurate profiling: for instance if you compare against a neural network controller you can compare the time spent in the feedback phase only. As the preparation phase can be during the previous sampling time. But again if you have very short sampling time the difference is smaller.

For more on RTI and it's implementation you can have a look to RTI paper

aghezz1 · 2024-12-12T09:26:31Z

safe_control_gym/controllers/mpc/mpc_acados.py

@@ -260,38 +260,120 @@ def processing_acados_constraints_expression(self,
        ocp.constraints.ubu = self.env.constraints.input_constraints[0].upper_bounds
        ocp.constraints.idxbu = idxbu # active constraints dimension
        '''
-
-        ub = {'h': set_acados_constraint_bound(h_expr, 'ub', self.constraint_tol),


Most of the code could avoided if the constraints keep their structures as simple bounds, linear, ... when you pass them to the environment. I know that rewriting everything as h(x, u) \leq 0 make many things simpler but also slower 😄
So to keep your code as it is, I think is good to preprocess the constraints if the solver you use exploits the constraint structure.

My implementation is a bit complicated but the rationale behind is simple:

acados prefers double-sided constraints as lh <= h(x, u) <= uh where the bounds are finite (not 1e10). Large numbers creates numerical difficulties.

Connected to 1. is therefore better to specify lh <= h(x, u) <= uhthan -inf <= -h(x, u) + lh <= 0 AND -inf <= h(x, u) - uh <=0

Some constraints structures like simple bounds or linear constraint are directly supported within acados. So if they are correctly declared, acados will not compile and build external functions.

Therefore in my implementation I do two steps:

I look into your constraints and check if the constraints is double-sided. Case 1., 2., 3., they simply look to different possible structure of the constraint Jacobian. Example: you would enter case 1 if you have constraints only on the state, case 2 if only on the control, case 3 if constraints are both on state and control.

I extract lower and upper bound from your generic expression h

I now have all the fields h, lh, uh for initial, path and terminal constraints. I populate the acados OCP object with these fields. Then, I call the acados helper function for detecting if this constraints are simple bounds or linear.

If such constraint structures are detected, the acados detecting constraints directly writes in your ocp object

adamhall

Looks pretty great! I think my comments are similar to @Federico-PizarroBejarano and @MingxuanChe. I managed to run your fork (with some small changes to the t_wall stuff) and got similar results to what you posted.

I think the biggest thing is about the constraint parsing. We can either keep it in mpc_acados or modify constraints.py to keep have a way to give the constraints in the form that is best. Does anyone have a preference?

adamhall · 2024-12-13T22:53:57Z

safe_control_gym/controllers/mpc/mpc_acados.py

@@ -260,38 +260,120 @@ def processing_acados_constraints_expression(self,
        ocp.constraints.ubu = self.env.constraints.input_constraints[0].upper_bounds
        ocp.constraints.idxbu = idxbu # active constraints dimension
        '''
-
-        ub = {'h': set_acados_constraint_bound(h_expr, 'ub', self.constraint_tol),


This is very interesting! I didn't realize that the solvers care that much about how constraints are specified. I wonder if instead of having this block of code we modify or add something to constraints.py to handle different formulations. For example, the DefaultConstraint class is a child of BoudnedConstraint where we could add an option or function that creates them in the form of lb < h(x,u) < ub. On the contrary, this would only be useful in mpc_acados, so maybe it makes sense to do the parsing in mpc_acados, unless ipopt also benefits from having this constraint structure?

If we do keep the code likee this, I think some additional comments, like the ones in your comment above, might be helpful when we have to edit or debug in the future.

adamhall · 2024-12-13T22:56:08Z

safe_control_gym/controllers/mpc/mpc_acados.py

+                    lb.update({'he': set_acados_constraint_bound(he_expr, 'lb')})
+
+        if ub != {}:
+            # make sure all the ub and lb are 1D numpy arrays


again, I wonder if we can ensure the bounds are 1D numpy arrays in the constraint class such that this code isn't needed here.

adamhall · 2024-12-13T22:58:35Z

safe_control_gym/experiments/base_experiment.py

+            return self.data['controller_data'][0]['t_wall'][0]
+        except:
+            return np.nan
+


I think the problem with this is that there is a lot of logging that goes on inside some of the select_action functions. If we can separate out that logging, then I think the timings from base_experiment would make sense, but it might be necessary that all the algo's have their own timing routine as they all log a little differently?

aghezz1 · 2024-12-23T09:30:29Z

I think the biggest thing is about the constraint parsing. We can either keep it in mpc_acados or modify constraints.py to keep have a way to give the constraints in the form that is best. Does anyone have a preference?

Would be cool to modify constraints.py (cf. my comment above), I could have a look into it but might be more efficient if some of you join, and we do a session together.

aghezz1 added 4 commits December 5, 2024 10:38

improve acados mpc

205dcc4

fix call to casadi integrators to avoid warning for deprecation

044d812

incluse t_wall to stats report

bb47497

get t_wall from acados mpc

a70f8e6

aghezz1 mentioned this pull request Dec 5, 2024

Improve acados mpc formulation #182

Closed

aghezz1 added 3 commits December 5, 2024 12:29

improving plots

0eb0b29

improving plots 2

08e293d

add warmstart_type field for acados, avoid multiple compilation of ac…

6702a4d

…ados solver and mc

aghezz1 marked this pull request as ready for review December 5, 2024 14:23

Federico-PizarroBejarano requested review from Federico-PizarroBejarano, adamhall and MingxuanChe and removed request for adamhall December 6, 2024 16:55

Federico-PizarroBejarano assigned aghezz1 Dec 6, 2024

Federico-PizarroBejarano added the enhancement New feature or request label Dec 6, 2024

Federico-PizarroBejarano reviewed Dec 6, 2024

View reviewed changes

fixing t_wall metrics for other controllers and mc

264afd5

renaming warm starting strategy

cfe0159

MingxuanChe reviewed Dec 11, 2024

View reviewed changes

aghezz1 commented Dec 12, 2024

View reviewed changes

adamhall requested changes Dec 13, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Improvements over acados MPC formulation #183

Improvements over acados MPC formulation #183

aghezz1 commented Dec 5, 2024 •

edited

Loading

Federico-PizarroBejarano left a comment

Federico-PizarroBejarano Dec 6, 2024

aghezz1 Dec 9, 2024

Federico-PizarroBejarano Dec 9, 2024

MingxuanChe Dec 11, 2024

adamhall Dec 13, 2024

Federico-PizarroBejarano Dec 6, 2024

aghezz1 Dec 9, 2024

Federico-PizarroBejarano Dec 9, 2024

MingxuanChe Dec 11, 2024

aghezz1 Dec 12, 2024

Federico-PizarroBejarano commented Dec 9, 2024

aghezz1 commented Dec 11, 2024

MingxuanChe commented Dec 11, 2024

MingxuanChe left a comment

MingxuanChe Dec 11, 2024

aghezz1 Dec 12, 2024

MingxuanChe Dec 11, 2024

aghezz1 Dec 12, 2024

adamhall Dec 13, 2024

aghezz1 Dec 23, 2024

aghezz1 commented Dec 12, 2024

aghezz1 Dec 11, 2024

adamhall Dec 13, 2024

aghezz1 Dec 23, 2024

aghezz1 Dec 12, 2024

aghezz1 Dec 12, 2024

aghezz1 Dec 12, 2024

adamhall left a comment

adamhall Dec 13, 2024

adamhall Dec 13, 2024

adamhall Dec 13, 2024

aghezz1 commented Dec 23, 2024

Improvements over acados MPC formulation #183

Are you sure you want to change the base?

Improvements over acados MPC formulation #183

Conversation

aghezz1 commented Dec 5, 2024 • edited Loading

Federico-PizarroBejarano left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Federico-PizarroBejarano commented Dec 9, 2024

aghezz1 commented Dec 11, 2024

MingxuanChe commented Dec 11, 2024

MingxuanChe left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

aghezz1 commented Dec 12, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

adamhall left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

aghezz1 commented Dec 23, 2024

aghezz1 commented Dec 5, 2024 •

edited

Loading