Recall from previous lessons that the root locus of a unity-feedback single-input single-output (SISO) loop with open-loop transfer function \( L(s) = K G(s)H(s) \) is the set of roots of the characteristic equation

\[ 1 + K G(s)H(s) = 0, \quad K \in \mathbb{R}_{\ge 0}. \]

Writing \( G(s)H(s) = \dfrac{N(s)}{D(s)} \) with \( N(s), D(s) \) polynomials and \( \deg D = n \), the characteristic equation becomes

\[ D(s) + K N(s) = 0. \]

For every fixed real \( K \), this is a polynomial equation in \( s \) of degree \( n \). Let its roots be \( s_1(K),\dots,s_n(K) \). The root locus is the union of all such points in the complex plane:

\[ \mathcal{R} = \bigcup_{K \ge 0} \{\, s_i(K) : i=1,\dots,n \,\}. \]

Hand construction is based on qualitative rules (asymptotes, breakaway points, symmetry, etc.). Computer-aided plotting replaces geometric constructions with systematic numerical computation of \( s_i(K) \) on a dense grid of gains. For high-order systems, this is essential: the root locus may have complicated branching behavior that is impractical to sketch manually.

The generic numerical workflow for root locus plotting is summarized below.

flowchart TD
  A["Specify plant G(s)H(s) = N(s)/D(s)"] --> B["Choose K_min, K_max, number of samples"]
  B --> C["For each K in the grid: form D(s) + K N(s)"]
  C --> D["Compute all polynomial roots (closed-loop poles)"]
  D --> E["Store (K, roots)"]
  E --> F["Plot trajectories of roots in the complex plane"]
        

In robotic control, \( G(s) \) often represents a linearized joint or end-effector dynamics model. Computer-aided root locus is then used to tune proportional or PD gains so that the resulting closed-loop poles lie in regions corresponding to desired damping and speed.

Let the open-loop transfer function be written with a monic denominator (no loss of generality for control design):

\[ G(s)H(s) = \frac{N(s)}{D(s)} = \frac{b_m s^m + b_{m-1} s^{m-1} + \cdots + b_0} {s^n + a_{n-1} s^{n-1} + \cdots + a_0}, \quad m \le n. \]

The closed-loop characteristic polynomial is obtained from \( 1 + K G(s)H(s)=0 \), that is

\[ P(s,K) \triangleq D(s) + K N(s) = s^n + \alpha_{n-1}(K) s^{n-1} + \cdots + \alpha_0(K) = 0. \]

After aligning degrees of \( N(s) \) and \( D(s) \) by padding with zeros, the coefficients depend affinely on \( K \):

\[ \alpha_i(K) = a_i + K c_i,\quad i=0,\dots,n-1, \]

where \( c_i \) are determined by the numerator coefficients \( b_j \).

2.1 Companion-matrix representation

Numerical root-finding libraries often operate on polynomials directly. A complementary linear-algebra viewpoint uses the companion matrix. For each fixed \( K \), define

\[ C(K) = \begin{bmatrix} 0 & 1 & 0 & \cdots & 0 \\ 0 & 0 & 1 & \cdots & 0 \\ \vdots & \vdots & \ddots & \ddots & \vdots \\ 0 & 0 & \cdots & 0 & 1 \\ -\alpha_0(K) & -\alpha_1(K) & \cdots & -\alpha_{n-2}(K) & -\alpha_{n-1}(K) \end{bmatrix}. \]

Proposition: The eigenvalues of \( C(K) \) are exactly the roots of \( P(s,K) \).

Sketch of proof: The characteristic polynomial of \( C(K) \) is by construction \( \det(s I - C(K)) = P(s,K) \), a standard result in linear algebra. Therefore the eigenvalues of \( C(K) \) are the roots of \( P(s,K) \). Conversely, any root of \( P(s,K) \) is an eigenvalue of \( C(K) \). Thus computing the closed-loop poles reduces to an eigenvalue problem.

This formulation is important for numerical implementations in languages where high-quality eigenvalue solvers (e.g., LAPACK via Eigen in C++) are more accessible than specialized polynomial root routines.

A simple but effective root locus algorithm samples the gain \( K \) on a grid:

