Add stability constraints to the Park 2004 model.

[moorepants]

Fixes #251

This change requires the development version of SymPy: 1.15.dev0.

SymPy recently gained a feature to calculate the stability criteria for a linear system. It uses the Routh-Hurwitz method to find the set of inequalities that guarantee the roots of the characteristic equation all have negative real parts. opty recently gained the functionality to add extra inequality constraints to the equations of motion. This is particularly useful for problems where you want to find parameters that ensure asymptotic stability of the model. The Park 2004 example is one such problem. Here we identify the control parameters from measurement data for the nonlinear human standing model but also linearize the dynamics model and can ensure that the identified control parameters will guarantee stability of the closed loop linear model.

TODO

• Improve get_asymptotic_stability_criteria() upstream in SymPy, StateSpace.get_asymptotic_stability_conditions() hangs on state matrices with moderately long matrix entry expressions sympy/sympy#28010
• Figure out how to include this example with a development version of SymPy since 1.15 will take a year to release.

[moorepants]

This now works if the line in sympy is changed to determinant = self.A.charpoly(s) (the determinant call was not completing).

[moorepants]

The found closed loop gains give stable eigenvalues:

In [5]: (h.gain_scale_factors.flatten()*ps).reshape(2, 4)
Out[5]: 
array([[982.30185469, 157.07713533, 172.95396715,  43.99265946],
       [ 52.93813837, 285.14860231,  54.81236929,  23.62525222]])

In [6]: Av - Bv@(h.gain_scale_factors.flatten()*ps).reshape(2, 4)
Out[6]: 
array([[  0.        ,   0.        ,   1.        ,   0.        ],
       [  0.        ,   0.        ,   0.        ,   1.        ],
       [-20.29629028,  15.00255964,  -0.68010049,   0.9255682 ],
       [ 71.09443324, -61.06920624,  -5.56308184,  -5.86417512]])

In [7]: np.linalg.eig(Av - Bv@(h.gain_scale_factors.flatten()*ps).reshape(2, 4))
Out[7]: 
EigResult(eigenvalues=array([-2.2039178 +8.60448279j, -2.2039178 -8.60448279j,
       -1.06822001+1.02478701j, -1.06822001-1.02478701j]), eigenvectors=array([[ 0.00727927+0.0266394j ,  0.00727927-0.0266394j ,
        -0.19984966-0.25601935j, -0.19984966+0.25601935j],
       [-0.02690066-0.10502492j, -0.02690066+0.10502492j,
        -0.32900705-0.31562988j, -0.32900705+0.31562988j],
       [-0.24526114+0.00392328j, -0.24526114-0.00392328j,
         0.47584871+0.06868165j,  0.47584871-0.06868165j],
       [ 0.96297199+0.j        ,  0.96297199-0.j        ,
         0.67490531+0.j        ,  0.67490531-0.j        ]]))

In [8]: ev, _ = np.linalg.eig(Av - Bv@(h.gain_scale_factors.flatten()*ps).reshape(2, 4))

In [9]: ev
Out[9]: 
array([-2.2039178 +8.60448279j, -2.2039178 -8.60448279j,
       -1.06822001+1.02478701j, -1.06822001-1.02478701j])

[moorepants]

I opened this SymPy issue regarding the limitations of the stability conditions function: sympy/sympy#28010

[moorepants]

@chris-konrad and @tvdbogert, this may be of interest. It still needs some tweaks upstream in SymPy for it to work smoothly, but this demonstrates adding criteria to guarantee stability in a parameter identification problem.

[leomanga]

Hello, the stability criteria should now work correctly (sympy/sympy#28265).

[moorepants]

This works with SymPy master. Nice! I think I'll wait to merge (if I do) for when SymPy 1.15 comes out.

[moorepants]

Getting this unrelated error in the docs build:

Extension error:
Here is a summary of the problems encountered when running the examples:

Unexpected failing examples (1):

    ../examples-gallery/beginner/plot_brachistochrone.py failed leaving traceback:

    Traceback (most recent call last):
      File "/usr/share/miniconda/envs/opty-dev/lib/python3.9/site-packages/sphinx_gallery/scrapers.py", line 389, in save_figures
        rst = scraper(block, block_vars, gallery_conf)
      File "/usr/share/miniconda/envs/opty-dev/lib/python3.9/site-packages/sphinx_gallery/scrapers.py", line 157, in matplotlib_scraper
        image_rsts.append(_anim_rst(anim, image_path, gallery_conf))
      File "/usr/share/miniconda/envs/opty-dev/lib/python3.9/site-packages/sphinx_gallery/scrapers.py", line 250, in _anim_rst
        anim.save(str(image_path), writer=writer, dpi=use_dpi)
      File "/usr/share/miniconda/envs/opty-dev/lib/python3.9/site-packages/matplotlib/animation.py", line 1089, in save
        writer.grab_frame(**savefig_kwargs)
      File "/usr/share/miniconda/envs/opty-dev/lib/python3.9/contextlib.py", line 137, in __exit__
        self.gen.throw(typ, value, traceback)
      File "/usr/share/miniconda/envs/opty-dev/lib/python3.9/site-packages/matplotlib/animation.py", line 225, in saving
        self.finish()
      File "/usr/share/miniconda/envs/opty-dev/lib/python3.9/site-packages/matplotlib/animation.py", line 495, in finish
        self._frames[0].save(
    IndexError: list index out of range