A joint servo is a feedback loop that regulates each joint angle \( q_i(t) \) around a desired joint trajectory \( q_{d,i}(t) \). At the core of the loop is a controller that converts the joint position/velocity error into an actuator command (typically torque or motor current).

For each joint \(i\), define the position error \( e_i(t) = q_{d,i}(t) - q_i(t) \) and velocity error \( \dot e_i(t) = \dot q_{d,i}(t) - \dot q_i(t) \). The joint servo loop then:

• measures \( q_i(t) \) and (possibly estimated) \( \dot q_i(t) \),
• computes the error \( e_i(t), \dot e_i(t) \),
• generates a torque command using a PD/PID law (plus gravity compensation),
• applies the torque to the actuator, closing the loop.

flowchart TD
  R["Desired joint trajectory q_d(t)"] --> E["Error computation: e = q_d - q"]
  E --> C["Controller: PD / PID + gravity term g(q)"]
  C --> U["Actuator command tau"]
  U --> P["Robot joint / arm dynamics"]
  P --> S["Sensors: encoders, velocity estimate"]
  S --> E
        

In the lab you will instantiate this loop for one joint (for detailed analysis) and then extend it to multiple joints, relying on approximate decoupling between joints using the diagonal inertia and damping approximations discussed in manipulator dynamics.

Consider an \(n\)-DOF manipulator with dynamics (in joint coordinates)

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

where \( \mathbf{M}(\mathbf{q}) \) is the positive-definite inertia matrix, \( \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} \) collects Coriolis/centrifugal terms, and \( \mathbf{g}(\mathbf{q}) \) is the gravity torque vector.

A standard joint-space PD + gravity compensation law (per joint) is

\[ \tau_i(t) = K_{p,i}\big(q_{d,i}(t) - q_i(t)\big) + K_{d,i}\big(\dot q_{d,i}(t) - \dot q_i(t)\big) + g_i(\mathbf{q}(t)), \quad i = 1,\dots,n, \]

where \(K_{p,i} > 0\) and \(K_{d,i} \ge 0\) are scalar gains and \( g_i(\mathbf{q}) \) is the \(i\)-th component of the gravity torque vector.

For joints that require elimination of steady-state error under constant load or slowly varying commands, an integral term is often added:

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

In vector form (neglecting integral action for now),

\[ \boldsymbol{\tau}(t) = \mathbf{K}_p \mathbf{e}(t) + \mathbf{K}_d \dot{\mathbf{e}}(t) + \mathbf{g}(\mathbf{q}(t)), \]

where \( \mathbf{K}_p, \mathbf{K}_d \) are diagonal gain matrices, and \( \mathbf{e}(t) = \mathbf{q}_d(t) - \mathbf{q}(t) \). This is the control law you will implement in the lab.

For analysis, consider a single revolute joint with dynamics

\[ J \ddot q(t) + b \dot q(t) + g(q(t)) = \tau(t), \]

where \(J > 0\) is the equivalent inertia and \(b \ge 0\) is viscous friction. Assume a constant desired position \( q_d \) (\(\dot q_d = 0\)) and a PD + gravity law

\[ \tau(t) = K_p\big(q_d - q(t)\big) + K_d\big(0 - \dot q(t)\big) + g(q(t)). \]

Define the error \( e(t) = q_d - q(t) \), so that \( \dot e(t) = -\dot q(t) \) and \( \ddot e(t) = -\ddot q(t) \). Substituting the control law into the dynamics gives

\[ J \ddot q + b \dot q + g(q) = K_p e + K_d \dot e + g(q), \]

so the gravity term cancels and we obtain, after substituting \( \ddot q = -\ddot e \), \( \dot q = -\dot e \),

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

The characteristic polynomial is \( J s^2 + (b+K_d)s + K_p = 0 \), with roots

\[ s_{1,2} = \frac{-(b+K_d) \pm \sqrt{(b+K_d)^2 - 4 J K_p}}{2 J}. \]

Closed-loop stability requires \( J > 0 \), \( K_p > 0 \), and \( b+K_d > 0 \), in which case both roots have negative real part (Routh conditions for a second-order polynomial).

It is convenient to rewrite the error dynamics in the standard second-order form

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

where

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

From this identification we can design gains for desired natural frequency \( \omega_n \) and damping ratio \( \zeta \in (0,1) \):

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

Lyapunov proof sketch. Consider the Lyapunov candidate

\[ V(e,\dot e) = \tfrac{1}{2} J \dot e^2 + \tfrac{1}{2} K_p e^2, \]