1. Choose \( K_{\min} \), \( K_{\max} \), and a number of samples \( N_K \).
2. For each grid point \( K_j = K_{\min} + (j-1)\Delta K \), compute the polynomial coefficients \( \alpha_i(K_j) \).
3. Compute the eigenvalues of \( C(K_j) \) or the roots of \( P(s,K_j) \).
4. Store all roots and plot them as a function of gain.

To assess stability, define the maximum real part of the closed-loop poles:

\[ \sigma_{\max}(K) \triangleq \max_{i=1,\dots,n} \Re\bigl(s_i(K)\bigr). \]

The system is asymptotically stable if and only if \( \sigma_{\max}(K) < 0 \). A computer program can approximate the largest admissible gain \( K_{\text{stab}} \) by scanning over \( K \) and detecting when \( \sigma_{\max}(K) \) crosses zero.

For robotic servo loops (e.g., joint-position control with proportional gain), this scan yields a safe range of gains where all closed-loop poles remain in the left half-plane, providing a first guarantee of stability before finer tuning.

A second numerical issue is branch tracking. Since roots are recomputed independently for each \( K_j \), the ordering of \( s_i(K_j) \) may change. To draw smooth trajectories, one can pair roots between successive values of \( K \) by minimal distance:

\[ \text{given } \{s_i(K_j)\},\ \{s_k(K_{j+1})\}, \ \text{connect pairs that minimize } \sum_i \bigl| s_i(K_j) - s_{\pi(i)}(K_{j+1}) \bigr|^2, \]

where \( \pi \) is a permutation. In practice, a greedy nearest-neighbor pairing in the complex plane is often sufficient.

Python, together with numpy, scipy, and the python-control library, provides a convenient environment for numerical root locus plotting. In robotics, this can be combined with modeling tools such as roboticstoolbox-python to linearize joint dynamics and design servo gains.

4.1 From-scratch root locus for a cubic plant

Consider the plant \( G(s) = \dfrac{K}{s(s+2)(s+5)} \) in unity feedback. The closed-loop characteristic polynomial is

\[ P(s,K) = s^3 + 7 s^2 + 10 s + K. \]

The code below constructs this polynomial for many values of \( K \), computes the roots, and plots the root locus.

import numpy as np
import matplotlib.pyplot as plt

# Closed-loop polynomial: P(s, K) = s^3 + 7 s^2 + 10 s + K
D = np.array([1.0, 7.0, 10.0, 0.0])   # coefficients of D(s)
N = np.array([0.0, 0.0, 0.0, 1.0])    # coefficients of N(s) (unity)

def closed_loop_coeffs(K):
    return D + K * N  # s^3 + 7 s^2 + 10 s + K

# K grid
K_min, K_max = 0.0, 80.0
num_K = 200
K_values = np.linspace(K_min, K_max, num_K)

roots = []

for K in K_values:
    coeffs = closed_loop_coeffs(K)
    r = np.roots(coeffs)
    roots.append(r)

roots = np.array(roots)  # shape (num_K, 3)

# Plot root locus
plt.figure()
for i in range(roots.shape[1]):
    plt.plot(np.real(roots[:, i]), np.imag(roots[:, i]), ".-")

plt.axvline(0.0, linestyle="--")  # imaginary axis
plt.xlabel("Real(s)")
plt.ylabel("Imag(s)")
plt.title("Root locus for G(s) = K / (s (s+2) (s+5))")
plt.grid(True)
plt.show()

# Optional: approximate stability range by checking Re(s) < 0
stable_K = []
for K, r in zip(K_values, roots):
    if np.all(np.real(r) < 0.0):
        stable_K.append(K)

print("Approximate stable K range from scan:",
      min(stable_K), "to", max(stable_K))
      

4.2 Using python-control and robotic models

With python-control, construction is simplified:

import control as ctrl

# Example robotic joint plant (linearized): G(s) = 1 / (J s^2 + B s)
J = 0.01  # kg m^2
B = 0.1   # N m s/rad
s = ctrl.TransferFunction.s
G_joint = 1 / (J * s**2 + B * s)

# Proportional control with gain K
# Root locus of loop transfer function K * G_joint
ctrl.root_locus(G_joint)  # opens a matplotlib window

# To highlight damping lines or desired pole locations
import matplotlib.pyplot as plt
plt.title("Root locus of robotic joint with proportional control")
plt.show()
      

