We consider a robot (or a 1-DOF surrogate) that moves along a known contact normal direction toward a rigid environment (e.g., a vertical wall or a tabletop). The high-level goal is to track a position command that brings the end-effector to the surface, but without exceeding a prescribed safe contact force.

Building on previous lessons, we assume:

• The robot dynamics and Jacobians are known (from kinematics/dynamics).
• We already know how to generate a desired joint or task-space velocity command from a standard tracking controller (e.g., PD or computed-torque).
• We know how to handle inequality constraints via saturation / feasibility (Lesson 3).

In this lab, we wrap any such nominal controller with a safety filter that modifies the commanded normal velocity when the robot is close to contact, such that:

• Impact velocity is bounded by a function of distance to the surface.
• Under a worst-case environment stiffness, the peak contact force cannot exceed a user-chosen limit.

flowchart TD
  A["Start control cycle (k)"] --> B["Read state q(k), qdot(k)"]
  B --> C["Compute distance d(k) to surface and normal velocity v_n(k)"]
  C --> D["Compute safe bound v_safe(d(k))"]
  D --> E["Get nominal command v_des(k) from tracking controller"]
  E --> F["Apply safety filter: modify normal component to satisfy |v_n| <= v_safe"]
  F --> G["Convert to joint command (e.g., qdot_cmd)"]
  G --> H["Send to low-level servo / simulator and log data"]
        

We first analyze a scalar surrogate model: a point mass of mass \( m \) moving along a line toward a rigid wall at the origin. Let \( x(t) > 0 \) denote the distance to the wall, with the wall at \( x = 0 \). Before contact, dynamics are:

\[ m \ddot{x}(t) = u(t), \quad x(t)>0. \]

The environment is modeled as a worst-case linear spring of stiffness \( k_{\text{env}} \) when penetrated:

\[ F_{\text{env}}(x) = \begin{cases} 0, & x \ge 0,\\ -k_{\text{env}} x, & x < 0. \end{cases} \]

Suppose the mass hits the wall with (signed) velocity \( v_0 = \dot{x}(t_0^-) < 0 \) at the moment \( x(t_0) = 0 \). After contact, assuming no control action for a short time window, the dynamics become a pure mass-spring system. By energy conservation:

\[ \frac{1}{2} m v_0^2 = \frac{1}{2} k_{\text{env}} x_{\max}^2, \]

where \( x_{\max} < 0 \) is the maximal penetration. Taking magnitudes:

\[ |x_{\max}| = |v_0| \sqrt{\frac{m}{k_{\text{env}}}}. \]

The maximum contact force magnitude is:

\[ F_{\max} = k_{\text{env}} |x_{\max}| = |v_0| \sqrt{m k_{\text{env}}}. \]

If we require that the peak force not exceed a safety threshold \( F_{\text{safe}} \), we must ensure

\[ F_{\max} \le F_{\text{safe}} \quad \Rightarrow \quad |v_0| \le \frac{F_{\text{safe}}}{\sqrt{m k_{\text{env}}}}. \]

Thus, for a given mass and environment stiffness, there is a maximum safe impact speed \( v_{\text{force}} \):

\[ v_{\text{force}} := \frac{F_{\text{safe}}}{\sqrt{m k_{\text{env}}}}. \]

This relation is the first building block of our contact-safe filter. In practice, we take \( k_{\text{env}} \) as a worst-case upper bound on environment stiffness, so that the above inequality is conservative.

The previous bound constrains the impact velocity. We also want to ensure that the controller can brake to (almost) zero velocity before the wall from a given distance. Assume a known maximum deceleration ability \( a_{\text{brake}} > 0 \) (e.g. from torque limits). If we adopt a simple constant-deceleration model from distance \( d \), then the stopping distance constraint is:

\[ d_{\text{stop}} = \frac{v^2}{2 a_{\text{brake}}} \le d \quad \Rightarrow \quad |v| \le \sqrt{2 a_{\text{brake}} d}. \]

Thus, if at distance \( d \) we enforce \( |v| \le v_{\text{brake}}(d) \) with

\[ v_{\text{brake}}(d) := \sqrt{2 a_{\text{brake}} d}, \]

then the mass can, in principle, stop before reaching the wall.

Combining this with the force-based bound yields a distance-dependent safe velocity:

\[ v_{\text{safe}}(d) := \min\bigl\{ v_{\text{force}},\, v_{\text{brake}}(d),\, v_{\max} \bigr\}, \]

where \( v_{\max} \) is a global speed limit (e.g. from hardware or performance requirements). The lab will implement a controller that guarantees \( |\dot{x}(t)| \le v_{\text{safe}}(x(t)) \).

For an \( n \)-DOF manipulator with joint coordinates \( q \in \mathbb{R}^n \), inertia matrix \( \mathbf{M}(q) \), and end-effector Jacobian \( \mathbf{J}(q) \in \mathbb{R}^{3 \times n} \), the standard dynamics with contact wrench \( \lambda \) are

\[ \mathbf{M}(q)\ddot{q} + \mathbf{C}(q,\dot{q})\dot{q} + \mathbf{g}(q) + \mathbf{J}(q)^\top \lambda = \boldsymbol{\tau}. \]

Let \( x(q) \in \mathbb{R}^3 \) be the end-effector position and \( \mathbf{J}_x(q) \) its translational Jacobian so that \( \dot{x} = \mathbf{J}_x(q)\dot{q} \). For a unit contact normal \( n \in \mathbb{R}^3 \), the normal component of end-effector velocity is

\[ v_n = n^\top \dot{x} = n^\top \mathbf{J}_x(q)\dot{q}. \]

The operational-space inertia matrix (already introduced in task-space control) is

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

The effective mass in direction \( n \) is the scalar

\[ m_{\text{eff},n}(q) := \frac{1}{n^\top \mathbf{J}_x(q)\mathbf{M}(q)^{-1}\mathbf{J}_x(q)^\top n} = n^\top \boldsymbol{\Lambda}(q) n. \]

This scalar plays the same role as \( m \) in the 1-DOF case: if the end-effector hits a spring-like environment along \( n \), the peak force is approximately

\[ F_{\max} \approx |v_n(t_0^-)| \sqrt{ m_{\text{eff},n}(q(t_0))\, k_{\text{env}} }. \]

Therefore, for a given force limit \( F_{\text{safe}} \) and stiffness bound \( k_{\text{env}} \), the safe impact velocity along \( n \) satisfies

\[ |v_n(t_0^-)| \le v_{\text{force},n}(q(t_0)) := \frac{F_{\text{safe}}}{\sqrt{ m_{\text{eff},n}(q(t_0))\, k_{\text{env}} }}. \]

We will use \( v_{\text{safe}}(d,q) \) in the manipulator case, combining distance-based braking and configuration-dependent effective mass:

\[ v_{\text{safe}}(d,q) := \min\Bigl\{ v_{\text{force},n}(q),\; \sqrt{2 a_{\text{brake}} d},\; v_{\max} \Bigr\}. \]

Let \( v_{\text{des}} \in \mathbb{R}^3 \) be the nominal end-effector velocity produced by a tracking controller (e.g., operational-space controller from Chapter 4). We decompose it into normal and tangential components:

\[ v_{\text{des},n} = (n^\top v_{\text{des}}) n, \quad v_{\text{des},t} = v_{\text{des}} - v_{\text{des},n}. \]

Suppose the robot is outside contact (\( d > 0 \)) and moving toward the surface, i.e. \( n^\top v_{\text{des}} < 0 \). Define the scalar:

\[ v_n^{\text{des}} := n^\top v_{\text{des}} < 0. \]

We then apply a scalar saturation on the normal component:

\[ v_n^{\text{safe}} := \max\bigl\{-v_{\text{safe}}(d,q),\, v_n^{\text{des}}\bigr\}, \]

and reconstruct the filtered velocity:

\[ v_{\text{safe}} = v_{\text{des},t} + v_n^{\text{safe}} n. \]

If \( n^\top v_{\text{des}} \ge 0 \) (moving away from the surface), we simply take \( v_{\text{safe}} = v_{\text{des}} \).

Guarantee (1-DOF surrogate). In the 1-DOF case (\( v_{\text{safe}}(d) \) scalar, motion toward smaller \( x \)), if at each time \( t \) we enforce \( |\dot{x}(t)| \le v_{\text{safe}}(x(t)) \), then:

• The mass cannot hit the wall with speed exceeding \( v_{\text{force}} \), hence \( F_{\max} \le F_{\text{safe}} \).
• If the controller actually uses deceleration up to \( a_{\text{brake}} \), the mass can fully stop before the wall (up to numerical tolerances).