which satisfies \( V \ge 0 \) and \( V = 0 \) only when \( e = \dot e = 0 \). Using the error dynamics \( J \ddot e + (b+K_d)\dot e + K_p e = 0 \), we obtain

\[ \dot V = J \dot e \ddot e + K_p e \dot e = \dot e \big(J \ddot e + K_p e\big) = -(b+K_d)\dot e^2 \le 0. \]

Thus the equilibrium \( e = \dot e = 0 \) is globally stable; with \(b+K_d > 0\) and the system being linear time-invariant, the origin is in fact globally asymptotically stable. This provides a theoretical guarantee that your joint servo will converge to the desired constant joint angle under the idealized assumptions (perfect gravity compensation, no saturation, no unmodeled couplings).

On real hardware (or in most simulators), the controller runs as a digital loop with sampling time \( T_s \). Let \( e[k] = q_d[k] - q[k] \) denote the sampled error at time \( t_k = k T_s \).

A common discrete-time PD + I implementation uses:

\[ \begin{aligned} e[k] &= q_d[k] - q[k], \\ \dot e[k] &\approx \frac{e[k] - e[k-1]}{T_s}, \\ I[k] &= I[k-1] + T_s\, e[k], \\ \tau[k] &= K_p e[k] + K_d \dot e[k] + K_i I[k] + g\big(q[k]\big), \end{aligned} \]

where \(I[k]\) is the discrete-time integral state. In actual implementation you will:

• choose \(T_s\) significantly smaller than the desired closed-loop time constants,
• limit \(I[k]\) to prevent integral windup under torque saturation,
• optionally filter the measured velocity (or the discrete derivative) to attenuate noise.

Note that even though the theoretical analysis in Section 3 was continuous-time, for sufficiently small \(T_s\) the discrete implementation approximates the continuous behavior well. A detailed discrete-time stability analysis can be performed using z-transform techniques (covered in digital control), but for this lesson we rely on the continuous-time gain design and then validate experimentally.

Actuator saturation. Motors and drives can only deliver torque within a bounded range \([\tau_{\min}, \tau_{\max}]\). In the controller you should constrain the commanded torque:

\[ \tau_{\text{cmd}}[k] = \min\big( \tau_{\max}, \max(\tau_{\min}, \tau[k]) \big). \]

If integral action is present, you must avoid integrating error when the actuator is saturated in a direction that would further increase saturation (anti-windup).

Velocity estimation. Encoders usually measure position only. A simple velocity approximation is a backward difference:

\[ \dot q[k] \approx \frac{q[k] - q[k-1]}{T_s}, \]

possibly passed through a low-pass filter to reduce quantization noise. Alternatively, one can use observers or filters (to be studied later), but for this lab a finite difference with mild filtering suffices.

Safety. Before running the controller on a real robot:

• verify software joint limits and hardware limit switches,
• start with very small gains and small motion ranges,
• ensure an emergency stop is reachable,
• test one joint at a time with the others locked or in a safe configuration.

We first implement a simple one-DOF joint model with PD + gravity compensation in Python. For a more complex arm, you could use a robotics library such as roboticstoolbox-python or a physics engine such as pybullet, but here we keep the dynamics explicit.

import numpy as np
import matplotlib.pyplot as plt

# Physical parameters of a simple pendulum-like joint
J = 0.05      # inertia [kg m^2]
b = 0.01      # viscous friction [N m s/rad]
m = 1.0       # link mass [kg]
ell = 0.5     # link COM distance [m]
g = 9.81      # gravity [m/s^2]

def gravity_torque(q):
    # simple planar joint: tau_g = m g l sin(q)
    return m * g * ell * np.sin(q)

# Desired position: step from 0 to qd at t = 0
qd = np.deg2rad(45.0)  # 45 degrees

# Continuous-time design: choose wn, zeta and derive Kp, Kd
wn = 8.0   # rad/s
zeta = 0.7
Kp = J * wn**2
Kd = 2.0 * J * zeta * wn - b

print("Kp =", Kp, "Kd =", Kd)

# Discrete simulation
Ts = 0.001          # controller and integration step [s]
t_final = 2.0
N = int(t_final / Ts)

q = 0.0             # initial position
dq = 0.0            # initial velocity

qs = np.zeros(N)
dqs = np.zeros(N)
taus = np.zeros(N)
ts = np.linspace(0.0, t_final, N)

for k in range(N):
    # controller (no integral term)
    e = qd - q
    de = 0.0 - dq
    tau = Kp * e + Kd * de + gravity_torque(q)

    # simple saturation
    tau_max = 10.0
    tau_min = -10.0
    tau = np.clip(tau, tau_min, tau_max)

    # joint dynamics: J ddq + b dq + g(q) = tau
    ddq = (tau - b * dq - gravity_torque(q)) / J

    # Euler integration
    dq = dq + Ts * ddq
    q = q + Ts * dq

    qs[k] = q
    dqs[k] = dq
    taus[k] = tau