In a robotics workflow, the parameters \( J \) and \( B \) can be obtained from a roboticstoolbox rigid-body model. Root locus then helps choose proportional gains that yield acceptable damping and natural frequency for joint motion.

In C++, polynomial root-finding can be implemented via the companion matrix and the Eigen library. This approach integrates naturally with ROS-based robotics stacks, where the same codebase may use Eigen for kinematics/dynamics and ros_control/control_toolbox for online controllers.

#include <iostream>
#include <vector>
#include <complex>
#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::VectorXcd;

// Build companion matrix for monic polynomial
// P(s) = s^n + a_{n-1} s^{n-1} + ... + a_0
MatrixXd companion(const std::vector<double>& a) {
    const std::size_t n = a.size();
    MatrixXd C = MatrixXd::Zero(n, n);
    for (std::size_t i = 0; i + 1 < n; ++i) {
        C(i, i + 1) = 1.0;
    }
    for (std::size_t i = 0; i < n; ++i) {
        C(n - 1, i) = -a[i];
    }
    return C;
}

int main() {
    // Example: P(s, K) = s^3 + 7 s^2 + 10 s + K
    double K_min = 0.0, K_max = 80.0;
    int numK = 50;

    for (int j = 0; j <= numK; ++j) {
        double K = K_min + (K_max - K_min) * j / static_cast<double>(numK);
        // coefficients a_0, a_1, a_2 for s^0, s^1, s^2
        std::vector<double> a = {K, 10.0, 7.0};  // [a0, a1, a2]
        MatrixXd C = companion(a);
        Eigen::EigenSolver<MatrixXd> es(C);
        VectorXcd eigvals = es.eigenvalues();

        std::cout << "K = " << K << " poles: ";
        for (int i = 0; i < eigvals.size(); ++i) {
            std::cout << eigvals[i] << " ";
        }
        std::cout << std::endl;
    }

    return 0;
}
      

In a ROS-based robotic application, one would typically use offline tools (like the above C++ or Python script) to determine suitable gains for a joint controller, then configure a control_toolbox::Pid object with these gains in a ros_control controller.

Java does not have a built-in polynomial root solver, but libraries such as Apache Commons Math provide complex root-finding routines. Robotics frameworks such as WPILib (for mobile robots) then use the resulting gains in real-time controllers.

import org.apache.commons.math3.complex.Complex;
import org.apache.commons.math3.analysis.solvers.LaguerreSolver;

public class RootLocusCubic {
    // P(s, K) = s^3 + 7 s^2 + 10 s + K
    public static Complex[] rootsForGain(double K) {
        // Coefficients in descending powers: [1, 7, 10, K]
        double[] coeffs = new double[] {1.0, 7.0, 10.0, K};
        LaguerreSolver solver = new LaguerreSolver();
        // Initial guess (not used by solveAllComplex internally)
        Complex[] roots = solver.solveAllComplex(coeffs, 0.0);
        return roots;
    }

    public static void main(String[] args) {
        double Kmin = 0.0, Kmax = 80.0;
        int numK = 20;
        for (int j = 0; j <= numK; ++j) {
            double K = Kmin + (Kmax - Kmin) * j / (double) numK;
            Complex[] roots = rootsForGain(K);
            System.out.print("K = " + K + " poles: ");
            for (Complex r : roots) {
                System.out.print(r + " ");
            }
            System.out.println();
        }
    }
}
      

In a WPILib-based robotic project, one might use such a utility to analyze the effect of proportional or PD gains on a linear joint model and then configure edu.wpi.first.math.controller.PIDController with the selected gains.

MATLAB's Control System Toolbox provides the classical rlocus function, and Simulink integrates it with graphical design tools. Robotics System Toolbox can generate linearized models of manipulators and mobile robots, which can be passed directly into root locus analysis.

7.1 Basic MATLAB example

% Plant: G(s) = K / (s (s+2) (s+5))
s = tf('s');
G = 1 / (s * (s + 2) * (s + 5));

figure;
rlocus(G);
title('Root locus of G(s) = K / (s (s+2) (s+5))');
grid on;

% Read off gain for a chosen dominant pole location, or use rlocfind
[k_selected, poles_selected] = rlocfind(G);
disp(k_selected);
disp(poles_selected);
      

7.2 Robotic joint from Robotics System Toolbox