The manipulator case uses the same idea along the normal direction with \( m_{\text{eff},n}(q) \) instead of \( m \).

In this lab, we implement the safety filter both for a scalar mass and in a generic normal direction, and we log the resulting contact force to verify the theoretical bound.

flowchart TD
  S["Nominal controller outputs v_des"] --> D["Compute distance d and normal n"]
  D --> B["Compute v_safe(d,q) from force and braking limits"]
  B --> P["Project v_des to safe v_safe: saturate normal component"]
  P --> J["Convert v_safe to joint command (qdot_cmd = J^+ v_safe)"]
  J --> L["Send command to robot / simulator; log x, v, F"]
        

We simulate the 1-DOF system with sampling period \( h \). Let \( x_k \approx x(kh) \), \( v_k \approx \dot{x}(kh) \). A simple explicit Euler discretization is:

\[ \begin{aligned} v_{k+1} &= v_k + \frac{h}{m} u_k,\\ x_{k+1} &= x_k + h v_k. \end{aligned} \]

For contact force logging we use:

\[ F_k = \begin{cases} 0, & x_k \ge 0,\\ -k_{\text{env}} x_k, & x_k < 0. \end{cases} \]

The controller at step \( k \):

1. Compute distance \( d_k := x_k \).
2. Compute nominal velocity command \( v_k^{\text{des}} \) (e.g. from a PD controller).
3. Compute \( v_{\text{safe}}(d_k) \) and apply the scalar saturation to obtain \( v_k^{\text{safe}} \).
4. Choose a simple velocity-tracking law, e.g. \( u_k = m k_v (v_k^{\text{safe}} - v_k) \).

This is the core loop implemented in all code listings below (with minor syntactic differences between languages).

The Python code uses numpy for simulation and can be easily extended to a manipulator by replacing the scalar dynamics with calls to a robotics library (e.g., a custom dynamics function or a simulator).

import numpy as np
import matplotlib.pyplot as plt

# Physical parameters
m = 1.0                # kg
k_env = 5000.0         # N/m (worst-case environment stiffness)
F_safe = 80.0          # N (allowed peak force)
a_brake = 30.0         # m/s^2 (approx. max decel)
v_max = 1.0            # m/s (global speed limit)

# Derived parameters
v_force = F_safe / np.sqrt(m * k_env)

# Simulation parameters
h = 0.0005             # s
T = 1.0                # total time
N = int(T / h)

# Controller gains (nominal PD in position)
x_target = 0.0         # wall at x=0
k_p = 50.0
k_d = 5.0
k_v = 200.0            # velocity servo gain

# Initial conditions
x = 0.25               # 25 cm away from wall
v = 0.0

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

for k in range(N):
    # Distance to wall
    d = x

    # Nominal desired velocity from PD on position
    # Negative sign moves toward the wall at x=0
    v_des = -k_p * (x - x_target) - k_d * v

    # Compute distance-based braking bound (non-negative)
    v_brake = np.sqrt(max(0.0, 2.0 * a_brake * d))

    # Safe velocity bound
    v_safe_bound = min(v_force, v_brake, v_max)

    # Apply safety filter only when moving toward wall (v_des < 0)
    if v_des < -v_safe_bound:
        v_safe_cmd = -v_safe_bound
    else:
        v_safe_cmd = v_des

    # Simple velocity-tracking control (u = m k_v (v_safe_cmd - v))
    u = m * k_v * (v_safe_cmd - v)

    # Integrate dynamics
    v = v + (h / m) * u
    x = x + h * v

    # Contact force model (penetration x < 0)
    F = 0.0
    if x < 0.0:
        F = -k_env * x

    xs[k] = x
    vs[k] = v
    Fs[k] = F

# Post-simulation checks
print("Max penetration (m):", np.min(xs))
print("Max contact force (N):", np.max(np.abs(Fs)))
print("Theoretical safe bound F_safe (N):", F_safe)

plt.figure()
plt.subplot(3,1,1)
plt.plot(ts, xs)
plt.ylabel("x [m]")

plt.subplot(3,1,2)
plt.plot(ts, vs)
plt.ylabel("v [m/s]")

plt.subplot(3,1,3)
plt.plot(ts, Fs)
plt.ylabel("F [N]")
plt.xlabel("t [s]")
plt.tight_layout()
plt.show()
      

