From the robotics dynamics course, you know that an \( n \)-DOF manipulator has dynamics

\[ \mathbf{M}(q)\ddot{q} + \mathbf{C}(q,\dot{q})\dot{q} + \mathbf{g}(q) + \mathbf{f}_v \dot{q} = \tau, \]

where \( q \in \mathbb{R}^n \) is the vector of joint positions, \( \mathbf{M}(q) \) is the symmetric positive-definite inertia matrix, \( \mathbf{C}(q,\dot{q})\dot{q} \) collects Coriolis/centrifugal terms, \( \mathbf{g}(q) \) the gravity vector, \( \mathbf{f}_v \) viscous friction coefficients, and \( \tau \in \mathbb{R}^n \) the vector of joint torques (or generalized actuator inputs).

For basic joint-space PD/PID design we often start from the independent joint approximation, obtained by:

• linearizing around a nominal configuration \( q_0 \),
• approximating \( \mathbf{M}(q_0) \) as constant and diagonal,
• treating coupling, Coriolis and gravity as either small disturbances or slowly varying terms.

For each joint \( i \) we get a scalar second-order model

\[ J_i \ddot{q}_i + b_i \dot{q}_i = \tau_i + d_i(t), \]

with inertia \( J_i > 0 \), viscous friction \( b_i \ge 0 \), control input \( \tau_i \) and lumped disturbance \( d_i(t) \) (unmodeled couplings, gravity mismatch, etc.).

The control objective in joint space (from Lesson 1 of this chapter) is to make each joint track a desired trajectory \( q_{d,i}(t) \). Define the tracking error and its derivative:

\[ e_i(t) = q_{d,i}(t) - q_i(t), \qquad \dot{e}_i(t) = \dot{q}_{d,i}(t) - \dot{q}_i(t). \]

We will design feedback laws for \( \tau_i \) so that \( e_i(t) \to 0 \) with prescribed transient behavior.

flowchart TD
  Qd["Desired joint trajectory qd(t)"] --> SUM["Error e = qd - q"]
  Q["Measured joint position q(t)"] --> SUM
  SUM --> CTRL["Joint PD / PID controller"]
  CTRL --> TAU["Actuator command tau"]
  TAU --> PLANT["Joint dynamics J d2q + b dq = tau + d(t)"]
  PLANT --> Q
        

The simplest stabilizing feedback for the approximate second-order joint model is a proportional-derivative (PD) controller:

\[ \tau_i = K_{p,i} e_i + K_{d,i} \dot{e}_i, \]

where \( K_{p,i} > 0 \) is the proportional gain and \( K_{d,i} \ge 0 \) the derivative gain. At the vector level:

\[ \tau = \mathbf{K}_p e + \mathbf{K}_d \dot{e}, \]

with diagonal positive-definite gain matrices \( \mathbf{K}_p = \operatorname{diag}(K_{p,1},\dots,K_{p,n}) \) and \( \mathbf{K}_d = \operatorname{diag}(K_{d,1},\dots,K_{d,n}) \).

Consider a single joint with dynamics \( J\ddot{q} + b\dot{q} = \tau \) and PD law \( \tau = K_p(q_d - q) + K_d(\dot{q}_d - \dot{q}) \). Assume for now that \( q_d \) is constant (set-point regulation) so that \( \dot{q}_d = \ddot{q}_d = 0 \). Then the error dynamics are:

\[ e = q_d - q,\quad \dot{e} = -\dot{q},\quad \ddot{e} = -\ddot{q}, \]

and substituting into the plant equation:

\[ J(-\ddot{e}) + b(-\dot{e}) = K_p e + K_d \dot{e} \;\Rightarrow\; J\ddot{e} + (b + K_d)\dot{e} + K_p e = 0. \]

Thus the closed-loop error dynamics are linear, time-invariant, and second order:

\[ \ddot{e} + \frac{b+K_d}{J}\dot{e} + \frac{K_p}{J} e = 0. \]

You have already seen that canonical second-order systems are written as

