We assume a rigid serial manipulator with known kinematics and dynamics (as in a standard robotics course), equipped with a wrist force/torque sensor and position encoders. The lab has two control-oriented goals:

• Surface following: Track a curve on an (approximately) planar surface while maintaining a desired normal force.
• Peg-in-hole: Insert a cylindrical peg into a hole by exploiting compliance in position/orientation rather than perfect geometric alignment.

Let \( \mathbf{q} \in \mathbb{R}^n \) denote joint angles and \( \mathbf{x} \in \mathbb{R}^3 \) the end-effector position (orientation control is analogous but omitted in some derivations to keep notation light). The direct kinematics and Jacobian are known:

\[ \mathbf{x} = \mathbf{f}(\mathbf{q}), \qquad \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q}) \dot{\mathbf{q}}, \]

and the manipulator dynamics (from a robotics course) are

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{S}^\top \mathbf{u} + \mathbf{J}(\mathbf{q})^\top \mathbf{f}_{\text{ext}}, \]

where \( \mathbf{u} \) is the vector of actuator inputs (joint torques), \( \mathbf{S} \) is the actuator selection matrix, and \( \mathbf{f}_{\text{ext}} \) is the Cartesian wrench at the end-effector measured by the force sensor.

The environment surface is modeled as a plane

\[ \Sigma = \left\{ \mathbf{x} \in \mathbb{R}^3 \,\middle|\, \mathbf{n}^\top (\mathbf{x} - \mathbf{x}_0) = 0 \right\}, \]

where \( \mathbf{n} \in \mathbb{R}^3 \) is a unit normal and \( \mathbf{x}_0 \) is a known point on the surface. The signed distance to the plane is

\[ s(\mathbf{x}) = \mathbf{n}^\top (\mathbf{x} - \mathbf{x}_0). \]

In the lab, \( s(\mathbf{x}) \) will be indirectly observed through the measured normal force when the manipulator is in contact with the surface.

For compliant interaction, it is useful to decompose motion and force into normal and tangential components with respect to the surface. Let \( \mathbf{n} \) be the surface normal and choose two orthonormal tangent vectors \( \mathbf{t}_1, \mathbf{t}_2 \) spanning the tangent plane. Define

\[ \mathbf{T}_t = \begin{bmatrix} \mathbf{t}_1 & \mathbf{t}_2 \end{bmatrix} \in \mathbb{R}^{3 \times 2}, \qquad \mathbf{T}_n = \mathbf{n} \in \mathbb{R}^{3 \times 1}. \]

For a Cartesian position error \( \mathbf{e}_x = \mathbf{x} - \mathbf{x}_d \) with desired trajectory \( \mathbf{x}_d(t) \),

\[ \mathbf{e}_t = \mathbf{T}_t^\top \mathbf{e}_x \in \mathbb{R}^2, \qquad e_n = \mathbf{n}^\top \mathbf{e}_x \in \mathbb{R}. \]

Similarly decompose the external force:

\[ \mathbf{f}_t = \mathbf{T}_t^\top \mathbf{f}_{\text{ext}}, \qquad f_n = \mathbf{n}^\top \mathbf{f}_{\text{ext}}. \]

A simple linear environment model in the normal direction is

\[ f_n = -k_e \, s(\mathbf{x}) - d_e \, \dot{s}(\mathbf{x}), \]

where \( k_e \) is the environment stiffness and \( d_e \) an environment damping term. For the lab, we treat \( k_e, d_e \) as unknown but constant, and their effect is observed via the measured force.

A task-space impedance controller specifies a desired dynamic relation between motion error and external force. In full Cartesian form:

\[ \mathbf{M}_c \ddot{\mathbf{e}}_x + \mathbf{D}_c \dot{\mathbf{e}}_x + \mathbf{K}_c \mathbf{e}_x = \mathbf{f}_{\text{ext}} - \mathbf{f}_d, \]