% Import robot model and linearize a joint about a configuration
robot = importrobot('my_robot.urdf');
robot.DataFormat = 'row';
q0 = homeConfiguration(robot);

% Create a simple joint-space PD servo model in Simulink or as a
% linearized plant using dynamics functions (schematic example):
% [A,B,C,D] = linearizeJoint(robot, jointIndex, q0);
% G_joint = tf(ss(A,B,C,D));

% Once G_joint is available:
figure;
rlocus(G_joint);
title('Root locus of robotic joint servo loop');
grid on;
      

This workflow allows the designer to move from a geometric robot description (URDF/CAD) to a linear model in MATLAB, use root locus to select controller gains, and then deploy those gains back to Simulink or to real-time controllers (for example, Simulink Real-Time or ROS nodes).

flowchart TD
  M["Robot model (URDF/CAD)"] --> L["Linearize to joint plant G(s)"]
  L --> R["Compute root locus (MATLAB/Python)"]
  R --> K["Select controller gains"]
  K --> D["Deploy gains to embedded/ROS controller"]
        

Wolfram Mathematica has high-level support for control systems. A root locus can often be obtained with a single command once the transfer function model is defined. Robotic dynamics can be modeled via rigid body mechanics functions and then linearized for root locus analysis.

(* Define symbolic variables *)
Clear[s, k];
G = k/(s (s + 2) (s + 5));

(* Transfer-function and root locus *)
sys = TransferFunctionModel[G, s];

RootLocusPlot[sys, {k, 0, 80},
  PlotRange -> All,
  Frame -> True,
  FrameLabel -> {"Re(s)", "Im(s)"},
  PlotLegends -> {"Closed-loop poles"}]

(* Example: robotic joint plant *)
J = 0.01; B = 0.1;
Gjoint = k/(J s^2 + B s);
sysJoint = TransferFunctionModel[Gjoint, s];
RootLocusPlot[sysJoint, {k, 0, 300}]
      

In a robotics context, one may obtain \( J \) and \( B \) from a symbolic multibody model of a manipulator link, then use RootLocusPlot to design gains.

In the following problems, focus on the interplay between analytic properties of the characteristic polynomial and their computer-aided implementation.

Problem 1 (Characteristic polynomial for computation): Consider the unity-feedback system with \( G(s) = \dfrac{K}{s(s+3)(s+6)} \). Derive the closed-loop characteristic polynomial \( P(s,K) \) in monic form and express its coefficients as affine functions of \( K \). How would you pass this polynomial to a numerical root finder?

Solution:

First expand the denominator:

\[ s(s+3)(s+6) = s \bigl(s^2 + 9 s + 18 \bigr) = s^3 + 9 s^2 + 18 s. \]

The open-loop transfer function is \( G(s) = \dfrac{K}{s^3 + 9 s^2 + 18 s} \). For unity feedback, the characteristic equation is \( 1 + G(s) = 0 \), i.e.

\[ 1 + \frac{K}{s^3 + 9 s^2 + 18 s} = 0 \quad \Longleftrightarrow \quad s^3 + 9 s^2 + 18 s + K = 0. \]

Hence \( P(s,K) = s^3 + 9 s^2 + 18 s + K \). The coefficients in monic form are \( \alpha_2(K) = 9 \), \( \alpha_1(K) = 18 \), \( \alpha_0(K) = K \). These are affine in \( K \), namely \( \alpha_0(K) = 0 + 1 \cdot K \), \( \alpha_1(K) = 18 + 0 \cdot K \), \( \alpha_2(K) = 9 + 0 \cdot K \).

In a numerical routine (Python, C++, or Java), for each gain \( K \) we construct the coefficient array \( [1,\alpha_2(K),\alpha_1(K),\alpha_0(K)] \) and pass it to a polynomial root solver or convert it to a companion matrix.

Problem 2 (Routh-based check of numerical stability scan): For the cubic polynomial from the Python example, \( P(s,K) = s^3 + 7 s^2 + 10 s + K \), use the Routh–Hurwitz criterion to find the exact range of \( K \) for which the system is asymptotically stable. Compare your analytic interval with the approximate interval obtained by numerical scanning.

Solution:

A general cubic \( s^3 + a s^2 + b s + c \) has all roots with negative real parts if and only if \( a > 0 \), \( b > 0 \), \( c > 0 \), and \( a b > c \).