The logged maximum force should stay below or near \( F_{\text{safe}} \) (up to discretization effects), illustrating the correctness of the safety filter.

The following standalone C++ program implements the same 1-DOF lab. It prints basic statistics that can be compared with the theoretical force bound. In a real robot, the same logic would be placed inside the real-time control loop, with sensor readings and torque commands.

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

int main() {
    // Physical parameters
    const double m = 1.0;
    const double k_env = 5000.0;
    const double F_safe = 80.0;
    const double a_brake = 30.0;
    const double v_max = 1.0;

    // Derived
    const double v_force = F_safe / std::sqrt(m * k_env);

    // Simulation
    const double h = 0.0005;
    const double T = 1.0;
    const int N = static_cast<int>(T / h);

    // Controller parameters
    const double x_target = 0.0;
    const double k_p = 50.0;
    const double k_d = 5.0;
    const double k_v = 200.0;

    double x = 0.25;
    double v = 0.0;

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

    for (int k = 0; k < N; ++k) {
        double d = x;
        double v_des = -k_p * (x - x_target) - k_d * v;

        // Distance-based bound
        double v_brake = std::sqrt(std::max(0.0, 2.0 * a_brake * d));
        double v_safe_bound = std::min(v_force, std::min(v_brake, v_max));

        double v_safe_cmd = v_des;
        if (v_des < -v_safe_bound) {
            v_safe_cmd = -v_safe_bound;
        }

        // Velocity-tracking torque
        double u = m * k_v * (v_safe_cmd - v);

        // Integrate
        v += (h / m) * u;
        x += h * v;

        // Contact force
        double F = 0.0;
        if (x < 0.0) {
            F = -k_env * x;
        }

        xs[k] = x;
        vs[k] = v;
        Fs[k] = F;
    }

    double max_penetration = xs[0];
    double max_force = std::fabs(Fs[0]);
    for (int k = 0; k < N; ++k) {
        if (xs[k] < max_penetration) max_penetration = xs[k];
        if (std::fabs(Fs[k]) > max_force) max_force = std::fabs(Fs[k]);
    }

    std::cout << "Max penetration (m): " << max_penetration << std::endl;
    std::cout << "Max contact force (N): " << max_force << std::endl;
    std::cout << "Theoretical bound F_safe (N): " << F_safe << std::endl;

    return 0;
}
      

This C++ core can be integrated with a robotics framework such as ROS2, with x and v replaced by measured end-effector distance and normal velocity, and u mapped to joint torques via an inverse-dynamics controller.

In Java, we can implement the same discrete-time loop as part of a simulation class. This is suitable for lab exercises using Java-based simulation environments or middleware.

public class ContactSafeLab {

    public static void main(String[] args) {
        double m = 1.0;
        double kEnv = 5000.0;
        double fSafe = 80.0;
        double aBrake = 30.0;
        double vMax = 1.0;

        double vForce = fSafe / Math.sqrt(m * kEnv);

        double h = 0.0005;
        double T = 1.0;
        int N = (int) (T / h);

        double xTarget = 0.0;
        double kP = 50.0;
        double kD = 5.0;
        double kV = 200.0;

        double x = 0.25;
        double v = 0.0;

        double maxPenetration = x;
        double maxForce = 0.0;

        for (int k = 0; k < N; k++) {
            double d = x;
            double vDes = -kP * (x - xTarget) - kD * v;

            double vBrake = Math.sqrt(Math.max(0.0, 2.0 * aBrake * d));
            double vSafeBound = Math.min(vForce, Math.min(vBrake, vMax));

            double vSafeCmd = vDes;
            if (vDes < -vSafeBound) {
                vSafeCmd = -vSafeBound;
            }

            double u = m * kV * (vSafeCmd - v);

            v = v + (h / m) * u;
            x = x + h * v;

            double F = 0.0;
            if (x < 0.0) {
                F = -kEnv * x;
            }

            if (x < maxPenetration) {
                maxPenetration = x;
            }
            if (Math.abs(F) > maxForce) {
                maxForce = Math.abs(F);
            }
        }

        System.out.println("Max penetration (m): " + maxPenetration);
        System.out.println("Max contact force (N): " + maxForce);
        System.out.println("Theoretical bound F_safe (N): " + fSafe);
    }
}
      

The lab exercise can invite students to vary kEnv, fSafe, and aBrake, and observe their effect on the resulting penetration and contact force.

