Up to this point, control objectives were mainly formulated as motion-tracking tasks: the end-effector should follow a desired joint-space or task-space trajectory \( \mathbf{q}_d(t) \) or \( \mathbf{x}_d(t) \) with small tracking error. The canonical joint-space dynamics (without external contact) are

\[ \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 inertia matrix, \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) captures Coriolis and centrifugal terms, and \( \mathbf{g}(\mathbf{q}) \) is gravity. A standard model-based controller (e.g. computed-torque) shapes the closed-loop dynamics to behave like a well-damped second-order system, focusing on motion error \( \mathbf{e}_q = \mathbf{q} - \mathbf{q}_d \).

In interaction tasks (polishing, assembly, human–robot collaboration, surface following), the quality of performance is mainly determined by the forces exerted on the environment: normal forces should stay inside a safe band, tangential forces should match process constraints, and impulsive forces should be minimized. Let \( \mathbf{F} \) denote the generalized contact force at the end-effector and \( \mathbf{F}_d \) a desired force (or wrench).

The central issue is that motion and force at the contact are coupled. Pushing a rigid wall by an additional \( 1 \,\text{mm} \) may create hundreds of Newtons of extra force, while the same displacement in free space creates none.

flowchart TD
  T0["Robotic task"] --> T1["Free-space motion?"]
  T1 -->|yes| GOAL1["Primary goal: \ntrack trajectory x_d(t)"]
  T1 -->|no| T2["Task involves contact with environment"]
  T2 --> T3["Identify interaction goals: \nforce bands, stiffness, safety"]
  T3 --> T4["Choose control focus"]
  T4 -->|dominant motion| C1["Motion-centric control \nwith force limits"]
  T4 -->|dominant force| C2["Force-centric or impedance control"]
  C1 --> MOTIV["Need to understand \nmotion-force coupling"]
  C2 --> MOTIV
        

This lesson develops simple analytical models for motion–force coupling, showing rigorously that:

• Pure position tracking can yield unbounded contact forces when the environment is stiff.
• Closed-loop stability and safety depend on the combined robot–environment dynamics.
• Force-centric or impedance objectives are mathematically natural for contact tasks.

We recall the mapping between task-space wrenches and joint torques. For an end-effector position/orientation vector \( \mathbf{x} \in \mathbb{R}^m \) with Jacobian \( \mathbf{J}(\mathbf{q}) = \partial \mathbf{x}/\partial \mathbf{q} \), a contact wrench \( \mathbf{F} \in \mathbb{R}^m \) induces joint torques

\[ \boldsymbol{\tau}_\text{contact} = \mathbf{J}(\mathbf{q})^\top \mathbf{F}. \]

Accounting for contact, the joint-space dynamics become

\[ \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}. \]

The environment must now be modeled to close the loop between \( \mathbf{x} \) and \( \mathbf{F} \). In many tasks a local linear approximation of the environment normal force is appropriate:

\[ F_n = \begin{cases} 0, & \text{if } x_n \le x_s,\\[4pt] k_e (x_n - x_s) + b_e \dot{x}_n, & \text{if } x_n > x_s, \end{cases} \]

where:

• \( x_n \) is displacement along the surface normal,
• \( x_s \) is the (unknown or partially known) surface location,
• \( k_e \) is environment stiffness,
• \( b_e \) is environment damping.

For analytical insight, we will use a 1-DOF translational model with generalized coordinate \( x \) interacting with a stiff wall at \( x = x_s \). The robot is modeled as a mass–spring–damper plus control input:

\[ m \ddot{x} + b \dot{x} + k_r (x - x_0) = u - F_e(x,\dot{x}), \]

where \( m \), \( b \), \( k_r \) are effective robot parameters, \( x_0 \) is a bias position, and \( F_e \) is the environment reaction force of the form above.

Consider a simple proportional position controller in task space for the 1-DOF model:

\[ u = k_p (x_d - x), \]

where \( x_d \) is a desired position given by a trajectory planner designed under free-space assumptions. Suppose that this trajectory commands the robot to penetrate the wall, i.e. \( x_d > x_s \).

