Classic position control regulates a configuration variable (e.g. joint angle or end-effector pose) to follow a reference, often trying to reject external disturbances. Pure force control, on the other hand, aims to track a desired interaction force even if the resulting motion is not specified in detail. Impedance control interpolates between these two extremes by specifying the dynamic relation between motion and external force.

For an end-effector Cartesian position \( \mathbf{x} \in \mathbb{R}^m \), desired trajectory \( \mathbf{x}_d(t) \), and external wrench \( \mathbf{F}_\text{ext} \), a common target behavior is

\[ \mathbf{M}_d \ddot{\mathbf{e}} + \mathbf{D}_d \dot{\mathbf{e}} + \mathbf{K}_d \mathbf{e} = \mathbf{F}_\text{ext}, \quad \mathbf{e} := \mathbf{x} - \mathbf{x}_d, \]

where \( \mathbf{M}_d \), \( \mathbf{D}_d \), \( \mathbf{K}_d \) are desired inertia, damping, and stiffness matrices the designer chooses. The robot controller then generates joint torques that make the input-output map \( \mathbf{F}_\text{ext} \mapsto \mathbf{x} \) behave as if the end-effector were a mass-spring-damper system.

flowchart TD
  R["Ref trajectory x_d, x_d_dot, x_d_ddot"] --> E["Compute error e = x - x_d"]
  E --> IMP["Impedance law: M_d * e_ddot + D_d * e_dot + K_d * e = F_ext"]
  IMP --> Fd["Compute desired interaction wrench F"]
  Fd --> JT["Map wrench to joint torques via J^T"]
  JT --> ACT["Actuators / low-level torque loop"]
  ACT --> PLANT["Robot + environment"]
  PLANT -->|measured x, F_ext| E
        

The key design question is how to choose and implement \( \mathbf{M}_d, \mathbf{D}_d, \mathbf{K}_d \) so that the robot is sufficiently compliant for safety yet precise enough for the task. This lesson focuses on shaping stiffness and damping and analyzing the resulting closed-loop behavior.

Consider a 1-DOF translational mass–spring–damper system with displacement \( x(t) \), external force \( F_\text{ext}(t) \), mass \( M > 0 \), damping \( D \ge 0 \), and stiffness \( K \ge 0 \):

\[ M \ddot{x}(t) + D \dot{x}(t) + K x(t) = F_\text{ext}(t). \]

Taking Laplace transforms with zero initial conditions, \( X(s) = \mathcal{L}\{x(t)\} \) and \( F_\text{ext}(s) = \mathcal{L}\{F_\text{ext}(t)\} \), and defining the velocity transform \( V(s) = s X(s) \), we obtain

\[ M s^2 X(s) + D s X(s) + K X(s) = F_\text{ext}(s). \]

The mechanical impedance is defined as the ratio of force to velocity in the Laplace domain:

\[ Z(s) := \frac{F_\text{ext}(s)}{V(s)} = \frac{M s^2 + D s + K}{s} = M s + D + \frac{K}{s}. \]

Impedance control aims to make the closed-loop robot dynamics behave like a desired impedance \( Z_d(s) \). In joint space we shape the mapping \( \tau_\text{ext} \mapsto \dot{q} \), while in task space we shape \( \mathbf{F}_\text{ext} \mapsto \dot{\mathbf{x}} \).

Consider a single revolute joint with coordinate \( q(t) \), inertia \( J > 0 \), motor torque \( \tau_m(t) \), and external torque \( \tau_\text{ext}(t) \) due to contact with the environment. Neglecting friction and gravity (or assuming they are compensated), the joint dynamics are

\[ J \ddot{q}(t) = \tau_m(t) + \tau_\text{ext}(t). \]

Let the desired joint trajectory be \( q_d(t) \) with derivatives \( \dot{q}_d(t), \ddot{q}_d(t) \). Define the tracking error \( e(t) := q(t) - q_d(t) \). A simple joint-space impedance controller is

\[ \tau_m(t) = J \ddot{q}_d(t) - K (q(t) - q_d(t)) - D (\dot{q}(t) - \dot{q}_d(t)), \]

where \( K \ge 0 \) and \( D \ge 0 \) are scalar design parameters (desired stiffness and damping). Substituting into the joint dynamics gives

\[ \begin{aligned} J \ddot{q} &= J \ddot{q}_d - K (q - q_d) - D (\dot{q} - \dot{q}_d) + \tau_\text{ext} \\ J (\ddot{q} - \ddot{q}_d) &+ D (\dot{q} - \dot{q}_d) + K (q - q_d) = \tau_\text{ext}. \end{aligned} \]

