Riccati Feedback Matrices in Python Interfaces

[reichardtj]

For educational purposes, I'm trying to implement a multiple shooting Gauss Newton method in python using HPIPM according to Giftthaler et al's really nice paper https://arxiv.org/pdf/1711.11006.pdf - there in equations 14 and 15, authors use the feedback matrices from the Riccati recursion in order to calculate control and feedbacks that reduce the defects at the connection points of the multiple shooting methods.

Giftthaler's control toolbox implementation makes use of these feedback matrices also in the HPIPM interface: https://github.com/ethz-adrl/control-toolbox/blob/e7a29a6b28dd516563b8be84d8980897cd2b10aa/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp#L316-L352

Unfortunately, the feedback matrices are not in the python interface, hence, I've provided the PR #115 .

Adding the following to the example https://github.com/giaf/hpipm/blob/master/examples/python/example_qp_getting_started.py

idx = 4
print('ric_Ls', solver.get_feedback(qp, 'ric_Ls', idx))
print('ric_Lr', solver.get_feedback(qp, 'ric_Lr', idx))
print('ric_lr', solver.get_feedback(qp, 'ric_lr', idx))
print('ric_P', solver.get_feedback(qp, 'ric_P', idx))
print('ric_p', solver.get_feedback(qp, 'ric_p', idx))
print('ric_k', solver.get_feedback(qp, 'ric_k', idx))
#print('ric_K',  solver.get_feedback(qp, 'ric_K', idx))

produces the following output:

ric_Ls
[[       0.        ]
 [16495664.91858685]]
ric_Lr [[16495664.91858691]]
ric_lr [[6.0621988e-08]]
ric_P
[[2.72106961e+14 2.72106961e+14]
 [2.72106961e+14 2.72106961e+14]]
ric_p
[[2.]
 [2.]]
ric_k [[-3.67502542e-15]]

I cannot judge if the huge values in ric_P, ric_Ls and ric_Lr are to be expected at the last stage and in the first stage for ric_p and ric_P - intermediate states produce "normal values". However, the primary issue is that solver.get_feedback(qp, 'ric_K', idx) crashes the python interpreter and I'm lost as to why since the interface in the PR is the same for all Riccati matrices. So maybe there is an issue in the c-code?

Also, it would be really nice to document the use of the Riccati recursion matrices. I'm confused by the names and the control-toolbox is not all that helpful:

https://github.com/ethz-adrl/control-toolbox/blob/e7a29a6b28dd516563b8be84d8980897cd2b10aa/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp#L333-L336

So a definite mapping of the functions in HPIPM to the notation in Giftthaler et al's paper would be really great.

Thanks a lot for considering this.

[reichardtj]

Indeed #117 (comment) and giaf/blasfeo@d4387e5 fixes the issue. Thanks for that.

The python interface is now working (and could be merged?):

idx = 2
print('ric_Ls', solver.get_feedback(qp, 'ric_Ls', idx))
print('ric_Lr', solver.get_feedback(qp, 'ric_Lr', idx))
print('ric_lr', solver.get_feedback(qp, 'ric_lr', idx))
print('ric_P', solver.get_feedback(qp, 'ric_P', idx))
print('ric_p', solver.get_feedback(qp, 'ric_p', idx))
print('ric_k', solver.get_feedback(qp, 'ric_k', idx))
print('ric_K',  solver.get_feedback(qp, 'ric_K', idx))

produces

ric_Ls [[1.66998988]
 [4.26865103]]
ric_Lr [[2.93892197]]
ric_lr [[0.34026082]]
ric_P [[3.20978816 2.77802029]
 [2.77802029 5.23047499]]
ric_p [[1.43176787]
 [1.5475453 ]]
ric_k [[-0.11577743]]
ric_K [[-0.56823213 -1.4524547 ]]

which leaves only the documentation to be desired and may a comment regarding the use of the matrices in the control toolbox