In previous lessons, we defined joint-space control objectives and derived PD/PID control laws for each joint. The remaining question is: how do we choose the gains \( K_p, K_d, K_i \) so that:

• tracking is fast but not oscillatory,
• overshoot and settling time satisfy specifications,
• actuators are not saturated or excited into instability.

We focus on a single joint with gravity compensation (Lesson 3) and approximate linear dynamics. For a revolute joint with coordinate \( q \) and control torque \( \tau \), after gravity compensation and linearization around a desired position \( q_d \), the local dynamics can be approximated as

\[ J \ddot{q} + B \dot{q} = \tau + d(t), \]

where \( J > 0 \) is the effective joint inertia, \( B \ge 0 \) represents viscous friction, and \( d(t) \) is a lumped disturbance (unmodeled dynamics, coupling, etc.). For a PD control law

\[ \tau = -K_p e - K_d \dot{e}, \quad e = q - q_d, \]

the error dynamics govern performance. Practical gain tuning is the art of shaping these error dynamics to meet specifications while dealing with modeling uncertainty.

flowchart TD
  A["Start: performance specs (Ts, Mp, bandwidth)"] --> B["Approximate joint model (J, B)"]
  B --> C["Choose controller type: PD or PID"]
  C --> D["Compute initial gains from model (zeta, wn mapping)"]
  D --> E["Simulate / test on robot at low gains"]
  E --> F["Adjust gains iteratively for robustness and limits"]
  F --> G["Validate: tracking, overshoot, actuator limits"]
        

Substituting the PD law into the linearized joint equation, and assuming a constant desired position \( q_d \) (i.e., \( \dot{q}_d = \ddot{q}_d = 0 \)), we obtain

\[ J \ddot{q} + B \dot{q} = -K_p (q - q_d) - K_d \dot{q} + d(t). \]

In terms of error \( e = q - q_d \), we have

\[ J \ddot{e} + (B + K_d) \dot{e} + K_p e = d(t). \]

Ignoring disturbances \( d(t) \) for the moment, the homogeneous closed-loop error dynamics are

\[ J \ddot{e} + (B + K_d) \dot{e} + K_p e = 0. \]

Dividing by \( J \) yields

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

This is a standard second-order linear system. We can match it to the canonical form

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

where \( \zeta \) is the damping ratio and \( \omega_n \) is the natural frequency. Matching coefficients gives the design equalities

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

Hence, for chosen \( \zeta, \omega_n \) and known \( J, B \), we obtain

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

Stability condition. For this second-order system, asymptotic stability (i.e., \( e(t) \to 0 \) as \( t → \infty \)) requires positive coefficients:

\[ J > 0, \quad B + K_d > 0, \quad K_p > 0. \]

Since strictly passive mechanics ensure \( J > 0 \), we primarily need \( K_p > 0 \) and \( K_d > -B \). In practice, we choose \( K_d \) strictly positive so that friction and derivative feedback both contribute damping.

For a standard underdamped second-order system with \( 0 < \zeta < 1 \), the step-response measures are approximately

• Peak overshoot

  \[ M_p = \exp\!\left( -\frac{\pi \zeta}{\sqrt{1 - \zeta^2}} \right). \]

• Settling time (2% criterion)

  \[ T_s \approx \frac{4}{\zeta \omega_n}. \]

• Rise time (10–90%) can be approximated but is less directly used in gain selection.

Given a desired maximum overshoot \( M_p^{\star} \), we can solve for \( \zeta \):

\[ \zeta = \frac{-\ln M_p^{\star}}{\sqrt{\pi^2 + \left( \ln M_p^{\star} \right)^2 }}. \]

Given a desired settling time \( T_s^{\star} \) and the chosen \( \zeta \), we pick

\[ \omega_n = \frac{4}{\zeta T_s^{\star}}. \]

Finally, convert \( \zeta, \omega_n \) to PD gains using the mapping from Section 2:

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

In robotics, \( J \) can be taken as the scalar equivalent of the joint's effective inertia, e.g., the corresponding diagonal entry of the manipulator inertia matrix \( \mathbf{M}(\mathbf{q}) \), evaluated at the operating configuration \( \mathbf{q}_d \). Friction \( B \) is often small or uncertain, and a common approximation is to neglect \( B \) in the formula while retaining physical damping.