Since \( e = q - q_d \), \( \dot{e} = \dot{q} - \dot{q}_d \), \( \ddot{e} = \ddot{q} - \ddot{q}_d \), the closed-loop error dynamics are

\[ J \ddot{e}(t) + D \dot{e}(t) + K e(t) = \tau_\text{ext}(t). \]

Thus the joint behaves as a virtual mass–spring–damper driven by the external torque \( \tau_\text{ext} \). When \( \tau_\text{ext} = 0 \) the error dynamics reduce to the homogeneous equation

\[ J \ddot{e}(t) + D \dot{e}(t) + K e(t) = 0. \]

This is the canonical second-order system familiar from basic control theory, and its parameters \( J, D, K \) directly determine transient behavior (overshoot, settling time) and apparent stiffness at the end-effector.

The homogeneous error dynamics \( J \ddot{e} + D \dot{e} + K e = 0 \) can be written in the standard second-order form

\[ \ddot{e}(t) + 2 \zeta \omega_n \dot{e}(t) + \omega_n^2 e(t) = 0 \]

by defining the natural frequency \( \omega_n \) and damping ratio \( \zeta \) as

\[ \omega_n := \sqrt{\frac{K}{J}}, \qquad \zeta := \frac{D}{2 \sqrt{J K}}. \]

Design procedure (1-DOF joint).

1. Choose a desired natural frequency \( \omega_n \) based on bandwidth and tracking requirements. Larger \( \omega_n \) gives faster response but may amplify noise and excite unmodeled high-frequency dynamics.
2. Choose a damping ratio \( \zeta \): critical damping at \( \zeta = 1 \) eliminates overshoot, while a typical trade-off in robotics is \( 0.7 \le \zeta \le 1 \).
3. Compute \( K = J \omega_n^2 \) and \( D = 2 \zeta \sqrt{J K} \). These are the controller gains that realize the desired second-order behavior.

In practice, \( J \) may be configuration-dependent or uncertain. Then \( K \) and \( D \) are tuned experimentally around nominal values of \( J \), and safety limits on virtual stiffness are imposed so that the robot remains compliant during contact.

flowchart TD
  S["Specify performance: bandwidth, overshoot"] --> W["Choose omega_n (natural frequency)"]
  W --> Z["Choose zeta (~0.7..1 for robotics)"]
  Z --> G["Compute K = J * omega_n^2"]
  G --> D["Compute D = 2 * zeta * sqrt(J * K)"]
  D --> C["Apply (K, D) in impedance controller"]
  C --> T["Test and refine with safety limits"]
        

A key reason impedance control is popular in physical human–robot interaction is that the mapping from external torque \( \tau_\text{ext} \) to error velocity \( \dot{e} \) can be made passive. This yields strong stability guarantees when interacting with passive environments (such as springs or rigid walls).

For the 1-DOF system \( J \ddot{e} + D \dot{e} + K e = \tau_\text{ext} \) with \( J > 0 \), \( D \ge 0 \), \( K \ge 0 \), define the storage function

\[ V(e, \dot{e}) := \frac{1}{2} J \dot{e}^2 + \frac{1}{2} K e^2. \]

Differentiating along solutions:

\[ \begin{aligned} \dot{V} &= J \dot{e} \ddot{e} + K e \dot{e} = \dot{e} (J \ddot{e} + K e) \\ &= \dot{e} (\tau_\text{ext} - D \dot{e}) = \dot{e} \, \tau_\text{ext} - D \dot{e}^2. \end{aligned} \]

Thus \( \dot{V} \le \dot{e} \, \tau_\text{ext} \), and the input-output pair \( (\tau_\text{ext}, \dot{e}) \) is passive with storage \( V \). If the environment is also passive in the sense that the integral of \( -\tau_\text{ext} \dot{e} \) is bounded below, the closed-loop interconnection is stable (Lyapunov/passivity arguments).

In the special case of no external torques \( \tau_\text{ext} \equiv 0 \), we obtain

\[ \dot{V}(t) = - D \dot{e}(t)^2 \le 0, \]

which shows that \( V(t) \) is nonincreasing. If \( D > 0 \) and \( K > 0 \), standard LaSalle arguments imply that \( e(t) \rightarrow 0 \) and \( \dot{e}(t) \rightarrow 0 \) as \( t \rightarrow \infty \), so the tracking error converges to zero in free motion.

For an n-DOF manipulator with joint coordinates \( \mathbf{q} \in \mathbb{R}^n \), the standard dynamics (students are assumed to know from robotics dynamics) are

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} + \mathbf{J}(\mathbf{q})^\top \mathbf{F}_\text{ext}, \]