where \( \mathbf{M}_c, \mathbf{D}_c, \mathbf{K}_c \) are positive-definite shaping matrices and \( \mathbf{f}_d \) is the desired wrench. For planar surface following, it is convenient to choose the impedance to be diagonal in normal/tangential coordinates:

\[ \mathbf{K}_c = \mathbf{T} \begin{bmatrix} k_t \mathbf{I}_2 & 0 \\ 0 & k_n \end{bmatrix} \mathbf{T}^\top, \quad \mathbf{D}_c = \mathbf{T} \begin{bmatrix} d_t \mathbf{I}_2 & 0 \\ 0 & d_n \end{bmatrix} \mathbf{T}^\top, \]

where \( \mathbf{T} = [\mathbf{T}_t \ \mathbf{T}_n] \in \mathbb{R}^{3 \times 3} \) is orthonormal, \( k_t \) and \( d_t \) shape tangential tracking, and \( k_n, d_n \) shape compliance in the normal direction.

In the simplest quasi-static regime, we neglect the impedance inertia term and approximate

\[ \mathbf{D}_c \dot{\mathbf{e}}_x + \mathbf{K}_c \mathbf{e}_x \approx \mathbf{f}_{\text{ext}} - \mathbf{f}_d. \]

In the lab, one typical choice is:

• Large tangential stiffness: \( k_t \) large to accurately track the desired path on the surface.
• Moderate normal stiffness: \( k_n \) chosen such that the steady-state normal force error is small and the closed-loop system remains compliant.

Given a desired Cartesian wrench \( \mathbf{f}_c \) from the impedance relation, the joint torques are synthesized as

\[ \mathbf{u} = \mathbf{u}_{\text{dyn}}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}_r) + \mathbf{J}(\mathbf{q})^\top \mathbf{f}_c, \]

where \( \mathbf{u}_{\text{dyn}} \) is the inner joint-space torque controller (e.g. computed-torque from Chapter 3) tracking a reference joint acceleration \( \ddot{\mathbf{q}}_r \) that realizes the desired Cartesian motion.

Peg-in-hole insertion is a classic benchmark for hybrid position/force control. In a simplified planar model, the task variables are:

• \( y \): insertion direction along the hole axis (position-controlled).
• \( x \): lateral misalignment (force/compliance-controlled).
• \( \theta \): rotational misalignment of the peg.

Let \( \mathbf{z} = [x \ y \ \theta]^\top \) and define a selection matrix for position-controlled coordinates:

\[ \mathbf{S}_p = \begin{bmatrix} 0 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 0 \end{bmatrix}, \qquad \mathbf{S}_f = \mathbf{I}_3 - \mathbf{S}_p. \]

Hybrid position/force control specifies position control in the \strong>\( y \)-direction and force/compliance in \( x, \theta \):

\[ \mathbf{f}_c = \mathbf{S}_p \left( \mathbf{K}_p (\mathbf{z}_d - \mathbf{z}) + \mathbf{D}_p (\dot{\mathbf{z}}_d - \dot{\mathbf{z}}) \right) + \mathbf{S}_f \left( \mathbf{K}_f (\mathbf{f}_d - \mathbf{f}_{\text{ext}}) \right), \]

where \( \mathbf{K}_p, \mathbf{D}_p \) are position gains and \( \mathbf{K}_f \) is a force gain matrix on the lateral and rotational forces. The corresponding joint torques are again

\[ \mathbf{u} = \mathbf{u}_{\text{dyn}}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}_r) + \mathbf{J}(\mathbf{q})^\top \mathbf{f}_c. \]

Practically, the peg-in-hole task is executed in phases:

flowchart TD
  A["Approach (position control to pre-contact pose)"]
    --> B["Search / Surface following along hole top"]
    --> C["Alignment (hybrid control on x, theta)"]
    --> D["Insertion (position on y, force limits)"]
    --> E["Stop criterion (depth reached or force limit)"]
        

The lab will implement this state machine with different gain sets in each phase, while keeping the underlying hybrid/impedance structure consistent.