PD control ensures good tracking but can leave a steady-state error under constant disturbances or model mismatch. Introducing integral action:

\[ \tau = -K_p e - K_d \dot{e} - K_i \int_0^t e(\sigma)\, d\sigma \]

augments the error dynamics with an integral state \( x_I(t) = \int_0^t e(\sigma)\, d\sigma \). The state-space model becomes

\[ \begin{aligned} \dot{x}_I &= e, \\ J \ddot{e} + (B + K_d) \dot{e} + K_p e + K_i x_I &= d(t). \end{aligned} \]

For \( d(t) \) constant and moderate gains, the integral term drives the steady-state error to zero. However, the characteristic polynomial is third-order, and explicit pole placement becomes more complex. A practical rule is:

• First design a well-damped PD pair \( (K_p, K_d) \) using Section 3.
• Add a relatively small \( K_i \) so that the integral dynamic is slower than the dominant PD poles.
• Increase \( K_i \) gradually while monitoring overshoot and actuator limits.

The integral gain is often the smallest of the three, especially in high-friction or high-gear-ratio drives, to avoid integrator windup and oscillations.

In practice, model parameters \( J, B \) are uncertain, and robots are subject to joint coupling and unmodeled dynamics. A widely used workflow combines model-based initialization with experimental refinement:

1. Compute initial PD gains from approximate \( J \), desired \( M_p^{\star}, T_s^{\star} \).
2. Apply gains at a reduced scale (e.g., 50%) to ensure safety.
3. Command small step changes in joint position while monitoring overshoot, settling time, and actuator currents.
4. Adjust \( K_p \) upward for faster response, and \( K_d \) upward for more damping, but respect saturation limits.
5. Once PD is satisfactory, add a small \( K_i \) for steady-state accuracy.

A simplified variant of Ziegler–Nichols tuning (applied at the joint level) is:

• Set \( K_i = 0 \), \( K_d = 0 \).
• Increase \( K_p \) until sustained oscillations occur; the gain is the ultimate gain \( K_u \), and the oscillation period is \( T_u \).
• Choose PID gains proportional to \( K_u \) using empirical formulas.

For robotic manipulators, aggressive Ziegler–Nichols rules can excite flexible modes or saturate actuators. A more conservative scaling of \( K_u \) is recommended, together with safety monitors.

flowchart TD
  S["Set low gains Kp, Kd, Ki=0"] --> STEP["Apply small step in joint reference"]
  STEP --> OBS["Measure overshoot, Ts, current"]
  OBS --> ADJ["Adjust Kp (speed) and Kd (damping)"]
  ADJ --> CHECK["Check constraints: no oscillation, no saturation"]
  CHECK -->|ok| INT["Increase small Ki for zero steady-state error"]
  CHECK -->|not ok| STEP
  INT --> DONE["Finalize gains for joint"]
        

We now implement the basic mapping from performance specifications to PD gains, and a simple joint-space controller update, in several languages. We consider a single joint with parameters \( J, B \) and desired \( M_p^{\star}, T_s^{\star} \).

6.1 Python Example (Model-Based PD Gain Computation)

import math

def damping_ratio_from_overshoot(Mp_star):
    """
    Compute zeta from allowed peak overshoot Mp_star (0 < Mp_star < 1).
    """
    if Mp_star <= 0.0 or Mp_star >= 1.0:
        raise ValueError("Mp_star must be in (0, 1).")
    logMp = math.log(Mp_star)
    return -logMp / math.sqrt(math.pi**2 + logMp**2)

def pd_gains_from_specs(J, B, Mp_star, Ts_star):
    """
    Compute (Kp, Kd) for a single joint with inertia J and viscous friction B,
    given Mp_star (max overshoot) and Ts_star (desired settling time).
    """
    zeta = damping_ratio_from_overshoot(Mp_star)
    wn = 4.0 / (zeta * Ts_star)
    Kp = J * wn**2
    Kd = 2.0 * J * zeta * wn - B
    return Kp, max(Kd, 0.0)  # enforce nonnegative derivative gain