At steady state in contact with negligible velocity (\( \dot{x} \approx 0 \), \( \ddot{x} \approx 0 \)), the dynamics reduce to

\[ k_r (x - x_0) \approx u - F_e(x) = k_p (x_d - x) - k_e (x - x_s). \]

Solving for the equilibrium position \( x^\ast \):

\[ k_r (x^\ast - x_0) = k_p (x_d - x^\ast) - k_e (x^\ast - x_s), \]

\[ (k_r + k_p + k_e) x^\ast = k_p x_d + k_e x_s + k_r x_0, \]

\[ x^\ast = \frac{k_p x_d + k_e x_s + k_r x_0}{k_r + k_p + k_e}. \]

The steady-state contact force equals:

\[ F_e^\ast = k_e (x^\ast - x_s) = \frac{k_e}{k_r + k_p + k_e} \bigl(k_p (x_d - x_s) + k_r (x_0 - x_s)\bigr). \]

If we choose the reference frame such that \( x_0 = x_s \) (robot mechanical spring centered at the surface), this simplifies to

\[ F_e^\ast = \frac{k_e k_p}{k_r + k_p + k_e} (x_d - x_s). \]

Several key observations follow:

• For a stiff environment \( k_e \rightarrow \infty \), the factor \( \dfrac{k_e k_p}{k_r + k_p + k_e} \rightarrow k_p \), so \( F_e^\ast \approx k_p (x_d - x_s) \). A tiny position overshoot in the planner can produce a very large force proportional to \( k_p \).
• Increasing \( k_p \) to improve tracking in free space directly amplifies contact forces.
• The controller has no representation of a desired force \( F_d \). Any reasonable force objective (e.g. staying below a safety limit \( F_{\max} \)) is at best enforced by ad hoc saturation on \( u \).

Thus, even in this trivial 1-DOF case, position control is poorly matched to contact tasks: the controlled variable is displacement, but the task objective is force.

Consider the linearized closed-loop dynamics of the 1-DOF robot in contact under PD position control:

\[ u = k_p (x_d - x) - k_d \dot{x}. \]

Neglecting the internal robot spring \( k_r \) for simplicity (typical for high-bandwidth actuators) and choosing \( x_0 = x_s \), we obtain

\[ m \ddot{x} + (b + k_d) \dot{x} + (k_p + k_e) x = k_p x_d + k_e x_s. \]

The homogeneous dynamics are governed by the characteristic polynomial

\[ m \lambda^2 + (b + k_d) \lambda + (k_p + k_e) = 0. \]

The system is asymptotically stable if and only if

\[ m > 0,\quad b + k_d > 0,\quad k_p + k_e > 0. \]

For continuous-time dynamics, these conditions are easily satisfied by choosing \( k_d > -b \) and \( k_p > -k_e \). However:

• In practice, control is implemented digitally; large \( k_p + k_e \) leads to lightly damped or unstable discrete-time dynamics at a given sampling period.
• The effective interaction stiffness (how the environment perceives the robot) is dominated by \( k_p + k_e \). This can make the robot appear very stiff and non-compliant, which is undesirable for safety and for many tasks (e.g. surface following).

A more appropriate specification is to choose a desired interaction stiffness \( k_{\text{int}} \) and damping \( b_{\text{int}} \), and design the controller such that the closed-loop mapping \( x_d \mapsto F_e \) (or \( F_d \mapsto x \)) has those properties. This is precisely the domain of impedance and admittance control, but the mathematical motivation stems from the inadequacy of naive position control derived above.

To strengthen intuition, we implement a simple discrete-time simulation of the 1-DOF system:

\[ m \ddot{x} + b \dot{x} = u - F_e,\quad F_e = \begin{cases} 0, & x \le x_s,\\ k_e (x - x_s), & x > x_s, \end{cases} \]

with a PD position controller and a crude force-limited variant that saturates the control input when the contact force exceeds a threshold \( F_{\max} \). This is not a principled force controller, but it numerically illustrates the problems that motivate force/impedance control.