# Plot results
plt.figure()
plt.subplot(3, 1, 1)
plt.plot(ts, qs, label="q")
plt.axhline(qd, linestyle="--", label="q_d")
plt.ylabel("position [rad]")
plt.legend()

plt.subplot(3, 1, 2)
plt.plot(ts, dqs)
plt.ylabel("velocity [rad/s]")

plt.subplot(3, 1, 3)
plt.plot(ts, taus)
plt.ylabel("tau [N m]")
plt.xlabel("time [s]")

plt.tight_layout()
plt.show()
      

In your lab report, you should compare the measured overshoot and settling time from this simulation with the values predicted from the second-order model parameters \( \omega_n, \zeta \), and discuss discrepancies due to model simplifications (e.g., Euler integration, unmodeled friction).

For deployment on embedded controllers or within a ROS control loop, a typical C++ implementation uses a class encapsulating the PD + gravity law, often with Eigen for linear algebra (for multi-DOF). Below is a minimal single-joint example without ROS-specific boilerplate:

#include <cmath>
#include <algorithm>

struct JointServo
{
    double Kp;
    double Kd;
    double Ki;
    double tau_min;
    double tau_max;

    double integral;  // integral state

    JointServo(double kp, double kd, double ki,
               double tmin, double tmax)
        : Kp(kp), Kd(kd), Ki(ki),
          tau_min(tmin), tau_max(tmax),
          integral(0.0)
    {}

    double gravity_torque(double q)
    {
        // Example: planar link with m g l sin(q)
        const double m = 1.0;
        const double ell = 0.5;
        const double g = 9.81;
        return m * g * ell * std::sin(q);
    }

    double update(double q, double dq,
                  double qd, double dqd,
                  double dt)
    {
        double e = qd - q;
        double de = dqd - dq;

        // integrate error with simple clamping
        integral += e * dt;
        const double Imax = 5.0;
        if (integral > Imax) integral = Imax;
        if (integral < -Imax) integral = -Imax;

        double tau = Kp * e + Kd * de + Ki * integral
                     + gravity_torque(q);

        // saturation
        tau = std::clamp(tau, tau_min, tau_max);
        return tau;
    }
};

// In your real-time loop:
//   - dt is the measured control period
//   - q, dq from encoders/velocity estimator
//   - qd, dqd from trajectory generator
// JointServo servo(Kp, Kd, Ki, -10.0, 10.0);
// double tau_cmd = servo.update(q, dq, qd, dqd, dt);
      

For multi-DOF arms, you would typically replace the scalar quantities by Eigen vectors and matrices, and use a kinematics/dynamics library (e.g., KDL, RBDL) to compute \( \mathbf{g}(\mathbf{q}) \) for gravity compensation.

Java is less common in industrial robot control, but it appears in educational platforms and some middleware. Here is a minimal Java implementation of the same joint servo (without any external robotics library):

public class JointServo {
    private double Kp;
    private double Kd;
    private double Ki;
    private double tauMin;
    private double tauMax;
    private double integral;

    public JointServo(double kp, double kd, double ki,
                      double tauMin, double tauMax) {
        this.Kp = kp;
        this.Kd = kd;
        this.Ki = ki;
        this.tauMin = tauMin;
        this.tauMax = tauMax;
        this.integral = 0.0;
    }

    private double gravityTorque(double q) {
        double m = 1.0;
        double ell = 0.5;
        double g = 9.81;
        return m * g * ell * Math.sin(q);
    }

    public double update(double q, double dq,
                         double qd, double dqd,
                         double dt) {
        double e = qd - q;
        double de = dqd - dq;

        integral += e * dt;
        double Imax = 5.0;
        if (integral > Imax) integral = Imax;
        if (integral < -Imax) integral = -Imax;

        double tau = Kp * e + Kd * de + Ki * integral
                     + gravityTorque(q);

        if (tau > tauMax) tau = tauMax;
        if (tau < tauMin) tau = tauMin;
        return tau;
    }
}
      

On an actual robotic system, this class would be called from a high-priority control thread that reads joint states from the hardware interface and sends torque commands to the driver.

In MATLAB, we can implement the continuous-time dynamics and controller using ode45, and in Simulink using standard blocks (Sum, Gain, Integrator). Below is a script for simulating one joint with PD + gravity compensation:

% Parameters
J = 0.05;
b = 0.01;
m = 1.0;
ell = 0.5;
g = 9.81;

