Add MPC by kjohnsen · Pull Request #29 · CLOCTools/lds-ctrl-est

kjohnsen · 2024-06-12T19:08:18Z

No description provided.

The commit that the vcpkg submodule was linked to depended on a version of zlib that was downloaded from the zlib website that is outdated. This commit updates the vcpkg to the newest version, which gets zlib directly from the github repository.

Roughly implemented the example code for MPC controller, based on the example from cloctools/lqmpc.

… dependency

- The code for the MpcController is partially implemented (only necessary methods have been added, getters and setters need to be added) - Code still needs to be debugged (there are multiple memory leaks that need to be fixed)

include/ldsCtrlEst_h/lds_mpcctrl.h

kjohnsen · 2024-06-12T21:13:27Z

include/ldsCtrlEst_h/lds_mpcctrl.h

+    Matrix Px   = arma::kron(Matrix(N_, N_, arma::fill::eye), Q_);
+    Matrix Pu1  = arma::kron(Matrix(M_, M_, arma::fill::eye), 2 * S_ + R);
+    Matrix Pu2  = arma::kron(Matrix(eye_offset(M)) + Matrix(eye_offset(M, 1)), -S_);
+    Matrix Pu3  = block_diag(Matrix((M_ - 1) * m_, (M_ - 1) * m_, arma::fill::zeros), -S_);
+    Matrix Pu = Pu1 + Pu2 + Pu3;
+    P_ = arma::trimatu(2 * block_diag(Px, Pu)); // Taking only the upper triangular part


Are these sparse, like in the Python lqmpc? That P matrix gets pretty big, right?

They aren't sparse right now, but I think I should be able to make them sparse. Speaking of sparse matrices, is it okay if I define a type for sparse matrices in lds.h like the Matrix and Vector types?

Yep, I think it would be fine to define a sparse matrix type in lds.h

kjohnsen · 2024-06-12T21:22:26Z

include/ldsCtrlEst_h/lds_mpcctrl.h