where \( \mathbf{J}(\mathbf{q}) \) is the geometric Jacobian, \( \boldsymbol{\tau} \) are joint torques, and \( \mathbf{F}_\text{ext} \) is the Cartesian external wrench at the end-effector.

One approach to Cartesian impedance control is to use the operational-space inertia \( \boldsymbol{\Lambda}(\mathbf{q}) \) defined as

\[ \boldsymbol{\Lambda}(\mathbf{q}) := \bigl(\mathbf{J}(\mathbf{q}) \mathbf{M}(\mathbf{q})^{-1} \mathbf{J}(\mathbf{q})^\top \bigr)^{-1}, \]

and to realize the desired operational-space behavior

\[ \mathbf{M}_d \ddot{\mathbf{e}} + \mathbf{D}_d \dot{\mathbf{e}} + \mathbf{K}_d \mathbf{e} = \mathbf{F}_\text{ext}, \quad \mathbf{e} := \mathbf{x} - \mathbf{x}_d, \]

via an operational-space control law of the form

\[ \begin{aligned} \mathbf{F} &= \mathbf{M}_d \ddot{\mathbf{x}}_d - \mathbf{K}_d (\mathbf{x} - \mathbf{x}_d) - \mathbf{D}_d (\dot{\mathbf{x}} - \dot{\mathbf{x}}_d) \\ &\quad + \boldsymbol{\mu}(\mathbf{q}, \dot{\mathbf{q}}) + \mathbf{p}(\mathbf{q}), \end{aligned} \]

where \( \boldsymbol{\mu} \) and \( \mathbf{p} \) compensate Coriolis/centrifugal and gravity terms at the task space. The desired joint torques are then

\[ \boldsymbol{\tau} = \mathbf{J}(\mathbf{q})^\top \mathbf{F} + \boldsymbol{\tau}_\text{null}, \]

where \( \boldsymbol{\tau}_\text{null} \) is a null-space torque (e.g., for posture control) such that \( \mathbf{J} \mathbf{M}^{-1} \boldsymbol{\tau}_\text{null} = \mathbf{0} \). In this lesson we focus on the primary task behavior and assume \( \boldsymbol{\tau}_\text{null} = \mathbf{0} \).

Under ideal model matching (\( \mathbf{M}_d = \boldsymbol{\Lambda} \), exact cancellation of nonlinear terms), the closed-loop end-effector behavior is approximately governed by the desired impedance law. In practice, model errors and unmodeled dynamics slightly distort the realized impedance, but the passivity properties derived in joint-space still provide useful guidance for safe design.

Real impedance controllers run in discrete time with sensor noise, actuator limits, and communication delays. These factors impose practical limits on the achievable stiffness and damping.

• Torque and velocity limits. Saturation in \( \boldsymbol{\tau} \) or in joint velocities effectively clips the virtual stiffness, especially during impacts.
• Sensor noise. Damping terms depend on velocity estimates; differentiating noisy position measurements amplifies high frequencies. Low-pass filtering reduces noise but adds phase lag, which can reduce stability margins if gains are too high.
• Sampling and computation delay. High virtual stiffness combined with slow control loops can destabilize contact with stiff environments. A practical rule is to increase stiffness only as far as step-response tests in contact remain well damped.
• Task vs joint space trade-offs. Designing in task space may give intuitive stiffness along Cartesian axes, but mapping to joint torques can violate joint-level limits or create anisotropic stiffness in joint coordinates.

In later chapters on digital control and real-time implementation, these issues are revisited with more detailed discrete-time analysis. For now, it is important to recognize that very large \( \mathbf{K}_d \) is not always desirable; the goal is a stable, well-damped, task-appropriate impedance.

The following Python code simulates a 1-DOF joint with inertia \( J \) under the impedance controller from Section 3. We use numpy for numerical operations and illustrate how one would integrate such a controller with a robotics library (e.g., roboticstoolbox-python) in practice.

import numpy as np

# Physical parameters
J = 0.05  # joint inertia [kg m^2]
K = 10.0  # virtual stiffness [Nm/rad]
D = 2.0   # virtual damping [Nms/rad]

# Desired trajectory: step from 0 to 0.5 rad
def q_d(t):
    return 0.5 if t >= 0.1 else 0.0

def qd_d(t):
    return 0.0

def qdd_d(t):
    return 0.0

# External torque (e.g. contact disturbance)
def tau_ext(t):
    # short pulse disturbance
    return 0.2 if 0.3 <= t <= 0.35 else 0.0

