How Much Gain Can the Robot Arm Take?

A robot arm's joint is positioned by a proportional controller that reads the joint angle through a camera. The joint is a discrete plant B(z) / A(z) (coefficient lists, lowest power of z^-1 first), and the camera adds a delay of d samples, so the closed-loop poles are the roots of

A(z) + K * z^-d * B(z)       (b shifted right by d places, times K, added to a)

Raising K makes the arm stiffer and faster, until at some critical gain a pole reaches the unit circle and the arm starts to shake. Engineers then run it well below that gain (a gain margin), so they need to know where it is.

Write max_stable_gain(a, b, d, K_max) that finds the critical gain between 0 and K_max:

• the loop's radius at a gain K is the largest |z| of the roots of the polynomial above (numpy.roots);
• if the loop is still stable (radius below 1) at K_max, return K_max;
• otherwise bisect: keep lo stable (it starts at 0) and hi unstable (it starts at K_max); test the middle and move the end that has the same verdict; stop when hi - lo is below 1e-10 * max(1, hi) and return (lo + hi) / 2.

Any accurate method gets the same answer within the tolerance; bisection is the simple one that never fails.

Examples

Input:  a = [1, -0.9], b = [0, 0.2], d = 0, K_max = 100
Output: 9.5
Explanation: one pole at 0.9 - 0.2 K, which reaches -1 at K = 9.5.

Input:  a = [1, -0.9], b = [0, 0.2], d = 1, K_max = 100
Output: 5.0
Explanation: z^2 - 0.9 z + 0.2 K has complex poles of radius sqrt(0.2 K), which is 1 at K = 5.
One sample of camera delay has nearly halved the usable gain.

Input:  a = [1, -0.9], b = [0, 0.2], d = 0, K_max = 4
Output: 4
Explanation: still stable at the largest gain allowed.

Constraints

• the plant is stable (K = 0 is stable), and once a gain is unstable every larger gain up to K_max is unstable too, so there is exactly one critical gain
• no test has its critical gain within 1e-6 of K_max
• answers are compared with a tolerance of 1e-6

Goals

• Find the largest stable proportional gain by bisection on the pole radius
• Keep a bisection's invariant: the low end stable, the high end unstable
• Measure how a delay in the loop lowers the gain a loop can take