DARE does not solve and stuck in while loop

[guzhaoyuan]

I am trying to use the LQR controller to track state for cart_pole.

My plant was created by parsing the cart_pole.sdf. This create a discrete system.

When create the LQR controller, it linearizes the system around the desired fixed point and try to solve the LQR controller K using the function DiscreteAlgebraicRiccatiEquation.

The program stuck in the function DiscreteAlgebraicRiccatiEquation. https://github.com/RobotLocomotion/drake/blob/0816ac05dd9e03bbe5ccdbedf7e32bd72a885474/math/discrete_algebraic_riccati_equation.cc#L375-L425

It is the while loop in function reorder_eigen() blocks the whole program. https://github.com/RobotLocomotion/drake/blob/0816ac05dd9e03bbe5ccdbedf7e32bd72a885474/math/discrete_algebraic_riccati_equation.cc#L414

To re-produce this issue, pull from https://github.com/guzhaoyuan/drake/tree/cart_pole

bazel run //examples/multibody/cart_pole:cart_pole_lqr

[jwnimmer-tri]

This might be related to #8034?

[guzhaoyuan]

This might be related to #8034?

Looks like a same issue, with different system A B and Q R.

FYI:

[guzhaoyuan]

Problem solved. By applying patch from weiqiao's fix, the DARE solver works. The process is

git remote add weiqiao https://github.com/weiqiao/drake.git
git fetch weiqiao
git cherry-pick e777b31f04ec1d176a33d018669f62e9d3924e72
git cherry-pick aee6342eb01b142f5f109c6dba8eafa8f5994601
git cherry-pick a6f90a80f6842a67fe596c748e5b71eb56a7500a