To design and test controllers safely, it is common to start from a 1-DOF normal-direction model. Let \( x \) be the end-effector displacement along the surface normal (with the surface at \( x = 0 \) and contact when \( x \le 0 \)). Consider the mass \( m \) controlled by a Cartesian force input \( u \) and subject to an environment spring:

\[ m \ddot{x} = u + f_e, \qquad f_e = \begin{cases} 0, & x > 0, \\ -k_e x, & x \le 0. \end{cases} \]

An impedance controller in continuous time for a desired contact point \( x_d = 0 \) and desired normal force \( f_{n,d} > 0 \) can be written as

\[ m_c \ddot{e} + d_c \dot{e} + k_c e = f_e - f_{n,d}, \qquad e = x - x_d. \]

Solving for the required command force \( u = f_c \) gives

\[ f_c = m_c \ddot{e} + d_c \dot{e} + k_c e - f_e + f_{n,d}. \]

In discrete time with sampling period \( T_s \), approximate \( \dot{e} \) and \( \ddot{e} \) by finite differences:

\[ \dot{e}[k] \approx \frac{e[k] - e[k-1]}{T_s}, \qquad \ddot{e}[k] \approx \frac{e[k] - 2 e[k-1] + e[k-2]}{T_s^2}. \]

Substituting into the impedance law yields the discrete-time control:

\[ f_c[k] = m_c \frac{e[k] - 2 e[k-1] + e[k-2]}{T_s^2} + d_c \frac{e[k] - e[k-1]}{T_s} + k_c e[k] - f_e[k] + f_{n,d}. \]

In the lab implementation (Python, C++, Java, MATLAB, Mathematica), the same discrete-time relation is used, with saturation and low-pass filtering added as needed to handle sensor noise and actuator limits.

flowchart TD
  S["Sensor (x, f_e)"] --> E["Error e = x - x_d"]
  E --> DFD["Compute e[k], e[k-1], e[k-2]"]
  DFD --> IMP["Discrete impedance law f_c[k]"]
  IMP --> SAT["Saturation / safety checks"]
  SAT --> ACT["Actuator / inner joint controller"]
  ACT --> PLANT["1-DOF plant m x_ddot = u + f_e"]
  PLANT --> S
        

The following Python script simulates a 1-DOF mass contacting a rigid surface at x = 0 using the discrete-time impedance law derived above. The goal is to maintain a desired normal force while commanding the mass to move toward the surface.

import numpy as np
import matplotlib.pyplot as plt

# Physical parameters
m = 1.0          # kg
k_env = 5000.0   # N/m environment stiffness
x0 = 0.05        # initial position (m), positive = above surface

# Impedance parameters
m_c = 0.5
k_c = 2000.0
# critical damping approximation: d_c = 2 * sqrt(m_c * k_c)
d_c = 2.0 * np.sqrt(m_c * k_c)

# Desired normal force (positive compressive)
f_n_des = 20.0   # N
x_d = 0.0        # desired contact position at the surface

# Simulation parameters
T_s = 0.0005     # sampling period (s)
T_end = 0.8      # total simulation time (s)
N = int(T_end / T_s)

# State vectors
x = np.zeros(N)
v = np.zeros(N)
f_env = np.zeros(N)
f_cmd = np.zeros(N)

# Error history for discrete derivatives
e = np.zeros(N)

# Initial conditions
x[0] = x0
v[0] = 0.0
e_prev1 = 0.0
e_prev2 = 0.0