+
+ private:
+  /**
+   * @brief     Set the matrix for the OSQP solver from an Armadillo matrix


this doxygen comment (and for from_sparse and from_vec) is misleading; it sounds like it will directly set a matrix on the controller, but instead just does the conversion

Ok, I will fix that!

kjohnsen · 2024-06-12T21:26:56Z

include/ldsCtrlEst_h/lds_mpcctrl.h

+   * 
+   * @return    The identity matrix with an offset axis
+   */
+  arma::SpMat<data_t> eye_offset(size_t n, int k = -1) {


Not sure if it helps if its output is dense, but FYI arma has functions to help do this: https://arma.sourceforge.net/docs.html#diagmat, https://arma.sourceforge.net/docs.html#diag, https://arma.sourceforge.net/docs.html#diags_spdiags

I didn't see that! I think that will make it easier to do.

include/ldsCtrlEst_h/lds_mpcctrl.h

kjohnsen · 2024-06-12T21:33:52Z

include/ldsCtrlEst_h/lds_mpcctrl.h

+// Implement methods
+
+template <typename System>
+inline MpcController<System>::MpcController(const System& sys, Vector u_lb,


might as well put inline functions above in the class definition

kjohnsen · 2024-06-12T21:35:54Z

include/ldsCtrlEst_h/lds_mpcctrl.h

+                                      const Vector& u0, const Matrix& xr) {
+  size_t n_sim = t_sim / sys_.dt();  // Number of points per simulation step
+
+  OSQPSolution* sol;


Does the optimization problem get reinitialized on every step? That will probably be a lot slower than if parts can be reused and just the state is updated

Yes, I think the problem does get reinitialized every step. I mostly was trying to stick to what the Python package had (except currently the fast update function is never called, which I haven't fixed yet because the slow update doesn't work correctly), but I can look more into how to keep some of the previous states to prevent reinitializing at every step.

kjohnsen · 2024-06-12T21:44:01Z

include/ldsCtrlEst_h/lds_mpcctrl.h

+  // Solver, settings, matrices
+  OSQPSolver* solver;
+  OSQPSettings* settings;
+  OSQPCscMatrix* A = from_matrix(Acon_);
+  OSQPCscMatrix* P = from_matrix(P_);
+  OSQPFloat* q = from_vec(q_arma.t());
+  OSQPFloat* lb = from_vec(lb_);
+  OSQPFloat* ub = from_vec(ub_);
+
+  // Set settings
+  settings = (OSQPSettings*)malloc(sizeof(OSQPSettings));
+  if (settings) {
+    osqp_set_default_settings(settings);
+    settings->verbose = false;
+  }


I see that this follow the example of Python lqmpc, where prob.setup() is called every step. But is there a way to keep the solver, settings, matrices, vectors, etc. around from one step to the next? I imagine this would help things go faster, and might help us keep memory leaks under control since pointers wouldn't be created on every step

I didn't see this comment before I responded to the previous one, but I think that is probably what I should be aiming for and I will try my best to reuse as many of the variables as possible.

kjohnsen · 2024-06-12T21:48:53Z

include/ldsCtrlEst_h/lds_mpcctrl.h

+    throw std::runtime_error(error_message);
+  }
+
+  OSQPSolution* sol = std::move(solver->solution);


is this recommended by OSQP? there aren't memory problems with osqp_cleanup later after moving the solution?

I'm not sure. I only did this because I was trying to avoid copying data over again and again, but now that I am looking at it it might cause memory leaks. Let me look into the code a little bit more and see what exactly is going on, and I will try to make sure that there are no memory problems between copying the OSQP solution and clean up and freeing up memory.

kjohnsen · 2024-06-12T21:50:34Z

include/ldsCtrlEst_h/lds_mpcctrl.h

+
+  // Clean up
+  osqp_cleanup(solver);
+  if (q) free(q);


why free() instead of delete? (not saying delete is right; I'm not an expert on the finer points of memory management). Though these wouldn't be necessary if we reuse memory from step to step

As far as I know, free() is for freeing memory allocated with malloc, whereas delete is used for freeing memory allocated with new, but I will verify that to make sure I'm not doing anything wrong.

The follow-up question then is why malloc instead of new?

I don't actually have a good reason for using either. I mostly just followed what the other controllers did and tried to align closely to that.

kjohnsen · 2024-06-12T22:10:31Z

include/ldsCtrlEst_h/lds_mpcctrl.h

+  for (OSQPInt j = 0; j < mat->n; j++) {
+    for (OSQPInt i = 0; i < mat->m; i++) {
+      if (A(i, j) != 0) {
+        mat->i[n] = i;


some comments would be good here to be able to follow what's going on with i, x, p, n, etc.

kjohnsen · 2024-06-12T22:11:53Z

include/ldsCtrlEst_h/lds_mpcctrl.h

+  mat->nzmax = A.n_nonzero;
+  mat->nz = -1;  // -1 means the matrix is in CSC format (required for API)
+
+  mat->p = (OSQPInt*)malloc((mat->n + 1) * sizeof(OSQPInt));


again, hard to see if there's a potential memory bug here when I don't know what p, i, x are

I will fix that. Those are variables associated with the OSQPCscMatrix type.

OSQPInt *p¶
column pointers (size n+1); col indices (size nzmax) starting from 0 for triplet format

OSQPInt *i¶
row indices, size nzmax starting from 0

OSQPFloat *x¶
numerical values, size nzmax

kjohnsen · 2024-06-12T22:14:25Z

include/ldsCtrlEst_h/lds_mpcctrl.h

+    throw std::runtime_error(error_message);
+  }
+
+  arma::sp_mat::iterator it = A.begin();


I'm guessing it goes over elements and j is the index of the element?

kjohnsen

Overall, awesome job @i3ta! It's amazing how you put together so much technical stuff on the first draft with so little direction

Defined and renamed arma::SpMat<data_t> to the Sparse shorthand

…apper for OSQP that interfaces with Armadillo matrices

…ript

… for fast update

…es if problem is updated

Update OSQP Installation Process

…update method

…implement-mpc

…ll-state MPC

…utReference method

Added MPC control for Poisson system, and added example to show it in action

i3ta and others added 6 commits May 12, 2024 13:12

Update vcpkg version

22be42d

The commit that the vcpkg submodule was linked to depended on a version of zlib that was downloaded from the zlib website that is outdated. This commit updates the vcpkg to the newest version, which gets zlib directly from the github repository.

Implemented foundations for MPC controller example

f6d1f14

Roughly implemented the example code for MPC controller, based on the example from cloctools/lqmpc.

Updated vcpkg version

29b6a9c

Added osqp as a submodule and updated CMakeLists to include osqp as a…

281fafb

… dependency

Corrected matrix multiplication syntax

0e5048e

Implemented part of the code for MpcController

2f386be

- The code for the MpcController is partially implemented (only necessary methods have been added, getters and setters need to be added) - Code still needs to be debugged (there are multiple memory leaks that need to be fixed)

kjohnsen commented Jun 12, 2024

View reviewed changes

include/ldsCtrlEst_h/lds_mpcctrl.h Show resolved Hide resolved

kjohnsen commented Jun 12, 2024

View reviewed changes

include/ldsCtrlEst_h/lds_mpcctrl.h Show resolved Hide resolved

kjohnsen commented Jun 12, 2024

View reviewed changes

i3ta and others added 10 commits June 14, 2024 16:30

Renamed sparse matrices

7f4976f

Defined and renamed arma::SpMat<data_t> to the Sparse shorthand

Refactored OSQP to a new osqp_arma.h file, which acts as a partial wr…

0d7fe2e

…apper for OSQP that interfaces with Armadillo matrices

Added .idea to .gitignore for Jetbrains IDEs

86c9baf

Stored data and began adding visualization to example

1a73ea5

Fixed problems with example script and added lqmpc plotting Matlab sc…

6f4a300

…ript

Added option to get cost from control, and plotted cost in example

a6ad152

Optimized calculation of problem for slow update and fixed conditions…

d050d9f

… for fast update

Added documentation for the Control method

ddbe2b6

Optimized controller so it saves the OSQP object and only sets matric…

48bdd71

…es if problem is updated

automate OSQP build

8db0a88

kjohnsen and others added 10 commits July 3, 2024 11:56

removed osqp submodule

9598e98

Merge pull request #30 from CLOCTools/kyle-mpc

87e4c44

Update OSQP Installation Process

Refactored slow_update to calc_trajectory and removed less used fast_…

accd3a9

…update method

Merge remote-tracking branch 'origin/aaron-implement-mpc' into aaron-…

7b95ae0

…implement-mpc

Modified control to use measurements and estimate state instead of fu…

c451f93

…ll-state MPC

Updated and removed some unnecessary variables, and added ControlOutp…

c719b75

…utReference method

Began implementing eg_plds_mpc

6e01606

Added MPC implementation for Poisson system

0344473

Added MPC control for Poisson system, and added example to show it in action

Fixed OSQP installation

0232f60

Removed unnecessary osqp submodule

14498e9

Comments

Conversation

kjohnsen commented Jun 12, 2024

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

kjohnsen Jun 12, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

kjohnsen Jun 12, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

kjohnsen Jun 12, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

kjohnsen left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

kjohnsen Jun 12, 2024 •

edited

Loading

kjohnsen Jun 12, 2024 •

edited

Loading

kjohnsen Jun 12, 2024 •

edited

Loading

kjohnsen left a comment •

edited

Loading