Extracting feedback-gain matrices from d_ocp_qp_ipm_ws about hpipm HOT 18 CLOSED

Update: I noticed that the above extraction of TVLQR-feedback works also for the first-stage, if I set the settings flag ric_alg=1 via d_ocp_qp_ipm_arg_set_ric_alg(&ric_alg, &arg);
So when the square-root ricatti algorithm is active, it seems to work.
Just for my understanding -- what actually happens to the workspace_.L[0] variable when ric_alg=0?

from hpipm.

Hi, that's a good hint!

When ric_alg=0 the Riccati is implemented using a different algorithm (the 'classical' Riccati recursion, without the cholesky factorization of the recursion matrix).

I'll look into adding a proper getter to get the feedback matrix, such that this works regardless of the algorithm used (and if the algorithm is changed again in the future), following what I wrote in the other issue. Get back on that soon!

from hpipm.

Hi @giaf,
thanks for clarifying this! I would welcome a dedicated getter-routine for the Feedback-matrices very much, and it may also help to get further users in the DDP/iLQR communities!
Speaking of this, I noticed that a getter method for another quantity would be incredibly helpfull, too.
To simplify the discussion, let's take a look at equations, maybe this paper here. The relevant equation is (6):
In here the control input update gets computed as

and the "feedback matrix" in question is It would be absolutely fantastic if you could also add a getter to the second quantity in Eq. (6), that is , which is basically a feedforward term.
Thanks a lot in advance!!!

from hpipm.

Ok, these days here we had a group retreat and now I'm going on holidays for a few days, so it will need a bit more time.

Btw, what is this peace of code doing? I'm not familiar with eigen

TVLQR_K_ [i] = (-Ls * Lr.partialPivLu().inverse()).transpose();

Is Lr.partialPivLu() taking the LU factorization of Lr? In theory Lr should already contain the lower triangular Cholesky factorization, so no extra factorization should be needed.

Anyway, I will study the references you sent me, to be sure to implement exactly what is needed.

from hpipm.

Thanks a lot @giaf, there is no urgency here.
You're right about the LU factorization, this is redundant.
Let me know if you have questions, I can also send you different references in case the above mentioned one is too general.

from hpipm.

Hi Markus,

I added the getters, an example of the usage is here
https://github.com/giaf/hpipm/blob/master/examples/c/example_d_ocp_qp_unconstr.c#L375

There are getters for the matrices Lr, Ls and P. The feedback matrix K can be easily computed as in your example, as

K= - (Ls * inv(Lr))'

(where the inverse is only for mathematical notation and it should be replaced by a solution of a triangular system).
If the initial state is eliminated from the optimization variables before calling HPIPM, then at the first stage the matrix K has to be computed externally, as you are already doing. However, now the getter for P returns the correct value for the Riccati recursion matrix for all algorithms in HPIPM.

There are getters also for the vectors ls and p, but these are only valid in case of unconstrained MPC problems. In case of constrained problems, they may return different values depending on the chosen algorithm (i.e. absolute vs relative formulation, enabling of predictor-corrector and so on).
The feedback vector k is computed as

k = - inv(Lr) * ls

(where the inverse is only for mathematical notation and it should be replaced by a solution of a triangular system).

from hpipm.

Thanks for the quick implementation @giaf !
I will try to find a free time-slot for testing the new getters as soon as possible and come back to you soon!

from hpipm.

@giaf I was able to test the new getters and am glad to confirm that they match exactly what I needed! A quick clarification:
There is only a getter d_ocp_qp_ipm_get_ric_lr (not ls) and hence the feedback vector k should be computed as

k = - inv(Lr).transpose() * lr

Is this what you had in mind? In any case, it matches the quantity that I had cited above!

from hpipm.

Yes sorry, this is what I meant!

from hpipm.

Great! Do you think it would make sense to fast-forward hpipm release 0.1.1 to include these changes?

from hpipm.

It doesn't make sense now, since I'm also in the middle of some work on QCQP. That stuff has rather high priority, so I would expect to finalize it in the next few weeks. At that point, I would advance the version to 0.1.2.

For now the best option for you would be to refer to the current commit 806c845

from hpipm.

Okay, that sounds good to me! I wil get notified about the new release automatically anyway. All the best, and thanks again!

from hpipm.

@markusgft I recently did some more work on this, now there are also routines to directly get the feedback gains K and k. All these routines now also work for the constrained case, and their API is slightly changed now, in case you want to upgrade.

from hpipm.

Hi @giaf, thanks for the heads up!
I upgraded to your new interface and did some tests (comparison to LQR/iLQR backward pass), with the following findings:

• the vector that you call k, i.e. the feedforward term, matches the iLQR feedforward for all stages
• the feedback matrix K is computed indentically to the LQR bakward pass for all stages but for the first.
  I believe the reason for that behaviour is that for stage k=0, HPIPM does not have meaningful entries for the matrix that you call Ls. What I did so far, is that I manually reconstructed the LQR feedback matrix for stage 0, using the following trick:

  // get Lr for stage 0 and compute its inverse, Lr_inv_ (omitted)
  // ...
  // retrieve Riccati matrix for stage 1 (we call it S, others call it P)
    Eigen::Matrix<double, state_dim, state_dim> S1;
    ::d_ocp_qp_ipm_get_ric_P(&qp_, &arg_, &workspace_, 1, S1.data());

    // step2: compute G[0]
    Eigen::Matrix<double, control_dim, state_dim> G;
    G = p.U_[0]; // cross-terms derivative 
    G.noalias() += p.B_[0].transpose() * S1 * p.A_[0];

    // step3: compute K[0] = H.inverse() * G
    K_[0] = (-Lr_inv_[0].transpose() * Lr_inv_[0] * G);   // <== this is the correct LQR feedback gain matrix for stage 0

Do you think it makes sense for you to adapt the computation of K[0] on hpipm side? Thanks in advance!

from hpipm.

Hi @markusgft thanks for the feedback!
Could you give me some more details on the setup? Like, constrained or unconstrained problem? Value of nx[0]? (i.e. did you remove x0, or is it used for equal upper and lower bounds?)

from hpipm.

Hi @giaf, sure!
The minimal setup that I was testing was an unconstrained problem, and

nu_[N_] = 0;                      // last input is not a decision variable
nx_[0] = 0;                       // initial state is not a decision variable but given

I hope that helps!

from hpipm.

Ok then this is the reason for K[0]: in HPIPM this is a matrix of size nu[0] x nx[0], so 0. This is the intended behavior, since e.g. A[0] is not even part of the QP formulation, so there is no way I can compute this internally anyway.

If you keep x[0] as an optimization variable and set the relative upper and lower bounds to x0, then you would also get a meaningful K[0]. But this is much slower (and likely less accurate) in your case, since instead of solving an unconstrained MPC problem with a single Riccati swap, you would solve a constrained MPC problem using IPM and one Riccati swap per IPM iteration.

from hpipm.

Thank you @giaf for the clarification.
I will stick to the manual computation of the feedback gains for the first stage ... your new feature is very useful, since it allowed me to eliminate many redundant computations.
I saw that you released a tag for hpipm 0.1.2.... are you going to create a matching tag for blasfeo as well?

from hpipm.