for k in range(2, N):
    # Environment force (unilateral spring)
    if x[k-1] > 0.0:
        f_env[k-1] = 0.0
    else:
        f_env[k-1] = -k_env * x[k-1]

    # Position error
    e[k] = x[k-1] - x_d

    # Discrete-time derivatives
    de = (e[k] - e_prev1) / T_s
    dde = (e[k] - 2.0 * e_prev1 + e_prev2) / (T_s ** 2)

    # Impedance law (control force)
    f_c = m_c * dde + d_c * de + k_c * e[k] - f_env[k-1] + f_n_des

    # Simple saturation (safety)
    f_c = np.clip(f_c, -200.0, 200.0)

    f_cmd[k] = f_c

    # Plant integration (semi-implicit Euler)
    a = (f_c + f_env[k-1]) / m
    v[k] = v[k-1] + T_s * a
    x[k] = x[k-1] + T_s * v[k]

    # Shift error history
    e_prev2 = e_prev1
    e_prev1 = e[k]

# Plot results
t = np.arange(N) * T_s

plt.figure()
plt.subplot(2, 1, 1)
plt.plot(t, x, label="x (m)")
plt.axhline(0.0, linestyle="--", label="surface")
plt.ylabel("Position (m)")
plt.legend()

plt.subplot(2, 1, 2)
plt.plot(t, f_env, label="Contact force (N)")
plt.axhline(f_n_des, linestyle="--", label="desired force")
plt.xlabel("Time (s)")
plt.ylabel("Force (N)")
plt.legend()
plt.tight_layout()
plt.show()
      

The lab exercise is to tune k_c, d_c, and m_c to achieve well-damped transients and small steady-state force error without oscillations or chattering when contact is engaged.

The following C++ skeleton illustrates a Cartesian impedance controller for surface following in 3D using an existing kinematics/dynamics stack (e.g., your own implementation or a robotics library). The code focuses on control computations and omits low-level real-time details.

#include <Eigen/Dense>

class CartesianImpedanceController {
public:
    CartesianImpedanceController()
    {
        // Tangential and normal gains
        k_t_ = 2000.0;
        d_t_ = 40.0;
        k_n_ = 800.0;
        d_n_ = 20.0;
    }

    // Inputs:
    // x: current Cartesian position (3x1)
    // xd: desired Cartesian position (3x1)
    // dx: current Cartesian velocity (3x1)
    // dxd: desired Cartesian velocity (3x1)
    // f_ext: measured external force (3x1)
    // n: unit normal of the contact surface (3x1)
    // J: Jacobian matrix at current configuration (6xn)
    //
    // Output:
    // tau_cmd: desired joint torques (nx1)
    Eigen::VectorXd computeTorque(
        const Eigen::Vector3d &x,
        const Eigen::Vector3d &xd,
        const Eigen::Vector3d &dx,
        const Eigen::Vector3d &dxd,
        const Eigen::Vector3d &f_ext,
        const Eigen::Vector3d &n,
        const Eigen::MatrixXd &J,
        const Eigen::VectorXd &tau_dyn)
    {
        // Build tangent basis T_t from n (simple heuristic)
        Eigen::Vector3d t1;
        if (std::fabs(n.z()) > 0.5) {
            t1 = Eigen::Vector3d(1.0, 0.0, -n.x() / n.z());
        } else {
            t1 = Eigen::Vector3d(0.0, 1.0, -n.y() / n.z());
        }
        t1.normalize();
        Eigen::Vector3d t2 = n.cross(t1);
        t2.normalize();

        Eigen::Matrix<double, 3, 2> T_t;
        T_t.col(0) = t1;
        T_t.col(1) = t2;

        // Errors
        Eigen::Vector3d e = x - xd;
        Eigen::Vector3d de = dx - dxd;

        double e_n = n.dot(e);
        double de_n = n.dot(de);
        Eigen::Vector2d e_t = T_t.transpose() * e;
        Eigen::Vector2d de_t = T_t.transpose() * de;

        // Project measured force
        double f_n = n.dot(f_ext);
        Eigen::Vector2d f_t = T_t.transpose() * f_ext;

        // Desired normal force (lab parameter)
        double f_n_des = 20.0;

        // Tangential impedance (position tracking on the surface)
        Eigen::Vector2d f_t_cmd = -k_t_ * e_t - d_t_ * de_t;

        // Normal impedance around desired force
        double e_f = f_n - f_n_des;
        double f_n_cmd = -k_n_ * e_n - d_n_ * de_n - alpha_f_ * e_f;

        // Assemble Cartesian force vector
        Eigen::Vector3d f_c =
            T_t * f_t_cmd + n * f_n_cmd;

        // Joint torques: tau = tau_dyn + J.transpose() * f_c (force part only)
        Eigen::VectorXd tau_cmd = tau_dyn;
        tau_cmd.head(J.cols()) += J.topRows(3).transpose() * f_c;
        return tau_cmd;
    }