\[ \ddot{e} + 2\zeta \omega_n \dot{e} + \omega_n^2 e = 0, \]

where \( \omega_n > 0 \) is the natural frequency and \( \zeta \ge 0 \) is the damping ratio. Comparing with the PD closed-loop error equation, we obtain the gain-to-specification mapping:

\[ \frac{b + K_d}{J} = 2\zeta \omega_n,\qquad \frac{K_p}{J} = \omega_n^2. \]

Solving for the gains yields

\[ K_p = J \omega_n^2,\qquad K_d = 2J\zeta \omega_n - b. \]

For typical joint servos, internal motor/friction damping \( b \) is small compared with \( 2J\zeta\omega_n \), so designers often approximate \( K_d \approx 2J\zeta\omega_n \).

Using standard relations between \( \zeta \), \( \omega_n \) and performance indices — overshoot, rise time, settling time — you can choose PD gains directly from a specification such as “no more than 10% overshoot and settling time shorter than 0.5 s”.

Consider again the scalar closed-loop error system

\[ J\ddot{e} + (b+K_d)\dot{e} + K_p e = 0 \quad\Leftrightarrow\quad \ddot{e} = -\frac{b+K_d}{J}\dot{e} - \frac{K_p}{J} e. \]

Define the state vector \( x = \begin{bmatrix} e \\ \dot{e} \end{bmatrix} \) and consider the Lyapunov candidate

\[ V(x) = \frac{1}{2}J \dot{e}^2 + \frac{1}{2}K_p e^2. \]

Since \( J > 0 \) and \( K_p > 0 \), we have \( V(x) > 0 \) for \( x \neq 0 \) and \( V(0) = 0 \). Its time derivative along trajectories is

\[ \dot{V} = J \dot{e}\ddot{e} + K_p e\dot{e} = \dot{e}\bigl(J\ddot{e} + K_p e\bigr). \]

Using the closed-loop dynamics \( J\ddot{e} = -(b+K_d)\dot{e} - K_p e \), we obtain

\[ \dot{V} = \dot{e}\bigl(-(b+K_d)\dot{e}\bigr) = -(b+K_d)\dot{e}^2 \le 0 \]

whenever \( b+K_d \ge 0 \). Thus \( V \) is non-increasing and the origin \( (e,\dot{e}) = (0,0) \) is stable. Moreover, the largest invariant set contained in \( \{x : \dot{V}(x) = 0\} \) is just the origin (because \( \dot{V} = 0 \Rightarrow \dot{e} = 0 \Rightarrow \ddot{e} = -\frac{K_p}{J}e = 0 \Rightarrow e = 0 \)), so by LaSalle’s invariance principle the equilibrium is globally asymptotically stable.

For the vector case with diagonal \( \mathbf{J} \), \( \mathbf{K}_p \succ 0 \), \( \mathbf{K}_d \succeq 0 \), we choose

\[ V(e,\dot{e}) = \frac{1}{2} \dot{e}^\top \mathbf{J} \dot{e} + \frac{1}{2} e^\top \mathbf{K}_p e, \]

and obtain similarly

\[ \dot{V} = -\dot{e}^\top(\mathbf{B}+\mathbf{K}_d)\dot{e} \le 0, \]

where \( \mathbf{B} \succeq 0 \) collects viscous friction terms. The same LaSalle argument yields global asymptotic convergence of joint errors in the ideal independent-joint model.

PD controllers remove overshoot and shape transients, but in the presence of constant disturbances (e.g. unmodeled gravity, static friction) they may exhibit nonzero steady-state error. A standard remedy is to add an integral term:

\[ \tau_i = K_{p,i} e_i + K_{d,i} \dot{e}_i + K_{i,i} \int_0^t e_i(\sigma)\,d\sigma. \]

Let \( z_i(t) = \int_0^t e_i(\sigma)\,d\sigma \) be the integral state. For constant reference and the simple model \( J\ddot{q} + b\dot{q} = \tau \) (no disturbance), the closed-loop error dynamics become

