Lab 07-02: Force Control and Compliance

Objectives

By the end of this lab, you will be able to:

Implement impedance control for compliant robot motion
Design force feedback control loops
Execute contact tasks with force regulation
Tune compliance parameters for different tasks

Prerequisites

Completed Lab 07-01 (Basic Grasping)
Understanding of dynamics and control (Module 05)
Familiarity with contact forces in MuJoCo

Materials

Type	Name	Tier	Notes
software	MuJoCo 3.0+	required	Physics simulation with contacts
software	Python 3.10+	required	Programming environment
software	NumPy	required	Linear algebra
software	SciPy	required	Control design
simulation	arm-contact-table.xml	required	Robot arm with force sensing

Background

Why Force Control?

Position control alone is insufficient for:

Assembly tasks (inserting pegs into holes)
Polishing/grinding (maintaining consistent pressure)
Human-robot interaction (safe collaboration)
Unknown environments (exploring with contact)

Impedance Control

Impedance control makes the robot behave like a mass-spring-damper system:

$F = M_d(\ddot{x}_d - \ddot{x}) + B_d(\dot{x}_d - \dot{x}) + K_d(x_d - x)$

Where:

$M_d$ : Desired inertia matrix
$B_d$ : Desired damping matrix
$K_d$ : Desired stiffness matrix
$x_d$ : Desired position trajectory

Force Control

Direct force control regulates contact forces:

$\tau = J^T(q)(F_d + K_f(F_d - F_{measured}))$

Instructions

Step 1: Set Up Force-Sensing Robot

Load a robot with force/torque sensing capability:

import mujoco
import numpy as np
import matplotlib.pyplot as plt

# Load model with force sensing
model = mujoco.MjModel.from_xml_path(
    "textbook/assets/robot-models/mujoco/arm-contact-table.xml"
)
data = mujoco.MjData(model)

# Find the force sensor
sensor_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, "ee_force")
print(f"Force sensor ID: {sensor_id}")

# Get sensor data dimensions
sensor_adr = model.sensor_adr[sensor_id]
sensor_dim = model.sensor_dim[sensor_id]
print(f"Sensor address: {sensor_adr}, dimension: {sensor_dim}")

# Initialize simulation
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)

def get_ee_force():
    """Read end-effector force from sensor."""
    return data.sensordata[sensor_adr:sensor_adr + sensor_dim].copy()

# Test force reading
print(f"Initial EE force: {get_ee_force()}")

Expected: Force sensor identified and returning 3D or 6D force/torque readings.

Step 2: Implement Jacobian Computation

Compute the geometric Jacobian for force transformation:

def compute_jacobian(model, data, body_id):
    """
    Compute the geometric Jacobian for a body.

    Args:
        model: MuJoCo model
        data: MuJoCo data
        body_id: Body ID for Jacobian computation

    Returns:
        J: 6 x nv Jacobian matrix (linear + angular)
    """
    jacp = np.zeros((3, model.nv))  # Position Jacobian
    jacr = np.zeros((3, model.nv))  # Rotation Jacobian

    mujoco.mj_jacBody(model, data, jacp, jacr, body_id)

    # Stack to form full Jacobian
    J = np.vstack([jacp, jacr])

    return J

# Get end-effector body ID
ee_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "end_effector")

# Compute Jacobian
J = compute_jacobian(model, data, ee_body_id)
print(f"Jacobian shape: {J.shape}")
print(f"Jacobian (position rows):\n{J[:3, :]}")

Expected: Jacobian matrix computed with shape (6, nv).

Step 3: Implement Impedance Controller

Create an impedance controller for compliant motion:

class ImpedanceController:
    """Cartesian impedance controller for compliant motion."""

    def __init__(self, model, data, ee_body_id):
        self.model = model
        self.data = data
        self.ee_body_id = ee_body_id

        # Impedance parameters (diagonal for simplicity)
        # Stiffness: N/m for position, Nm/rad for orientation
        self.K = np.diag([500, 500, 500, 50, 50, 50])

        # Damping: Ns/m for position, Nms/rad for orientation
        self.D = np.diag([50, 50, 50, 5, 5, 5])

        # Desired inertia (optional, often set to identity)
        self.M = np.eye(6)

        # Nullspace stiffness for joint position regularization
        self.K_null = 10 * np.eye(model.nv)
        self.q_null = np.zeros(model.nv)  # Nominal joint positions

    def set_impedance(self, K_pos, K_ori, D_pos, D_ori):
        """Set impedance parameters."""
        self.K = np.diag([K_pos, K_pos, K_pos, K_ori, K_ori, K_ori])
        self.D = np.diag([D_pos, D_pos, D_pos, D_ori, D_ori, D_ori])

    def compute_pose_error(self, x_des, quat_des):
        """
        Compute pose error between current and desired.

        Returns:
            error: 6D pose error [position_error; orientation_error]
        """
        # Current pose
        x_cur = self.data.xpos[self.ee_body_id].copy()
        quat_cur = self.data.xquat[self.ee_body_id].copy()

        # Position error
        pos_error = x_des - x_cur

        # Orientation error (using quaternion difference)
        # quat_error = quat_des * quat_cur^(-1)
        quat_cur_inv = quat_cur.copy()
        quat_cur_inv[1:] *= -1  # Conjugate for unit quaternion

        # Quaternion multiplication
        quat_error = np.array([
            quat_des[0]*quat_cur_inv[0] - np.dot(quat_des[1:], quat_cur_inv[1:]),
            quat_des[0]*quat_cur_inv[1] + quat_cur_inv[0]*quat_des[1] +
                np.cross(quat_des[1:], quat_cur_inv[1:])[0],
            quat_des[0]*quat_cur_inv[2] + quat_cur_inv[0]*quat_des[2] +
                np.cross(quat_des[1:], quat_cur_inv[1:])[1],
            quat_des[0]*quat_cur_inv[3] + quat_cur_inv[0]*quat_des[3] +
                np.cross(quat_des[1:], quat_cur_inv[1:])[2],
        ])

        # Convert to axis-angle error (small angle approximation)
        ori_error = 2 * quat_error[1:] * np.sign(quat_error[0])

        return np.concatenate([pos_error, ori_error])

    def compute_velocity_error(self, xdot_des):
        """Compute Cartesian velocity error."""
        J = compute_jacobian(self.model, self.data, self.ee_body_id)
        xdot_cur = J @ self.data.qvel
        return xdot_des - xdot_cur

    def compute_torques(self, x_des, quat_des, xdot_des=None, F_ext=None):
        """
        Compute joint torques for impedance control.

        Args:
            x_des: Desired position [3]
            quat_des: Desired orientation quaternion [4]
            xdot_des: Desired velocity [6] (optional)
            F_ext: External force compensation [6] (optional)

        Returns:
            tau: Joint torques [nv]
        """
        if xdot_des is None:
            xdot_des = np.zeros(6)
        if F_ext is None:
            F_ext = np.zeros(6)

        # Compute errors
        pose_error = self.compute_pose_error(x_des, quat_des)
        vel_error = self.compute_velocity_error(xdot_des)

        # Compute Cartesian force from impedance law
        F_imp = self.K @ pose_error + self.D @ vel_error

        # Add external force compensation
        F_total = F_imp + F_ext

        # Get Jacobian
        J = compute_jacobian(self.model, self.data, self.ee_body_id)

        # Map to joint torques
        tau = J.T @ F_total

        # Add nullspace component for joint regularization
        N = np.eye(self.model.nv) - np.linalg.pinv(J) @ J
        tau_null = N @ (self.K_null @ (self.q_null - self.data.qpos[:self.model.nv]))

        # Add gravity compensation
        gravity_comp = self.data.qfrc_bias.copy()

        return tau + tau_null + gravity_comp

# Create controller
controller = ImpedanceController(model, data, ee_body_id)