    void setGains(double k_t, double d_t, double k_n, double d_n, double alpha_f)
    {
        k_t_ = k_t;
        d_t_ = d_t;
        k_n_ = k_n;
        d_n_ = d_n;
        alpha_f_ = alpha_f;
    }

private:
    double k_t_, d_t_;
    double k_n_, d_n_;
    double alpha_f_ = 5.0;  // force feedback gain
};
      

In a full implementation, tau_dyn is provided by an inner model-based torque controller, and the Jacobian is updated each cycle from forward kinematics. The lab experiment is to validate surface following while varying normal/tangential gains.

The Java snippet below shows a high-level hybrid position/force controller for the peg-in-hole task in the simplified x, y, theta coordinates. The actual mapping between these task variables and joint torques is assumed to be provided by a lower-layer library.

public class HybridPegInHoleController {

    // Position gains (for y)
    private double kpy = 2000.0;
    private double kdy = 60.0;

    // Force gains (for x and theta)
    private double kfx = 0.3;
    private double kftheta = 0.2;

    // Desired reference
    private double yDes;
    private double fnDes;      // desired normal force along x
    private double mDes;       // desired moment around theta (often 0)

    public HybridPegInHoleController(double yDes, double fnDes) {
        this.yDes = yDes;
        this.fnDes = fnDes;
        this.mDes = 0.0;
    }

    // Input state and sensor readings:
    // z = [x, y, theta]
    // zd = [xd, yd, thetad]
    // dz = time derivative of z
    // fext = [Fx, Fy, Mz] Cartesian wrench in local peg frame
    //
    // Output: desired Cartesian wrench [Fx_cmd, Fy_cmd, Mz_cmd]
    public double[] computeWrench(double[] z, double[] zd, double[] dz, double[] fext) {
        double x = z[0];
        double y = z[1];
        double theta = z[2];

        double xd = zd[0];
        double yd = zd[1];
        double thetad = zd[2];

        double dx = dz[0];
        double dy = dz[1];
        double dtheta = dz[2];

        double fx = fext[0];
        double fy = fext[1];
        double mz = fext[2];

        // Position error in y
        double ey = y - yd;
        double dey = dy - 0.0;

        // Position control on y (insertion direction)
        double Fy_cmd = -kpy * ey - kdy * dey;

        // Force error in x
        double efx = fx - fnDes;
        // Force error in moment (around theta)
        double efm = mz - mDes;

        // Force control on x and theta
        double Fx_cmd = -kfx * efx;
        double Mz_cmd = -kftheta * efm;

        return new double[]{Fx_cmd, Fy_cmd, Mz_cmd};
    }

    public void setPositionGains(double kpy, double kdy) {
        this.kpy = kpy;
        this.kdy = kdy;
    }

    public void setForceGains(double kfx, double kftheta) {
        this.kfx = kfx;
        this.kftheta = kftheta;
    }
}
      

The lab integration step is to connect computeWrench to the robot interface (or simulator) that converts a desired Cartesian wrench into joint torques using the Jacobian transpose and a dynamic compensation layer.

The following MATLAB script simulates the 1D impedance-controlled contact as in the Python example. It is easily converted into a Simulink model by replacing the loop with integrator and gain blocks.

m = 1.0;
k_env = 5000.0;
x0 = 0.05;

m_c = 0.5;
k_c = 2000.0;
d_c = 2.0 * sqrt(m_c * k_c);

f_n_des = 20.0;
x_d = 0.0;