\[ J\ddot{e} + (b+K_d)\dot{e} + K_p e + K_i z = 0,\qquad \dot{z} = e. \]

Taking Laplace transforms (with zero initial conditions) gives the characteristic polynomial of the error system:

\[ J s^3 + (b+K_d)s^2 + K_p s + K_i = 0. \]

Let \( a_3 = J \), \( a_2 = b+K_d \), \( a_1 = K_p \), \( a_0 = K_i \). The Routh–Hurwitz conditions for all roots to have negative real part are:

\[ a_3 > 0,\quad a_2 > 0,\quad a_1 > 0,\quad a_0 > 0,\quad a_2 a_1 > a_3 a_0. \]

Since \( a_3 = J > 0 \), the stability conditions become

\[ b+K_d > 0,\quad K_p > 0,\quad K_i > 0,\quad (b+K_d)K_p > J K_i. \]

In other words, once \( K_p \) and \( K_d \) have been chosen (e.g. from desired PD behavior), the integral gain must be sufficiently small:

\[ 0 < K_i < \frac{(b+K_d)K_p}{J}. \]

This explains a common practical rule: start from a well-tuned PD controller and then add a small integral gain, increasing it until steady-state error is removed but before oscillations appear.

For constant disturbances \( d \) added to the joint dynamics, the integral action guarantees zero steady-state error (in the ideal model) provided the closed-loop system remains stable. This can be verified using the final value theorem and noting that the closed-loop transfer function from disturbance to error has a factor \( s \) in the numerator due to the integrator.

Real robot controllers run at a finite sampling period \( T_s \) (e.g. 1 ms) and obtain joint positions from encoders. A practical discrete-time PD/PID controller for a single joint at time step \( k \) looks like:

• Position error: \( e[k] = q_d[k] - q[k] \).
• Derivative estimate: \( \dot{e}[k] \approx \frac{e[k] - e[k-1]}{T_s} \), usually filtered.
• Integral state: \( z[k] = z[k-1] + T_s e[k] \), often with anti-windup limiting.

A popular filtered derivative uses a first-order low-pass filter:

\[ \dot{e}_{\text{f}}[k] = (1-\alpha)\dot{e}_{\text{f}}[k-1] + \alpha\frac{e[k] - e[k-1]}{T_s}, \qquad 0 < \alpha \le 1. \]

The torque command is then

\[ \tau[k] = K_p e[k] + K_d \dot{e}_{\text{f}}[k] + K_i z[k]. \]

In multi-DOF manipulators, the same logic is applied joint-by-joint with vector gains. At this stage we are not cancelling coupling terms; we simply rely on high feedback gains and relatively slow motion so that the independent-joint approximation is acceptable. More advanced model-based compensation is addressed in later chapters.

Python is commonly used for prototyping robot controllers. Libraries such as roboticstoolbox-python (Peter Corke), numpy and scipy are particularly useful. Below is a simple simulation of PD and PID control for a single joint with parameters \( J, b \).

import numpy as np

def simulate_joint_pid(
    q0, dq0,
    qd,
    Kp, Kd, Ki,
    J=1.0, b=0.0,
    dt=0.001, T=2.0,
    use_integral=True
):
    """
    Simulate scalar joint dynamics:
        J * ddq + b * dq = tau
    with PD or PID control.
    """
    N = int(T / dt)
    q = np.zeros(N)
    dq = np.zeros(N)
    e_hist = np.zeros(N)
    t = np.linspace(0.0, T, N)

    q[0] = q0
    dq[0] = dq0
    z = 0.0                # integral state
    e_prev = qd - q0
    de_f = 0.0             # filtered derivative
    alpha = 0.2            # derivative filter weight

    for k in range(1, N):
        # For this demo, desired position is constant qd
        e = qd - q[k-1]
        de = (e - e_prev) / dt
        de_f = (1.0 - alpha) * de_f + alpha * de

        if use_integral:
            z += e * dt

        tau = Kp * e + Kd * de_f + (Ki * z if use_integral else 0.0)

        # Joint dynamics: explicit Euler integration
        ddq = (tau - b * dq[k-1]) / J
        dq[k] = dq[k-1] + ddq * dt
        q[k] = q[k-1] + dq[k] * dt

        e_hist[k] = e
        e_prev = e

    return t, q, dq, e_hist