qd = deg2rad(45);  % desired angle

wn = 8.0;
zeta = 0.7;

Kp = J * wn^2;
Kd = 2 * J * zeta * wn - b;

% continuous dynamics with controller
dyn = @(t, x) [
    x(2);  % x(1) = q, x(2) = dq
    ( ...
      Kp * (qd - x(1)) + ...
      Kd * (0 - x(2)) + ...
      m * g * ell * sin(x(1)) - ...
      b * x(2) - ...
      m * g * ell * sin(x(1)) ...
    ) / J  % J ddq + b dq + g(q) = tau
];

x0 = [0; 0];
tspan = [0 2];
[t, x] = ode45(dyn, tspan, x0);

q = x(:,1);
dq = x(:,2);

figure;
subplot(3,1,1);
plot(t, q); hold on;
yline(qd, "--");
ylabel("q [rad]");
legend("q", "q_d");

subplot(3,1,2);
plot(t, dq);
ylabel("dq [rad/s]");

% compute torque trajectory
tau = Kp * (qd - q) + Kd * (0 - dq) + m * g * ell * sin(q);
subplot(3,1,3);
plot(t, tau);
ylabel("tau [N m]");
xlabel("time [s]");
      

In Simulink, you can construct an equivalent model as follows:

• Use an integrator chain to represent the plant: \( \ddot q \rightarrow \dot q \rightarrow q \).
• Compute \( e = q_d - q \) with a Sum block and feed it to proportional and derivative Gain blocks.
• Compute the gravity torque \( g(q) = m g \ell \sin(q) \) with a Trigonometric Function block and a Gain block.
• Sum the PD output with \( g(q) \), then feed into the plant dynamics.
• Set the solver step size small enough relative to the desired bandwidth.

Mathematica is convenient for symbolic manipulation of the joint dynamics and for simulation. The snippet below simulates the same one-DOF joint under PD + gravity:

J = 0.05;
b = 0.01;
m = 1.0;
ell = 0.5;
g = 9.81;

qd = 45 Degree;

wn = 8.0;
zeta = 0.7;

Kp = J*wn^2;
Kd = 2*J*zeta*wn - b;

grav[q_] := m*g*ell*Sin[q];

eqs = {
  q'[t] == dq[t],
  J*dq'[t] + b*dq[t] + grav[q[t]] ==
    Kp*(qd - q[t]) + Kd*(0 - dq[t]) + grav[q[t]]
};

ics = {q[0] == 0, dq[0] == 0};

sol = NDSolve[
  Join[eqs, ics],
  {q, dq},
  {t, 0, 2}
][[1]];

Plot[
  {q[t] /. sol, qd},
  {t, 0, 2},
  PlotLegends -> {"q(t)", "q_d"},
  AxesLabel -> {"t [s]", "q [rad]"}
]
      

You can modify this notebook to explore the effect of varying \(K_p, K_d\) on the transient response and to symbolically verify the characteristic polynomial and its roots.

A recommended sequence for the lab is:

flowchart TD
  A["Start with one joint in simulation"] --> B["Choose wn, zeta and compute Kp, Kd"]
  B --> C["Run step tests, measure overshoot and settling time"]
  C --> D["Adjust gains if necessary (respect damping and bandwidth limits)"]
  D --> E["Port controller to real arm with very small gains"]
  E --> F["Perform safety checks and limit small motion tests"]
  F --> G["Log joint trajectories and torques for analysis"]
        

On a simulated arm:

• Implement the dynamics and controller as in the Python or MATLAB examples.
• Run step commands in each joint separately (while holding others fixed).
• Record overshoot, rise time, settling time, and steady-state error.

On a real arm:

• Start with very low gains (e.g., \( \omega_n \) small, high damping), verify stability.
• Increase gains gradually to approach desired performance while monitoring motor currents and joint temperatures.
• Compare measured responses with the simulated ones and discuss the influence of unmodeled dynamics (Coulomb friction, flexibility, sensor noise).

In this lab-focused lesson, you:

• reviewed the joint servo architecture and PD/PID + gravity compensation in joint space,
• derived and analyzed the closed-loop error dynamics for one joint, including Lyapunov stability and second-order design formulas,
• implemented the joint servo in multiple programming environments (Python, C++, Java, MATLAB/Simulink, Mathematica),
• outlined a practical procedure to transition from simulation to real hardware while respecting safety and saturation constraints,
• solved analytical problems that connect theoretical design parameters to observed transient performance.

These skills form the foundation for more advanced robot control structures in subsequent chapters, where models will be used more directly inside the controller, but the basic joint servo loop you implemented here will remain a building block.