def pd_control_update(q, qd, dq, dt, Kp, Kd):
    """
    Discrete-time PD controller (no integral term).
    q  : current joint position
    qd : desired joint position
    dq : current joint velocity
    dt : time step (unused here, but useful if integrating additional dynamics)
    """
    e = q - qd
    de = dq  # assuming qd is constant
    tau = -Kp * e - Kd * de
    return tau

if __name__ == "__main__":
    J = 0.5    # kg m^2
    B = 0.05   # N m s/rad
    Mp_star = 0.1   # 10% overshoot
    Ts_star = 0.5   # 0.5 s settling time

    Kp, Kd = pd_gains_from_specs(J, B, Mp_star, Ts_star)
    print(f"Kp = {Kp:.3f}, Kd = {Kd:.3f}")

    # Example control step
    q = 0.1      # rad
    qd = 0.0     # rad
    dq = 0.0     # rad/s
    dt = 0.001   # s
    tau = pd_control_update(q, qd, dq, dt, Kp, Kd)
    print(f"Control torque tau = {tau:.3f} N m")
      

6.2 C++ Example (PD Controller with Eigen)

For joint-wise control, we can implement a PD controller that operates on scalar joints or vectorized joints using Eigen. Below is a scalar example.

#include <iostream>
#include <cmath>

struct PDGains {
    double Kp;
    double Kd;
};

double dampingRatioFromOvershoot(double Mp_star) {
    if (Mp_star <= 0.0 || Mp_star >= 1.0) {
        throw std::runtime_error("Mp_star must be in (0, 1).");
    }
    double logMp = std::log(Mp_star);
    return -logMp / std::sqrt(M_PI * M_PI + logMp * logMp);
}

PDGains computePDGains(double J, double B, double Mp_star, double Ts_star) {
    double zeta = dampingRatioFromOvershoot(Mp_star);
    double wn   = 4.0 / (zeta * Ts_star);
    PDGains g;
    g.Kp = J * wn * wn;
    g.Kd = 2.0 * J * zeta * wn - B;
    if (g.Kd < 0.0) g.Kd = 0.0;
    return g;
}

double pdTorque(double q, double qd, double dq, const PDGains& g) {
    double e  = q - qd;
    double de = dq; // assume qd is constant
    return -g.Kp * e - g.Kd * de;
}

int main() {
    double J = 0.5;
    double B = 0.05;
    PDGains g = computePDGains(J, B, 0.1, 0.5);

    std::cout << "Kp = " << g.Kp << ", Kd = " << g.Kd << std::endl;

    double q = 0.1;
    double qd = 0.0;
    double dq = 0.0;

    double tau = pdTorque(q, qd, dq, g);
    std::cout << "tau = " << tau << std::endl;
    return 0;
}
      

6.3 Java Example (Joint Controller Class)

public class JointPDController {
    private double Kp;
    private double Kd;

    public JointPDController(double J, double B, double MpStar, double TsStar) {
        double zeta = dampingRatioFromOvershoot(MpStar);
        double wn   = 4.0 / (zeta * TsStar);
        this.Kp = J * wn * wn;
        this.Kd = 2.0 * J * zeta * wn - B;
        if (this.Kd < 0.0) {
            this.Kd = 0.0;
        }
    }

    public static double dampingRatioFromOvershoot(double MpStar) {
        if (MpStar <= 0.0 || MpStar >= 1.0) {
            throw new IllegalArgumentException("MpStar must be in (0, 1).");
        }
        double logMp = Math.log(MpStar);
        return -logMp / Math.sqrt(Math.PI * Math.PI + logMp * logMp);
    }

    public double computeTorque(double q, double qd, double dq) {
        double e  = q - qd;
        double de = dq; // assume qd constant
        return -Kp * e - Kd * de;
    }

    public double getKp() { return Kp; }
    public double getKd() { return Kd; }

    public static void main(String[] args) {
        double J = 0.5;
        double B = 0.05;
        JointPDController ctrl = new JointPDController(J, B, 0.1, 0.5);

        double tau = ctrl.computeTorque(0.1, 0.0, 0.0);
        System.out.println("Kp = " + ctrl.getKp() + ", Kd = " + ctrl.getKd());
        System.out.println("tau = " + tau);
    }
}
      

6.4 MATLAB/Simulink Example

In MATLAB, we can compute PD gains and simulate the joint error dynamics using ode45. In Simulink, the same gains can be used in a standard PID block with derivative and proportional actions only.