5.1 Python (NumPy) Implementation

import numpy as np

m = 1.0       # mass
b = 1.0       # viscous damping
k_p = 200.0   # position gain
k_d = 20.0    # velocity gain
k_e = 5e3     # environment stiffness (very stiff)
x_s = 0.0     # wall at x = 0
F_max = 80.0  # crude force limit

dt = 0.0005
T = 1.0
N = int(T / dt)

# desired trajectory: step from -0.02 to +0.03 m (penetrates wall)
def x_d(t):
    return -0.02 if t < 0.1 else 0.03

x = -0.02
v = 0.0

xs = np.zeros(N)
Fs = np.zeros(N)
us = np.zeros(N)
ts = np.linspace(0.0, T, N)

for i, t in enumerate(ts):
    # environment force
    if x <= x_s:
        F_e = 0.0
    else:
        F_e = k_e * (x - x_s)

    # PD position control
    e = x_d(t) - x
    u = k_p * e - k_d * v

    # crude force limit
    if F_e > F_max:
        # reduce actuation in same direction as penetration
        u = u - (F_e - F_max)

    # dynamics integration (explicit Euler)
    a = (u - F_e - b * v) / m
    v = v + dt * a
    x = x + dt * v

    xs[i] = x
    Fs[i] = F_e
    us[i] = u

# xs: position trajectory
# Fs: contact forces
# us: control inputs
# These can be plotted (time vs x and time vs F_e) to visualize force spikes.
      

5.2 C++ Implementation (Core Loop)

#include <iostream>
#include <vector>
#include <cmath>

int main() {
    const double m = 1.0;
    const double b = 1.0;
    const double k_p = 200.0;
    const double k_d = 20.0;
    const double k_e = 5000.0;
    const double x_s = 0.0;
    const double F_max = 80.0;
    const double dt = 0.0005;
    const double T = 1.0;
    const int N = static_cast<int>(T / dt);

    auto x_d = [](double t) {
        return (t < 0.1) ? -0.02 : 0.03;
    };

    double x = -0.02;
    double v = 0.0;

    std::vector<double> xs(N), Fs(N), us(N);

    for (int i = 0; i < N; ++i) {
        double t = i * dt;

        double F_e = (x <= x_s) ? 0.0 : k_e * (x - x_s);

        double e = x_d(t) - x;
        double u = k_p * e - k_d * v;

        if (F_e > F_max) {
            u -= (F_e - F_max);
        }

        double a = (u - F_e - b * v) / m;
        v += dt * a;
        x += dt * v;

        xs[i] = x;
        Fs[i] = F_e;
        us[i] = u;
    }

    // Print final values as a sanity check
    std::cout << "Final x = " << xs.back()
              << ", Final F_e = " << Fs.back() << std::endl;
    return 0;
}
      

5.3 Java Implementation (Core Loop)

public class ForceControlMotivation1DOF {

    static double x_d(double t) {
        return (t < 0.1) ? -0.02 : 0.03;
    }

    public static void main(String[] args) {
        double m = 1.0;
        double b = 1.0;
        double k_p = 200.0;
        double k_d = 20.0;
        double k_e = 5000.0;
        double x_s = 0.0;
        double F_max = 80.0;
        double dt = 0.0005;
        double T = 1.0;
        int N = (int) (T / dt);

        double x = -0.02;
        double v = 0.0;

        double[] xs = new double[N];
        double[] Fs = new double[N];

        for (int i = 0; i < N; i++) {
            double t = i * dt;

            double F_e = (x <= x_s) ? 0.0 : k_e * (x - x_s);

            double e = x_d(t) - x;
            double u = k_p * e - k_d * v;

            if (F_e > F_max) {
                u -= (F_e - F_max);
            }

            double a = (u - F_e - b * v) / m;
            v += dt * a;
            x += dt * v;

            xs[i] = x;
            Fs[i] = F_e;
        }

        System.out.println("Final x = " + xs[N - 1]
                + ", Final F_e = " + Fs[N - 1]);
    }
}
      

5.4 MATLAB/Simulink Script