T_s = 5e-4;
T_end = 0.8;
N = floor(T_end / T_s);

x = zeros(1, N);
v = zeros(1, N);
f_env = zeros(1, N);
f_cmd = zeros(1, N);
e = zeros(1, N);

x(1) = x0;
v(1) = 0.0;
e_prev1 = 0.0;
e_prev2 = 0.0;

for k = 3:N
    if x(k-1) > 0.0
        f_env(k-1) = 0.0;
    else
        f_env(k-1) = -k_env * x(k-1);
    end

    e(k) = x(k-1) - x_d;

    de = (e(k) - e_prev1) / T_s;
    dde = (e(k) - 2.0 * e_prev1 + e_prev2) / (T_s^2);

    f_c = m_c * dde + d_c * de + k_c * e(k) - f_env(k-1) + f_n_des;
    f_c = max(min(f_c, 200.0), -200.0);

    f_cmd(k) = f_c;

    a = (f_c + f_env(k-1)) / m;
    v(k) = v(k-1) + T_s * a;
    x(k) = x(k-1) + T_s * v(k);

    e_prev2 = e_prev1;
    e_prev1 = e(k);
end

t = (0:N-1) * T_s;
figure;
subplot(2,1,1);
plot(t, x); hold on; yline(0.0, "--");
ylabel("x (m)");
subplot(2,1,2);
plot(t, f_env); hold on; yline(f_n_des, "--");
ylabel("Force (N)"); xlabel("Time (s)");
      

A Simulink model can be built by:

• Using an integrator chain for x and v (plant dynamics).
• Implementing the impedance law in a MATLAB Function block using the same discrete-time formulas.
• Adding saturation blocks and scopes to visualize trajectories and contact forces.

Mathematica facilitates symbolic manipulation and numerical simulation of the impedance-controlled contact. The following notebook-style code simulates the 1D system with a continuous-time impedance law.

(* Physical parameters *)
m = 1.0;
kEnv = 5000.0;

(* Impedance parameters *)
mC = 0.5;
kC = 2000.0;
dC = 2.0*Sqrt[mC*kC];

fNDes = 20.0;
xDes = 0.0;

(* Environment force: unilateral spring *)
fEnv[x_] := Piecewise[{ {0.0, x > 0.0}, {-kEnv*x, x <= 0.0} }];

(* Impedance law:
   mC * x_ddot + dC * x_dot + kC * (x - xDes) == fEnv[x] - fNDes
   Solve for control force u so that m * x_ddot == u + fEnv[x]
*)

eqs = {
  x''[t] == (1.0/m)*
    (mC*(x''[t]) + dC*(x'[t]) + kC*(x[t] - xDes) - fEnv[x[t]] + fNDes
     + fEnv[x[t]])
};

ic = {x[0] == 0.05, x'[0] == 0.0};

sol = NDSolve[
  Flatten[{eqs, ic}],
  x,
  {t, 0, 0.8},
  Method -> {"StiffnessSwitching"}
];

Plot[
  Evaluate[x[t] /. sol],
  {t, 0, 0.8},
  AxesLabel -> {"t", "x(t)"},
  PlotLegends -> {"position"}
]

Plot[
  Evaluate[fEnv[x[t]] /. sol],
  {t, 0, 0.8},
  AxesLabel -> {"t", "force"},
  PlotLegends -> {"contact force"}
]
      

Variants of this model can be used to analyze the effect of changing \( m_c, d_c, k_c \) on transient response and to symbolically verify stability using Lyapunov energy functions.

In this lab lesson we instantiated the abstract concepts of force and compliant interaction control into two canonical tasks: surface following and peg-in-hole insertion. We constructed normal/tangential decompositions, derived impedance and hybrid position/force laws, and obtained discrete-time implementations suitable for real-time control. Multi-language implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica illustrate how the same underlying impedance structure is realized across platforms. These concrete implementations prepare you for more advanced chapters on robust, adaptive, and optimization-based contact control.