function robot_pd_tuning_example
    J = 0.5;
    B = 0.05;
    Mp_star = 0.1; % 10% overshoot
    Ts_star = 0.5; % 0.5 s settling time

    zeta = dampingRatioFromOvershoot(Mp_star);
    wn   = 4 / (zeta * Ts_star);
    Kp   = J * wn^2;
    Kd   = 2 * J * zeta * wn - B;
    Kd   = max(Kd, 0);

    fprintf("Kp = %.3f, Kd = %.3f\n", Kp, Kd);

    % Simulate closed-loop error dynamics for a unit step in qd
    qd = 1.0; % rad
    x0 = [0; 0]; % [e; de]

    tspan = [0 2];
    [t, x] = ode45(@(t, x) errorDynamics(t, x, J, B, Kp, Kd, qd), tspan, x0);

    e  = x(:,1);
    de = x(:,2);

    figure;
    plot(t, e);
    xlabel('Time [s]');
    ylabel('Position error e(t) [rad]');
    title('Closed-loop PD error dynamics');
end

function zeta = dampingRatioFromOvershoot(Mp_star)
    if Mp_star <= 0 || Mp_star >= 1
        error('Mp_star must be in (0, 1).');
    end
    logMp = log(Mp_star);
    zeta  = -logMp / sqrt(pi^2 + logMp^2);
end

function dx = errorDynamics(~, x, J, B, Kp, Kd, qd)
    e  = x(1);
    de = x(2);

    % tau = -Kp*(e) - Kd*(de), qd constant, so error dynamics:
    % J*ddot{e} + (B + Kd)*dot{e} + Kp*e = 0
    dde = -(B + Kd)/J * de - Kp/J * e;

    dx = [de; dde];
end
      

In Simulink, you can:

• Use a Step block for the desired position q_d.
• Subtract the measured joint position q to obtain error e.
• Feed e and dq into a PID Controller block configured as PD (zero integral action) with gains Kp and Kd.
• Connect the output torque to a joint dynamics block (e.g., a custom transfer function for J and B or a Simscape Multibody joint).

6.5 Wolfram Mathematica Example

Mathematica can symbolically derive the closed-loop poles as functions of gains.

(* Parameters *)
J  = 0.5;
B  = 0.05;
Kp = Symbol["Kp"];
Kd = Symbol["Kd"];

(* Characteristic polynomial: J s^2 + (B + Kd) s + Kp = 0 *)
s  = Symbol["s"];
charPoly = J s^2 + (B + Kd) s + Kp;

(* Solve for poles in terms of Kp, Kd *)
poles = Solve[charPoly == 0, s]

(* Substitute design (zeta, wn) mapping *)
zeta = 0.7;
Ts   = 0.5;
wn   = 4/(zeta*Ts);

KpVal = J*wn^2;
KdVal = 2*J*zeta*wn - B;

polesNumeric = poles /. {Kp -> KpVal, Kd -> KdVal} // N

(* Step response of error dynamics for chosen gains *)
sys = TransferFunctionModel[KpVal/(J s^2 + (B + KdVal) s + KpVal), s];
stepResp = OutputResponse[sys, UnitStep[t], {t, 0, 2}];
Plot[stepResp, {t, 0, 2}, AxesLabel -> {"t", "e(t)"}]
      

In this lesson we connected joint-space PD/PID gain tuning to the underlying second-order joint dynamics. By linearizing the gravity-compensated joint equation, we identified the mapping between physical parameters \( (J, B) \), closed-loop performance parameters \( (\zeta, \omega_n) \), and PD gains \( (K_p, K_d) \). This allowed us to translate time-domain specifications on overshoot and settling time into concrete gain values.

We then incorporated integral action for eliminating steady-state errors and discussed why integral gains must be chosen conservatively. Iterative and experimental workflows, including conservative Ziegler–Nichols-style procedures, were presented as essential complements to model-based design in the presence of uncertainty and coupling. Finally, we illustrated multi-language implementations (Python, C++, Java, MATLAB/Simulink, Mathematica) that convert performance requirements into controller gains and implement joint-space servo loops. These tuning tools form the practical foundation for joint servoing experiments to be explored in the next lesson.