m   = 1.0;
b   = 1.0;
k_p = 200.0;
k_d = 20.0;
k_e = 5e3;
x_s = 0.0;
F_max = 80.0;

dt = 5e-4;
T  = 1.0;
N  = round(T / dt);

x = -0.02;
v = 0.0;

xs = zeros(N,1);
Fs = zeros(N,1);
ts = (0:N-1)' * dt;

for i = 1:N
    t = ts(i);
    if t < 0.1
        xd = -0.02;
    else
        xd = 0.03;
    end

    if x <= x_s
        F_e = 0;
    else
        F_e = k_e * (x - x_s);
    end

    e = xd - x;
    u = k_p * e - k_d * v;

    if F_e > F_max
        u = u - (F_e - F_max);
    end

    a = (u - F_e - b * v) / m;
    v = v + dt * a;
    x = x + dt * v;

    xs(i) = x;
    Fs(i) = F_e;
end

% Plot results
figure; subplot(2,1,1);
plot(ts, xs); ylabel('x (m)'); grid on;
subplot(2,1,2);
plot(ts, Fs); ylabel('F_e (N)'); xlabel('t (s)'); grid on;
      

A Simulink diagram would mirror this loop using Integrator blocks for x and v, a MATLAB Function block or simple arithmetic blocks for F_e, the PD law and saturation on u.

5.5 Wolfram Mathematica Implementation

m   = 1.0;
b   = 1.0;
kP  = 200.0;
kD  = 20.0;
kE  = 5000.0;
xS  = 0.0;
fMax = 80.0;

xDes[t_] := Piecewise[{ {-0.02, t < 0.1}, {0.03, t >= 0.1} }];

dt = 0.0005;
tEnd = 1.0;
steps = Round[tEnd/dt];

x  = -0.02;
v  = 0.0;

data = Reap[
  Do[
    t = n*dt;
    fE = If[x <= xS, 0.0, kE (x - xS)];
    e  = xDes[t] - x;
    u  = kP e - kD v;
    If[fE > fMax, u = u - (fE - fMax)];
    a  = (u - fE - b v)/m;
    v  = v + dt a;
    x  = x + dt v;
    Sow[{t, x, fE}],
    {n, 0, steps}
  ]
][[2, 1]];

ListLinePlot[
  {data[[All, {1, 2}]], data[[All, {1, 3}]]},
  PlotLegends -> {"x(t)", "F_e(t)"},
  GridLines -> Automatic
]
      

In all implementations, increasing k_p or the environment stiffness k_e leads to very large and oscillatory contact forces under pure position control, numerically confirming the analytical arguments from Sections 3 and 4.

The main lesson from the analysis is that task requirements must be expressed not only in terms of motion but also in terms of forces, and the control structure must reflect those requirements.

flowchart TD
  A["Specify task: surface following, insertion, collaboration"] --> B["Define motion constraints (position, orientation)"]
  B --> C["Define force constraints (normal force band, limits)"]
  C --> D["Model environment locally (stiffness, damping, friction)"]
  D --> E["Analyze robot-environment interaction dynamics"]
  E --> F["Decide control objective: motion, force, or impedance"]
  F --> G["Select architecture: position with limits, hybrid, impedance"]
  G --> H["Tune gains to meet stability and interaction stiffness goals"]
        

Subsequent lessons in this chapter will formalize hybrid position/force, impedance, and admittance control, all of which follow this general design flow but implement different control objectives mathematically.

In this lesson we started from familiar robot dynamics and introduced simple contact models to formalize motion–force coupling. For a 1-DOF example, we proved that pure position control can yield large and poorly controlled contact forces, and that closed-loop properties depend on the combined robot–environment stiffness and damping.

We defined the notion of effective interaction stiffness and showed how it emerges from the controller and environment, rather than being a directly specified design parameter. Numerical simulations in Python, C++, Java, MATLAB and Mathematica illustrated how high-gain position control leads to force spikes in stiff contact.

These analyses provide the mathematical motivation for the force- and impedance-based control architectures studied in the remaining lessons of this chapter.