if __name__ == "__main__":
    # Example: J = 0.5, b = 0.05, desired position qd = 1 rad
    J = 0.5
    b = 0.05
    # Choose gains from a second-order specification
    zeta = 0.7
    omega_n = 6.0
    Kp = J * omega_n**2
    Kd = 2.0 * J * zeta * omega_n - b
    Ki = 5.0   # must satisfy Routh-Hurwitz constraint

    t, q, dq, e_hist = simulate_joint_pid(
        q0=0.0, dq0=0.0,
        qd=1.0,
        Kp=Kp, Kd=Kd, Ki=Ki,
        J=J, b=b,
        dt=0.001, T=2.0,
        use_integral=True
    )

    # Plotting (requires matplotlib)
    import matplotlib.pyplot as plt
    plt.figure()
    plt.plot(t, q, label="q(t)")
    plt.plot(t, np.ones_like(t), "--", label="qd")
    plt.xlabel("time [s]")
    plt.ylabel("position [rad]")
    plt.legend()
    plt.grid(True)
    plt.show()
      

In a full robotics software stack (e.g. ROS), the PD/PID law would be implemented inside a real-time control loop (for instance using ros_control), reading joint states from sensor_msgs/JointState and writing actuator commands to a hardware interface.

C++ is widely used for low-level robot control, often within frameworks like ROS and ros_control or Orocos. The following snippet shows a minimal PD/PID joint controller suitable for embedding into a control loop with fixed sampling period \( T_s \).

struct JointPID
{
    double Kp{0.0};
    double Kd{0.0};
    double Ki{0.0};

    double integral{0.0};
    double e_prev{0.0};
    double de_f{0.0};   // filtered derivative
    double alpha{0.2};  // derivative filter

    double update(double q, double dq,
                  double qd, double dqd,
                  double dt)
    {
        // tracking error (position only; dqd often 0 for setpoints)
        double e = qd - q;
        double de = (e - e_prev) / dt;
        de_f = (1.0 - alpha) * de_f + alpha * de;

        integral += e * dt;

        // anti-windup (simple clamping)
        const double z_max = 10.0;
        if (integral > z_max) integral = z_max;
        if (integral < -z_max) integral = -z_max;

        double tau = Kp * e + Kd * de_f + Ki * integral;

        e_prev = e;
        return tau;
    }
};

// Example usage in a high-rate control loop
// (e.g. inside a ROS control plugin)
void controlLoopStep(double dt)
{
    static JointPID pid;
    // gains could be parameterized or tuned online
    pid.Kp = 10.0;
    pid.Kd = 1.5;
    pid.Ki = 2.0;

    double q   = readJointPosition();
    double dq  = readJointVelocity();
    double qd  = desiredJointPosition();
    double dqd = 0.0; // for a constant setpoint

    double tau = pid.update(q, dq, qd, dqd, dt);
    sendJointTorqueCommand(tau);
}
      

In practice, this structure would be integrated with a robotics library (e.g. ros_control) that handles timing, joint state interfaces, and safety limits (effort saturation, rate limiting).

Java is less common at the lowest control level but appears in higher-level robot stacks (e.g. rosjava) and educational systems. Below is a simple joint PD/PID controller class.

public class JointPIDController {
    private double Kp;
    private double Kd;
    private double Ki;

    private double integral = 0.0;
    private double ePrev = 0.0;
    private double dFiltered = 0.0;
    private double alpha = 0.2; // derivative filter weight

    public JointPIDController(double Kp, double Kd, double Ki) {
        this.Kp = Kp;
        this.Kd = Kd;
        this.Ki = Ki;
    }