# Simulation setup
dt = 0.001
T = 1.0
N = int(T / dt)

q = 0.0
qd = 0.0

ts = []
qs = []
qds = []
taus = []

for k in range(N):
    t = k * dt

    # Desired signals
    q_ref = q_d(t)
    qd_ref = qd_d(t)
    qdd_ref = qdd_d(t)

    e = q - q_ref
    ed = qd - qd_ref

    # Impedance control law:
    # tau_m = J * qdd_ref - K * e - D * ed
    tau_m = J * qdd_ref - K * e - D * ed

    # Plant dynamics: J * qdd = tau_m + tau_ext
    qdd = (tau_m + tau_ext(t)) / J

    # Euler integration (for illustration)
    qd = qd + dt * qdd
    q = q + dt * qd

    ts.append(t)
    qs.append(q)
    qds.append(qd)
    taus.append(tau_m)

# Plotting (if matplotlib is available)
try:
    import matplotlib.pyplot as plt

    plt.figure()
    plt.plot(ts, qs, label="q(t)")
    plt.plot(ts, [q_d(t) for t in ts], "--", label="q_d(t)")
    plt.xlabel("time [s]")
    plt.ylabel("position [rad]")
    plt.legend()
    plt.title("1-DOF Joint Impedance Control (Python)")
    plt.show()
except ImportError:
    print("matplotlib not installed; skipping plots.")
      

In a full robot application, q and qd would come from joint encoders, and tau_m would be sent to the torque interface of a robot control framework such as ROS-control.

The following C++ snippet illustrates how a 1-DOF impedance controller could be implemented in a real-time loop, for instance inside a ROS controller node using libraries such as KDL for kinematics/dynamics.

#include <iostream>
#include <cmath>

struct JointImpedanceController {
    double J;  // inertia
    double K;  // stiffness
    double D;  // damping

    JointImpedanceController(double J_, double K_, double D_)
        : J(J_), K(K_), D(D_) {}

    // Compute motor torque given states and desired trajectory
    double computeTorque(double q, double qd,
                         double q_ref, double qd_ref, double qdd_ref,
                         double tau_ext) {
        double e = q - q_ref;
        double ed = qd - qd_ref;
        double tau_m = J * qdd_ref - K * e - D * ed;
        // total torque applied to joint (motor only)
        return tau_m;
    }
};

int main() {
    JointImpedanceController ctrl(0.05, 10.0, 2.0);

    double q = 0.0;
    double qd = 0.0;
    double dt = 0.001;
    double T = 1.0;
    int N = static_cast<int>(T / dt);

    for (int k = 0; k < N; ++k) {
        double t = k * dt;
        double q_ref = (t >= 0.1) ? 0.5 : 0.0;
        double qd_ref = 0.0;
        double qdd_ref = 0.0;

        double tau_ext = (t >= 0.3 && t <= 0.35) ? 0.2 : 0.0;

        double tau_m = ctrl.computeTorque(q, qd, q_ref, qd_ref, qdd_ref, tau_ext);

        // Plant integration: J * qdd = tau_m + tau_ext
        double qdd = (tau_m + tau_ext) / ctrl.J;

        qd += dt * qdd;
        q += dt * qd;
    }

    std::cout << "Final q = " << q << std::endl;
    return 0;
}
      

In a multi-DOF setting, the scalar variables become vectors and matrices, and computeTorque would use a dynamics library (e.g. KDL, RBDL, Pinocchio) to compute \( \mathbf{M}(\mathbf{q}) \), \( \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \), \( \mathbf{g}(\mathbf{q}) \), and \( \mathbf{J}(\mathbf{q}) \).

Java is often used for higher-level control or simulation in robotics. Below is a simple 1-DOF impedance controller; matrix operations for multi-DOF systems could rely on libraries such as EJML.

public class JointImpedanceController {
    private double J;  // inertia
    private double K;  // stiffness
    private double D;  // damping

    public JointImpedanceController(double J, double K, double D) {
        this.J = J;
        this.K = K;
        this.D = D;
    }

    public double computeTorque(double q, double qd,
                                double qRef, double qdRef, double qddRef,
                                double tauExt) {
        double e = q - qRef;
        double ed = qd - qdRef;
        double tauM = J * qddRef - K * e - D * ed;
        return tauM;
    }