In MATLAB, we can script the simulation or implement the logic as a Simulink block diagram. The code below mirrors the previous examples and is easily ported to Simulink using a MATLAB Function block.

% Parameters
m      = 1.0;
k_env  = 5000.0;
F_safe = 80.0;
a_brake = 30.0;
v_max  = 1.0;

v_force = F_safe / sqrt(m * k_env);

h = 5e-4;
T = 1.0;
N = floor(T / h);

x_target = 0.0;
k_p = 50.0;
k_d = 5.0;
k_v = 200.0;

x = 0.25;
v = 0.0;

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

for k = 1:N
    d = x;
    v_des = -k_p * (x - x_target) - k_d * v;

    v_brake = sqrt(max(0.0, 2.0 * a_brake * d));
    v_safe_bound = min([v_force, v_brake, v_max]);

    if v_des < -v_safe_bound
        v_safe_cmd = -v_safe_bound;
    else
        v_safe_cmd = v_des;
    end

    u = m * k_v * (v_safe_cmd - v);

    v = v + (h / m) * u;
    x = x + h * v;

    F = 0.0;
    if x < 0.0
        F = -k_env * x;
    end

    xs(k) = x;
    vs(k) = v;
    Fs(k) = F;
end

fprintf("Max penetration (m): %g\n", min(xs));
fprintf("Max contact force (N): %g\n", max(abs(Fs)));
fprintf("Theoretical bound F_safe (N): %g\n", F_safe);

figure;
subplot(3,1,1); plot(ts, xs); ylabel('x [m]');
subplot(3,1,2); plot(ts, vs); ylabel('v [m/s]');
subplot(3,1,3); plot(ts, Fs); ylabel('F [N]'); xlabel('t [s]');
      

In Simulink, one can implement the same logic using basic blocks: integrators for \( x \) and \( v \), a MATLAB Function block for the safety filter, and a saturation block to visualize the velocity bound \( v_{\text{safe}}(d) \).

Mathematica is convenient for symbolic manipulation and numerical simulation. The following code uses a simple loop with Table to reproduce the 1-DOF lab.

m      = 1.0;
kEnv   = 5000.0;
fSafe  = 80.0;
aBrake = 30.0;
vMax   = 1.0;

vForce = fSafe/Sqrt[m kEnv];

h = 0.0005;
tFinal = 1.0;
nSteps = Floor[tFinal/h];

xTarget = 0.0;
kP = 50.0;
kD = 5.0;
kV = 200.0;

x = 0.25;
v = 0.0;

data = Table[
   Module[{d, vDes, vBrake, vSafeBound, vSafeCmd, u, F},
    d = x;
    vDes = -kP (x - xTarget) - kD v;
    vBrake = Sqrt[Max[0.0, 2.0 aBrake d]];
    vSafeBound = Min[vForce, vBrake, vMax];

    vSafeCmd = If[vDes < -vSafeBound, -vSafeBound, vDes];

    u = m kV (vSafeCmd - v);

    v = v + (h/m) u;
    x = x + h v;

    F = If[x < 0.0, -kEnv x, 0.0];

    {k h, x, v, F}
   ],
   {k, 0, nSteps - 1}
];

ts = data[[All,1]];
xs = data[[All,2]];
vs = data[[All,3]];
Fs = data[[All,4]];

Print["Max penetration (m): ", Min[xs]];
Print["Max contact force (N): ", Max[Abs[Fs]]];
Print["Theoretical bound F_safe (N): ", fSafe];

ListLinePlot[
  {Transpose[{ts, xs}], Transpose[{ts, vs}], Transpose[{ts, Fs}]},
  PlotLegends -> {"x(t)", "v(t)", "F(t)"}
]
      

Mathematica can also be used to symbolically verify the energy-based derivations for the safe velocity bound and to explore parameter sensitivities analytically.

In this lab we designed and implemented a simple contact-safe controller based on a distance-dependent velocity bound, derived from energy and braking arguments. We generalized the 1-DOF analysis to manipulators via effective mass along the contact normal, and we interpreted the safety filter as a projection of nominal commands onto a velocity half-space. Multi-language implementations illustrated how the same theoretical idea can be realized in Python, C++, Java, MATLAB/Simulink, and Mathematica, preparing the ground for more advanced interaction and force control schemes in the next chapter.