    public double update(double q, double dq,
                         double qd,
                         double dt) {
        double e = qd - q;
        double de = (e - ePrev) / dt;
        dFiltered = (1.0 - alpha) * dFiltered + alpha * de;

        integral += e * dt;

        // naive anti-windup
        double zMax = 10.0;
        if (integral > zMax) integral = zMax;
        if (integral < -zMax) integral = -zMax;

        double tau = Kp * e + Kd * dFiltered + Ki * integral;
        ePrev = e;
        return tau;
    }
}
      

Linear algebra libraries such as EJML or Apache Commons Math can be used to extend this scalar controller to vectorized multi-joint implementations.

MATLAB is ubiquitous in robotics education. The following script simulates joint-space PD control using ode45. Simulink offers block-diagram equivalents using standard blocks (“Sum”, “Gain”, “Integrator”) for the same structure.

function joint_pd_demo()
    J = 0.5;
    b = 0.05;
    qd = 1.0;

    zeta = 0.7;
    omega_n = 6.0;
    Kp = J * omega_n^2;
    Kd = 2 * J * zeta * omega_n - b;

    x0 = [0.0; 0.0];  % [q; dq]
    tspan = [0 2];
    [t, x] = ode45(@(t, x) dynamics_pd(t, x, qd, J, b, Kp, Kd), tspan, x0);

    q = x(:, 1);
    figure;
    plot(t, q, "LineWidth", 1.5); hold on;
    plot(t, qd * ones(size(t)), "--", "LineWidth", 1.0);
    xlabel("time [s]");
    ylabel("joint position [rad]");
    legend("q(t)", "qd");
    grid on;
end

function dx = dynamics_pd(t, x, qd, J, b, Kp, Kd)
    q = x(1);
    dq = x(2);

    e = qd - q;
    de = -dq; % since qd is constant

    tau = Kp * e + Kd * de;

    ddq = (tau - b * dq) / J;
    dx = [dq; ddq];
end
      

In Simulink, a PID joint controller is created by feeding the error \( e = q_d - q \) into a built-in PID Controller block and connecting its output to a joint model block (for example, a second-order transfer function or a Simscape Multibody joint). The Robotics System Toolbox can be used to incorporate full manipulator models in the loop.

Wolfram Mathematica is well suited for symbolic analysis and numerical simulation of controlled joint dynamics. The snippet below defines a PD-controlled joint and uses NDSolve to simulate it.

J = 0.5;
b = 0.05;
qd = 1.0;
zeta = 0.7;
omegaN = 6.0;
Kp = J * omegaN^2;
Kd = 2 J zeta omegaN - b;

eqns = {
  q''[t] == (Kp (qd - q[t]) + Kd (0 - q'[t]) - b q'[t]) / J,
  q[0] == 0,
  q'[0] == 0
};

sol = NDSolve[eqns, q, {t, 0, 2}][[1]];

Plot[{q[t] /. sol, qd}, {t, 0, 2},
  PlotLegends -> {"q(t)", "qd"},
  AxesLabel -> {"t", "position"},
  GridLines -> Automatic
]
      

Symbolic tools can also be used to derive the characteristic polynomial, verify Routh–Hurwitz conditions, and perform parametric sweeps over \( K_p, K_d, K_i \).

In this lesson we moved from the full manipulator dynamics to an independent-joint approximation and derived PD and PID controllers directly in joint space. For PD control we established exact equivalence with a second-order linear system and used this to map between gains and classical performance specifications \( (\omega_n, \zeta) \). We then proved global asymptotic stability for vector PD control using a quadratic Lyapunov function. Extending to PID control, we obtained a cubic characteristic polynomial and used Routh–Hurwitz conditions to derive explicit inequalities on the integral gain ensuring stability. Finally, we illustrated implementation details in Python, C++, Java, MATLAB/Simulink and Wolfram Mathematica, emphasizing discrete-time realization, derivative filtering, and integral anti-windup. These joint-level PD/PID controllers form the baseline upon which more advanced model-based and task-space controllers will be built in subsequent chapters.