Here \( a = 7 \), \( b = 10 \), \( c = K \). The conditions are:

\[ 7 > 0,\quad 10 > 0,\quad K > 0,\quad 7 \cdot 10 > K. \]

Thus the system is stable if and only if \( 0 < K < 70 \). A numerical scan that searches for \( \sigma_{\max}(K) < 0 \) on a fine grid should return an interval closely approximating \( (0,70) \). Small discrepancies near \( K = 70 \) are due to finite resolution in the grid.

Problem 3 (Branch tracking heuristic): Suppose you have computed sets of closed-loop poles \( \{ s_i(K_j) \}_{i=1}^n \) and \( \{ s_i(K_{j+1}) \}_{i=1}^n \) at two consecutive gain values. Propose a simple algorithm to assign each root at \( K_j \) to a root at \( K_{j+1} \) so as to draw continuous loci. What numerical issues can arise?

Solution:

A simple greedy heuristic is:

1. Initialize the set of unused roots at \( K_{j+1} \) as all \( s_i(K_{j+1}) \).
2. For each root \( s_i(K_j) \), find among the unused roots at \( K_{j+1} \) the one minimizing the Euclidean distance \( |s_i(K_j) - s_k(K_{j+1})| \).
3. Connect \( s_i(K_j) \) to this closest root and mark it as used.

Issues: if \( \Delta K \) is large, roots may move a long distance between samples, causing incorrect pairing. Multiple roots can pass close to one another, making nearest-neighbor pairing ambiguous. Reducing \( \Delta K \) and using more sophisticated assignment (e.g., solving a global optimal matching problem) improves robustness.

Problem 4 (Analytic vs numeric for a robotic joint): Consider a simplified joint model \( G(s) = \dfrac{K}{J s^2 + B s} \) with positive constants \( J \) and \( B \). Derive the closed-loop characteristic polynomial for unity feedback and compute the closed-loop poles analytically as functions of \( K \). Explain how a computer program can verify your analytic expressions.

Solution:

The open-loop transfer function is \( G(s) = \dfrac{K}{J s^2 + B s} \). The characteristic equation is \( 1 + G(s) = 0 \), i.e.

\[ 1 + \frac{K}{J s^2 + B s} = 0 \quad \Longleftrightarrow \quad J s^2 + B s + K = 0. \]

Thus \( P(s,K) = J s^2 + B s + K \). The closed-loop poles are

\[ s_{1,2}(K) = \frac{-B \pm \sqrt{B^2 - 4 J K}}{2 J}. \]

For fixed \( J,B \), these expressions can be plotted directly as functions of \( K \) to obtain the root locus. A computer program can confirm the result by, for example, using numpy.roots in Python with polynomial coefficients \( [J,B,K] \) for many values of \( K \) and comparing the numerically computed roots with the closed-form formula above.

Problem 5 (Computational complexity): Suppose you implement root locus plotting for a degree \( n \) plant by repeatedly computing eigenvalues of the companion matrix for \( N_K \) gain values. Give a rough estimate of the computational complexity in terms of \( n \) and \( N_K \). How does this influence the choice of \( N_K \) in practice?

Solution:

A dense eigenvalue computation for an \( n \times n \) matrix typically costs on the order of \( \mathcal{O}(n^3) \) floating-point operations. Doing this for \( N_K \) values of \( K \) yields total cost

\[ \mathcal{O}\bigl(N_K n^3\bigr). \]

For modest orders (\( n \le 10 \)) and \( N_K \) in the hundreds, this is inexpensive on a modern computer. For higher-order systems, one chooses \( N_K \) adaptively, using finer sampling only in regions where poles are close to the imaginary axis or where design decisions are sensitive.

Computer-aided root locus plotting replaces graphical construction with systematic numerical computation of closed-loop poles as functions of loop gain. By expressing the characteristic polynomial \( P(s,K) = D(s) + K N(s) \) and, when convenient, its companion matrix, we exploit robust polynomial and eigenvalue solvers in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica.

These tools are deeply integrated into robotic control workflows: high-fidelity models from robotics libraries are linearized, root locus is used to select gains satisfying stability and transient constraints, and the resulting parameters are deployed in real-time controllers. The next chapter will build on this computational foundation to perform systematic root locus-based controller design.