    public static void main(String[] args) {
        JointImpedanceController ctrl =
            new JointImpedanceController(0.05, 10.0, 2.0);

        double q = 0.0;
        double qd = 0.0;
        double dt = 0.001;
        double T = 1.0;
        int N = (int) (T / dt);

        for (int k = 0; k < N; ++k) {
            double t = k * dt;
            double qRef = (t >= 0.1) ? 0.5 : 0.0;
            double qdRef = 0.0;
            double qddRef = 0.0;

            double tauExt = (t >= 0.3 && t <= 0.35) ? 0.2 : 0.0;

            double tauM = ctrl.computeTorque(q, qd, qRef, qdRef, qddRef, tauExt);

            double qdd = (tauM + tauExt) / ctrl.J;

            qd += dt * qdd;
            q += dt * qd;
        }

        System.out.println("Final q = " + q);
    }
}
      

Java-based simulation environments can use this logic inside a physics engine loop where the plant dynamics are handled by the engine and only the impedance law is implemented in user code.

MATLAB and Simulink are widely used for rapid prototyping of robot controllers. The script below simulates the same 1-DOF impedance system. In Simulink, the same model can be built using blocks.

% Parameters
J = 0.05;
K = 10.0;
D = 2.0;

% Simulation parameters
dt = 1e-3;
T  = 1.0;
N  = T / dt;

q   = 0.0;
qd  = 0.0;

ts  = zeros(N,1);
qs  = zeros(N,1);
qds = zeros(N,1);

for k = 1:N
    t = (k-1) * dt;

    if t >= 0.1
        q_ref = 0.5;
    else
        q_ref = 0.0;
    end
    qd_ref  = 0.0;
    qdd_ref = 0.0;

    if t >= 0.3 && t <= 0.35
        tau_ext = 0.2;
    else
        tau_ext = 0.0;
    end

    e  = q  - q_ref;
    ed = qd - qd_ref;

    tau_m = J * qdd_ref - K * e - D * ed;

    qdd = (tau_m + tau_ext) / J;

    qd = qd + dt * qdd;
    q  = q  + dt * qd;

    ts(k)  = t;
    qs(k)  = q;
    qds(k) = qd;
end

plot(ts, qs, ts, (ts >= 0.1) * 0.5);
legend('q(t)', 'q_d(t)');
xlabel('time [s]');
ylabel('position [rad]');
title('1-DOF Joint Impedance Control (MATLAB)');
      

Simulink block diagram. The same dynamics can be built in Simulink by cascading blocks:

• A Step block for \( q_d(t) \), subtracting the measured \( q(t) \) to form the error \( e(t) \).
• A gain block K on \( e \) and a gain block D on \( \dot{e} \), summed to form the virtual spring-damper torque.
• A Sum block adding \( \tau_m \) and the external torque input to form the net torque on the inertia block.
• An Integrator chain modeling the inertia \( J \) (from torque to acceleration, to velocity, to position).

Mathematica is well suited for both symbolic and numeric analysis of impedance dynamics. Below is a simple numeric simulation using NDSolve.

J  = 0.05;
K  = 10.0;
D  = 2.0;

qDes[t_] := Piecewise[{ {0.0, t < 0.1} }, 0.5];
qdDes[t_] := 0.0;
qddDes[t_] := 0.0;

tauExt[t_] := Piecewise[{ {0.2, 0.3 <= t <= 0.35} }, 0.0];

impedanceEqns = {
  q''[t] == (J*qddDes[t] - K*(q[t] - qDes[t]) - D*(q'[t] - qdDes[t]) + tauExt[t])/J,
  q[0] == 0.0,
  q'[0] == 0.0
};

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

Plot[{q[t] /. sol[[1]], qDes[t]}, {t, 0, 1},
     PlotLegends -> {"q(t)", "q_d(t)"},
     AxesLabel -> {"t", "position"}]
      

Symbolic manipulations (e.g. deriving \( \omega_n \) and \( \zeta \) as functions of \( J, D, K \)) can be done by introducing a Laplace variable and using LaplaceTransform and Simplify.

In this lesson we formalized impedance control as the design of a virtual mass–spring–damper relation between motion and external force. For a 1-DOF joint, a simple PD-plus-feedforward controller produces closed-loop dynamics of the form \( J \ddot{e} + D \dot{e} + K e = \tau_\text{ext} \), whose natural frequency and damping ratio can be shaped via \( K \) and \( D \). Using an operational-space formulation, similar behavior can be imposed at the end-effector, with joint torques obtained by transposing the Jacobian.

We also used energy-based arguments to show passivity of the impedance map, providing strong stability guarantees during interaction with passive environments. Finally, we illustrated basic implementations of impedance control in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, enabling students to experiment with stiffness–damping shaping in simulation and prepare for real robotic platforms.