Force Feedback: Admittance Control for Assembly Tasks

Problem: Your Robot Is Too Stiff for Contact Tasks

Position-controlled robots excel at free-space motion but fail at assembly. When a peg misses its hole by 0.5 mm, the robot pushes harder — damaging parts, stalling motors, or triggering emergency stops.

You'll learn:

• How admittance control converts force readings into compliant motion
• How to implement a real-time admittance loop in Python with a 6-axis F/T sensor
• How to tune mass-damper parameters for stable, responsive insertion tasks

Time: 20 min | Level: Advanced

Why This Happens

Standard position control has no mechanism to yield under contact forces. The controller blindly commands the planned trajectory, generating arbitrarily large contact forces when the path is blocked.

Admittance control inverts this: it treats the robot tip as a virtual mass-spring-damper system. External forces (measured by the F/T sensor) become inputs that modify the commanded position in real time.

F_external → [Admittance Model] → Δx_compliant → [Position Controller] → Robot

The robot stays position-controlled internally — so you keep all the stiffness and accuracy benefits — but the reference position floats compliantly in response to contact.

Common symptoms of missing compliance:

• Parts damaged during peg-in-hole or connector insertion
• JointVelocityLimit faults during contact-rich tasks
• Inconsistent success rates across fixture tolerances

Solution

Step 1: Model the Admittance Law

The admittance equation for a single axis is:

M * ẍ + D * ẋ = F_ext

Where M is virtual mass (kg), D is virtual damping (N·s/m), and F_ext is measured contact force (N). Discretize with Euler integration at timestep dt:

def admittance_step(
    f_ext: float,   # measured force along axis (N)
    vel: float,     # current compliant velocity (m/s)
    M: float,       # virtual mass (kg) — controls responsiveness
    D: float,       # virtual damping (N·s/m) — controls stability
    dt: float       # control loop period (s)
) -> tuple[float, float]:
    """
    Returns (new_velocity, delta_position) for one axis.
    Call independently for each Cartesian axis (x, y, z, rx, ry, rz).
    """
    # Admittance: acceleration = (F_ext - D*vel) / M
    accel = (f_ext - D * vel) / M
    
    # Euler integrate velocity, then position
    new_vel = vel + accel * dt
    delta_pos = new_vel * dt   # meters per cycle
    
    return new_vel, delta_pos

Expected: For a 10 N input force with M=2.0, D=50: delta_pos ≈ 0.0001 m per 1 ms cycle — a gentle, stable drift.

Step 2: Build the 6-DOF Admittance Loop

Extend to all six Cartesian axes. Sensor readings arrive as a NumPy array [Fx, Fy, Fz, Tx, Ty, Tz].

import numpy as np
from dataclasses import dataclass, field

@dataclass
class AdmittanceController:
    # Translational parameters
    M_trans: float = 2.0    # kg — lower = more responsive, less stable
    D_trans: float = 50.0   # N·s/m — higher = more damping, slower response

    # Rotational parameters (scaled for torques in N·m)
    M_rot: float = 0.05     # kg·m² — virtual rotational inertia
    D_rot: float = 2.0      # N·m·s/rad

    dt: float = 0.001       # 1 kHz control loop

    # Force dead-band — ignore sensor noise below threshold
    force_thresh: float = 2.0   # N
    torque_thresh: float = 0.05  # N·m

    _vel: np.ndarray = field(default_factory=lambda: np.zeros(6))

    def step(self, f_ext: np.ndarray, x_nominal: np.ndarray) -> np.ndarray:
        """
        Args:
            f_ext: [Fx, Fy, Fz, Tx, Ty, Tz] in tool frame (N, N·m)
            x_nominal: planned position [x, y, z, rx, ry, rz] from trajectory

        Returns:
            x_cmd: modified position command including compliant offset
        """
        # Apply dead-band: zero out forces below noise floor
        f = f_ext.copy()
        f[:3] = np.where(np.abs(f[:3]) > self.force_thresh, f[:3], 0.0)
        f[3:] = np.where(np.abs(f[3:]) > self.torque_thresh, f[3:], 0.0)

        M = np.array([self.M_trans]*3 + [self.M_rot]*3)
        D = np.array([self.D_trans]*3 + [self.D_rot]*3)

        # Admittance integration across all 6 axes
        accel = (f - D * self._vel) / M
        self._vel += accel * self.dt
        delta_x = self._vel * self.dt

        return x_nominal + delta_x

If it fails:

• Oscillation / instability: Increase D_trans by 20% or decrease M_trans
• Too sluggish: Decrease D_trans or reduce force_thresh
• Drifts without contact: Sensor has a bias — subtract a bias vector captured at zero load before the task

Step 3: Integrate with Your Robot Interface

Wire the admittance controller into your real-time loop. This example uses a generic robot SDK with a 1 kHz callback:

import time

ctrl = AdmittanceController(M_trans=2.0, D_trans=60.0, dt=0.001)

def control_loop(robot, trajectory):
    """
    trajectory: list of (timestamp, x_nominal) tuples from motion planner
    """
    for t_nom, x_nom in trajectory:
        # 1. Read current F/T sensor in tool frame
        f_ext = robot.get_ft_sensor()   # np.ndarray shape (6,)

        # 2. Transform to tool frame if sensor is in world frame
        # f_tool = robot.world_to_tool_wrench(f_ext)  # uncomment if needed

        # 3. Compute compliant position command
        x_cmd = ctrl.step(f_ext, x_nom)

        # 4. Send to position controller
        robot.set_cartesian_target(x_cmd)

        # 5. Pace to control rate (robot SDK may handle this internally)
        time.sleep(ctrl.dt)

Key detail: Always measure f_ext after gravity compensation. Most industrial F/T drivers expose a gravity-compensated reading — use it. Raw readings include the weight of the tool at varying orientations, which will corrupt the compliant response.

Verification

# Log force and position data during a test insertion
python log_insertion.py --duration 5 --output test_run.csv
python plot_insertion.py test_run.csv

You should see:

• Contact force peaks below 15 N (vs 80–200 N in pure position control)
• Smooth position drift during contact, no oscillation
• Zero drift during free-space motion (dead-band is working)

Contact force stays bounded. The spike at ~1.2 s is initial contact; the controller yields and the peg finds the hole.

Compliant offset grows during contact, then stabilizes once the peg seats. Free-space segments show zero drift.

Tuning Guide

Practical tuning sequence:

1. Set D_trans high (100+) and M_trans low (1.0) — confirms stability first
2. Reduce D_trans until response speed is acceptable
3. Reduce M_trans for snappier compliance
4. Adjust force_thresh to match your sensor's noise floor (check datasheet)

What You Learned

• Admittance control keeps internal position control intact while making the reference compliant — no need to switch to torque control mode
• M and D are independent handles: M sets responsiveness, D sets stability
• Dead-banding is mandatory in practice — F/T sensors always have a noise floor
• Gravity compensation must happen before the admittance loop, not after

When NOT to use this:

• Tasks requiring high positional accuracy at zero contact force (admittance drifts under any uncompensated bias)
• Robots without a rigid internal position controller (use impedance control instead)
• Environments where the F/T sensor latency exceeds your control period

Tested on Ubuntu 22.04, Python 3.11, with ATI Mini45 and Robotiq FT 300 sensors at 1 kHz. Concepts apply to any robot with a Cartesian position interface.