# Test: compute torques for current pose (should be near zero)
x_des = data.xpos[ee_body_id].copy()
quat_des = data.xquat[ee_body_id].copy()
tau = controller.compute_torques(x_des, quat_des)
print(f"Torques for holding position: {tau}")

Expected: ImpedanceController computes torques that maintain position when desired equals current.

Step 4: Test Compliant Motion

Execute a motion task with compliance:

def run_impedance_control(controller, x_trajectory, quat_fixed,
                          duration_per_point=1.0, dt=0.002):
    """
    Execute trajectory with impedance control.

    Args:
        controller: ImpedanceController instance
        x_trajectory: List of target positions
        quat_fixed: Fixed orientation quaternion
        duration_per_point: Time to spend at each waypoint
        dt: Simulation timestep

    Returns:
        history: Dictionary of recorded data
    """
    history = {
        'time': [],
        'x': [],
        'x_des': [],
        'force': [],
        'torque': []
    }

    t = 0
    waypoint_idx = 0

    steps_per_waypoint = int(duration_per_point / dt)

```python
```python
    while waypoint_idx < len(x_trajectory):

        x_des = x_trajectory[waypoint_idx]

        for step in range(steps_per_waypoint):
            # Compute control
            tau = controller.compute_torques(x_des, quat_fixed)

            # Apply torques (clamp for safety)
            tau_max = 100
            tau = np.clip(tau, -tau_max, tau_max)
            controller.data.ctrl[:len(tau)] = tau

            # Step simulation
            mujoco.mj_step(controller.model, controller.data)

            # Record data
            t += dt
            history['time'].append(t)
            history['x'].append(controller.data.xpos[controller.ee_body_id].copy())
            history['x_des'].append(x_des.copy())
            history['force'].append(get_ee_force())
            history['torque'].append(tau.copy())

        waypoint_idx += 1

    return history

# Define a simple trajectory
x_start = data.xpos[ee_body_id].copy()
x_trajectory = [
    x_start,
    x_start + np.array([0.1, 0, 0]),    # Move +X
    x_start + np.array([0.1, 0, -0.1]), # Move down (contact)
    x_start + np.array([0.1, 0, -0.05]),# Back up slightly
]
quat_fixed = data.xquat[ee_body_id].copy()

# Reset simulation
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)

# Run with low stiffness (compliant)
controller.set_impedance(K_pos=200, K_ori=20, D_pos=40, D_ori=4)
history_compliant = run_impedance_control(controller, x_trajectory, quat_fixed)

# Reset and run with high stiffness (stiff)
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)
controller.set_impedance(K_pos=2000, K_ori=200, D_pos=100, D_ori=10)
history_stiff = run_impedance_control(controller, x_trajectory, quat_fixed)

print(f"Compliant: max force = {np.max(np.abs(history_compliant['force'])):.2f} N")
print(f"Stiff: max force = {np.max(np.abs(history_stiff['force'])):.2f} N")

Expected: Compliant motion shows lower contact forces than stiff motion.

Step 5: Implement Force Controller

Create a direct force controller for contact tasks:

class ForceController:
    """Hybrid position/force controller."""

    def __init__(self, model, data, ee_body_id, sensor_id):
        self.model = model
        self.data = data
        self.ee_body_id = ee_body_id
        self.sensor_id = sensor_id

        # Selection matrix: 1 for force control, 0 for position control
        # Default: force control in Z, position in X, Y, Rx, Ry, Rz
        self.S_f = np.diag([0, 0, 1, 0, 0, 0])
        self.S_p = np.eye(6) - self.S_f

        # Position controller gains
        self.K_p = np.diag([500, 500, 500, 50, 50, 50])
        self.D_p = np.diag([50, 50, 50, 5, 5, 5])

        # Force controller gains
        self.K_f = 0.001  # Force feedback gain
        self.K_i = 0.0001  # Integral gain

        # Force integral term
        self.force_integral = np.zeros(6)

    def get_measured_force(self):
        """Get force measurement from sensor."""
        sensor_adr = self.model.sensor_adr[self.sensor_id]
        sensor_dim = self.model.sensor_dim[self.sensor_id]

        force = np.zeros(6)
        force[:sensor_dim] = self.data.sensordata[sensor_adr:sensor_adr + sensor_dim]
        return force

    def compute_torques(self, x_des, quat_des, F_des, dt=0.002):
        """
        Compute torques for hybrid position/force control.

        Args:
            x_des: Desired position [3]
            quat_des: Desired orientation quaternion [4]
            F_des: Desired force [6] (only force-controlled axes matter)
            dt: Control timestep for integral

        Returns:
            tau: Joint torques
        """
        # Get Jacobian
        J = compute_jacobian(self.model, self.data, self.ee_body_id)

        # Position control component
        # Compute pose error
        x_cur = self.data.xpos[self.ee_body_id].copy()
        pos_error = x_des - x_cur

        # Simple velocity from finite difference (or use qvel)
        xdot = J @ self.data.qvel

        # Position control force
        pose_error_6d = np.zeros(6)
        pose_error_6d[:3] = pos_error
        F_pos = self.K_p @ pose_error_6d - self.D_p @ xdot

        # Force control component
        F_measured = self.get_measured_force()
        F_error = F_des - F_measured

        # PI force control
        self.force_integral += F_error * dt
        F_force = self.K_f * F_error + self.K_i * self.force_integral

        # Combine using selection matrices
        F_total = self.S_p @ F_pos + self.S_f @ (F_des + F_force)

        # Map to joint torques
        tau = J.T @ F_total

        # Add gravity compensation
        tau += self.data.qfrc_bias.copy()

        return tau

    def set_force_axis(self, axis='z'):
        """Set which axis is force-controlled."""
        self.S_f = np.zeros((6, 6))
```python
```python
        if axis == 'x':

            self.S_f[0, 0] = 1
        elif axis == 'y':
            self.S_f[1, 1] = 1
        elif axis == 'z':
            self.S_f[2, 2] = 1
        self.S_p = np.eye(6) - self.S_f

        # Reset integral
        self.force_integral = np.zeros(6)

# Create force controller
force_ctrl = ForceController(model, data, ee_body_id, sensor_id)

# Test force regulation against surface
def run_force_control(controller, x_des, quat_des, F_des, duration=3.0, dt=0.002):
    """Run force control and record data."""
    history = {
        'time': [],
        'z': [],
        'force_z': [],
        'force_des': []
    }

    steps = int(duration / dt)

    for i in range(steps):
        # Compute and apply control
        tau = controller.compute_torques(x_des, quat_des, F_des, dt)
        tau = np.clip(tau, -100, 100)
        controller.data.ctrl[:len(tau)] = tau

        # Step simulation
        mujoco.mj_step(controller.model, controller.data)

        # Record
        history['time'].append(i * dt)
        history['z'].append(controller.data.xpos[controller.ee_body_id][2])
        history['force_z'].append(controller.get_measured_force()[2])
        history['force_des'].append(F_des[2])

    return history

# Set up: move to contact position first
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)

# Target position and force
x_des = data.xpos[ee_body_id].copy()
x_des[2] -= 0.1  # Move down toward table
quat_des = data.xquat[ee_body_id].copy()
F_des = np.array([0, 0, -10, 0, 0, 0])  # 10N pushing down

# Run force control
force_ctrl.set_force_axis('z')
history = run_force_control(force_ctrl, x_des, quat_des, F_des)

# Analyze results
final_force = np.mean(history['force_z'][-100:])
print(f"Desired force: {F_des[2]} N")
print(f"Achieved force: {final_force:.2f} N")
print(f"Steady-state error: {abs(F_des[2] - final_force):.2f} N")

Expected: Force controller regulates contact force to desired value within reasonable error.

Step 6: Surface Following Task

Combine position and force control for surface following:

def surface_following(controller, x_path, quat_des, F_normal, dt=0.002):
    """
    Follow a path while maintaining contact force.

    Args:
        controller: ForceController
        x_path: List of 2D positions (X, Y) to follow
        quat_des: Fixed orientation
        F_normal: Desired normal force (positive = pushing)
        dt: Control timestep

    Returns:
        history: Recorded trajectory data
    """
    history = {
        'time': [],
        'x': [],
        'y': [],
        'z': [],
        'force_z': []
    }

    t = 0
    F_des = np.array([0, 0, -F_normal, 0, 0, 0])

    for xy in x_path:
        # Target: follow XY from path, Z from force control
        x_des = np.array([xy[0], xy[1], 0.0])  # Z will be adjusted by force control

        # Run for some steps at each waypoint
        for _ in range(50):  # 100ms per point
            tau = controller.compute_torques(x_des, quat_des, F_des, dt)
            tau = np.clip(tau, -100, 100)
            controller.data.ctrl[:len(tau)] = tau

            mujoco.mj_step(controller.model, controller.data)

            t += dt
            ee_pos = controller.data.xpos[controller.ee_body_id]
            history['time'].append(t)
            history['x'].append(ee_pos[0])
            history['y'].append(ee_pos[1])
            history['z'].append(ee_pos[2])
            history['force_z'].append(controller.get_measured_force()[2])

    return history

# Create a circular path
theta = np.linspace(0, 2*np.pi, 50)
radius = 0.05
center = data.xpos[ee_body_id][:2]
x_path = [(center[0] + radius * np.cos(t),
           center[1] + radius * np.sin(t)) for t in theta]

# Reset and run
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)

# First establish contact
force_ctrl = ForceController(model, data, ee_body_id, sensor_id)
force_ctrl.set_force_axis('z')

# Run surface following
history_sf = surface_following(force_ctrl, x_path, quat_des, F_normal=5.0)

# Check force consistency
force_std = np.std(history_sf['force_z'])
force_mean = np.mean(history_sf['force_z'])
print(f"Surface following force: {force_mean:.2f} ± {force_std:.2f} N")

Expected: End-effector follows XY path while maintaining consistent contact force.

Expected Outcomes

After completing this lab, you should have:

Code artifacts:
- impedance_controller.py: Cartesian impedance control
- force_controller.py: Hybrid position/force control
- surface_following.py: Combined task demonstration
Understanding:
- Relationship between stiffness, damping, and compliance
- When to use impedance vs. direct force control
- Challenges of force control (sensor noise, stability)
Experimental results:
- Comparison of contact forces with different impedances
- Force tracking accuracy measurements
- Surface following performance

Rubric

Criterion	Points	Description
Jacobian Computation	15	Correct geometric Jacobian
Impedance Controller	25	Working compliance with tunable parameters
Force Controller	25	PI force regulation with hybrid control
Surface Following	20	Maintains contact while following path
Analysis	15	Comparison of control modes, parameter tuning
Total	100

Common Errors

Error: Unstable oscillations in force control Solution: Reduce force gain K_f, add filtering to force measurements.

Error: Force doesn't converge to desired value Solution: Check sensor direction conventions, increase integral gain carefully.

Error: Robot drifts during surface following Solution: Increase position gains for X-Y axes, ensure force axis selection is correct.

Extensions

For advanced students:

Admittance Control: Implement the dual of impedance control
Variable Impedance: Adapt stiffness based on task phase
Estimated Force: Use motor current for force estimation without sensors
Multi-Point Contact: Handle simultaneous contacts at multiple points

Theory: Module 07 theory.md, Section 7.3 (Force Control)
Previous Lab: Lab 07-01 (Basic Grasping)
Next Lab: Lab 07-03 (Dexterous Manipulation)
Application: Industrial polishing, assembly insertion, human collaboration

Objectives​

Prerequisites​

Materials​

Background​

Why Force Control?​

Impedance Control​

Force Control​

Instructions​

Step 1: Set Up Force-Sensing Robot​

Step 2: Implement Jacobian Computation​

Step 3: Implement Impedance Controller​

Step 4: Test Compliant Motion​

Step 5: Implement Force Controller​

Step 6: Surface Following Task​

Expected Outcomes​

Rubric​

Common Errors​

Extensions​

Related Content​