In a contact-rich task, success is decided by forces at the contact as much as by positions: a peg meets the rim of its hole, a hand holds a handle that can move only along a slide or an arc, a pusher steers an object it never holds. Position errors comparable to the clearances are unavoidable, and the controller decides what an error becomes. A stiff controller turns it into force, $F = K e$, until something gives: the actuators saturate, the grasp yields, the part tilts. A compliant controller turns it into a bounded force and a small displacement, which leaves room for a strategy (a search, following the mechanism, feedback) to finish the task.
[!established] Impedance control Impedance control regulates the relation between motion and force at the end effector rather than position alone: $F = K(x_\text{des} - x) + D(\dot x_\text{des} - \dot x)$, with stiffness and damping chosen per direction (Hogan, 1985). Without a force sensor, the force a held-still end effector applies is set by the stiffness times the offset of its target, which Lesson 8.3 measured. This lesson uses
mjcourse.control.cartesian_impedance, the 6-D version of Lesson 8.3’s controller (gravity compensation, null-space posture), on four tasks.
Insert runs one peg insertion with the hole’s position known only to within the chosen error. The peg has a radius of 12 mm and the square hole a half-width of 14 mm: 2 mm of clearance per side. Every number below comes from the script; the lab runs the same controller.
```lab peg {“dock”: true, “title”: “Peg in hole, 2 mm clearance”, “height”: 300}
## Mathematics
### What stiffness does with a position error
During the descent, the stiff controller's target moves down at 2 cm/s to 2 mm below the hole's floor. With the peg resting on the rim, the force it demands is $K$ times the distance between target and tip, up to 58 mm: $20\,000 \times 0.058 \approx 1160$ N, far more than the arm can deliver. Output (1) shows what happens instead. Within the clearance, the stiff and compliant controllers both insert; the stiff one seats the peg with 39.9 N ($K$ times the 2 mm the target sits below the floor), the compliant one with 2.0 N. At 5 mm of error, the stiff controller jams in all 8 directions with a median of 219 N held on the rim, and the peg tilts up to 78°: the saturated arm twists around the contact. At 3 mm the outcome depends on the direction: along an axis the peg jams or is forced in with a 9.5° tilt; along a diagonal the per-axis error is $3/\sqrt2 = 2.1$ mm, barely past the clearance, and the peg slides in.
The compliant controller never lets its target run more than $\delta$ = 5 mm below the tip, so the vertical force it can apply is bounded by $K_z\delta = 1000 \times 0.005 = 5$ N, which output (1) measures exactly on the rim. Saturating the target, not the force, is the simplest force limit an impedance controller has: it needs no force sensor and holds regardless of where the contact is. The 13 to 14 N peaks are impacts at 2 cm/s, not sustained loads.
> [!recommendation] Choose stiffness from the force you can tolerate
> Pick the largest error the task can see along each direction, $e$, and the largest force the parts, the gripper and the robot can take, $F_\text{max}$; then $K \le F_\text{max}/e$ along that direction. Keep the stiffness high where accuracy matters and no contact is expected, and low across directions that contact constrains. Bound the target's lead over the measured position for directions where contact is certain.
### Searching for the hole
A compliant controller makes a stall harmless; it does not insert the peg. With the peg pressing lightly on the surface, an Archimedean spiral $r = p\,\theta/2\pi$ around the estimate sweeps the surface until the peg drops into the hole. The pitch $p$ must not exceed twice the clearance $c$, or the path can pass between capture positions; here $p$ = 2 mm $\le 2c$ = 4 mm. Covering a disc of radius $e$ takes a path of about $\pi e^2/p$, so at path speed $v$ the search lasts about
$$t_\text{search} \approx \frac{\pi e^2}{p\,v}.$$
Output (2) measures it over 8 error directions: the longest searches (1.46, 3.25, 8.98 and 20.9 s at 3, 5, 8 and 12 mm) sit between 4% above and 17% below the formula (1.41, 3.93, 10.05 and 22.6 s). The quadratic dependence is the cost model: halving the perception error cuts the worst-case search fourfold, while doubling the speed only halves it and raises the friction drag that makes the peg lag behind its spiral.
### Mechanisms: drawers and doors
A drawer's slide and a door's hinge remove directions of motion. A controller that commands motion with a component across a removed direction converts that component into internal force. Pulling a drawer a distance $L$ along a direction pitched $\alpha$ above its slide, an impedance of stiffness $K$ ends up pressing across the slide with
$$F_\perp = K L \sin\alpha$$
if nothing else gives. Output (3): with $K$ = 500 N/m the force across the slide is 6.4 N at 5°, against 6.5 N predicted. At 10° and 20° it is 10.8 and 18.5 N against 13.0 and 25.7 N, because the gripper's jaws begin to give as well (4.0 and 13.5 mm pried open): the arm's impedance and the grasp act as springs in series and share the misalignment in inverse proportion to their stiffness. The stiff controller shows the limit of that picture. Its prediction is 261 to 1026 N, yet the handle sees 16.9 to 100.5 N, because the softest element now absorbs nearly all of the misalignment: the jaw servo (800 N/m, force-capped at 30 N) is pried open by 11.5, 23.4 and 40.4 mm, the last being the jaws' full stroke. A stiff arm does not remove compliance from the system; it moves the compliance to whatever is softest, and the grasp is rarely what you want that to be. The drawer opens about the same distance in every case, since the slide decides where it goes.
The door's handle is a vertical bar, and a parallel grasp on a cylinder acts as a hinge about the bar's axis: the hand keeps a fixed orientation while the bar turns between the pads, so the wrist does not have to turn with the door. Reach still limits the opening. Following the door toward 1.2 rad, the arm stops at 21.7°, where its elbow has folded until link 2 touches link 4 (last line of output 4), so the goal is 0.3 rad (17.2°). Output (4) tracks an arc about a hinge whose radius is believed 10 or 20% too short or too long. The stiff controller presses the handle radially with 9.6 to 13.3 N; the compliant one with 1.0 to 2.4 N. Compliance bounds the force, but both end at the wrong angle (13.6 to 20.8° instead of 17.2°): they follow the same wrong arc. The third controller has no hinge model at all. It pulls 2 cm ahead of the handle along the direction the handle last moved, ignoring apparent reversals, so the mechanism itself tells it where to go, and stops when the door's angle sensor reaches the goal: 18.3° (1.1° of overshoot) with at most 1.9 N across the arc. This is equilibrium point control as Jain and Kemp (2009) used it to open unknown doors and drawers.
### Pushing
A pusher with a flat face moving along its normal pushes a disc through the disc's centre, because the normal of a circle at any point passes through its centre: the puck slides along the face normal without turning. That makes open-loop pushing of a puck accurate when the start is known: 2.9 mm median error over 20 random starts with friction between 0.3 and 0.7 (output 5). It does nothing for errors in the start estimate. With σ = 10 mm, 15 of 20 pushes ended as far from the goal as the estimate was from the truth (median difference −1.3 mm). The other 5 had an error across the push line larger than the face's half-width (16 mm): the puck met the face's corner instead of its face, slid off sideways and ended 34 to 141 mm away. Closed loop, the face is turned every step so that its normal points from the puck's measured position to the goal, and the pusher stays just behind the puck: 3.2 mm median, 19 of 20 within 1 cm. The closed loop here reads the puck's true position (an ideal tracker); with a real one, its noise and latency set the final error instead. The general mechanics of pushing, including objects that turn, are Mason's (1986).
## Implementation
```io
INPUT: peg_insert.xml, articulated.xml, push.xml
PROCESS: peg insertion with three controllers over error size and direction; spiral search time; a drawer pulled off its axis; a door opened with a wrong hinge model and with none; pushing open and closed loop
OUTPUT: printed tables
```python file=examples/l10_3_contact_rich.py “"”Lesson 10.3: contact-rich tasks: peg insertion, a drawer, a door, pushing.
INPUT peg_insert.xml (arm7 with a peg of radius 12 mm over a square hole of half-width 14 mm: 2 mm clearance per side); articulated.xml (arm7 + gripper, a cabinet with a drawer and a door); push.xml (arm7 + gripper used as a pusher, a puck and a goal) PROCESS (1) peg insertion with the hole’s position known to within e mm (8 error directions): a stiff Cartesian impedance (20 kN/m); a compliant one (2 kN/m across, 1 kN/m along the hole) whose descending target never runs more than 5 mm below the tip, so the push stays near 5 N; the compliant one plus a spiral search when the peg stalls on the rim; outcome, peak and final contact force, largest tilt of the peg; (2) spiral search time against position error, compared with pi e^2 / (p v); (3) a drawer pulled 15 cm along a direction pitched alpha above its slide, stiff and compliant (500 N/m): force across the slide against K L sin(alpha), jaws pried open; (4) a door opened 0.3 rad by tracking an arc whose radius is off by -20 to +20 %, stiff and compliant, and by pulling along the handle’s own motion with no hinge model; (5) a puck pushed to a goal from random starts with random friction: open loop from the exact start, open loop from a noisy start estimate (sigma 10 mm), closed loop OUTPUT printed tables
Run: python examples/l10_3_contact_rich.py “””
import math import os
import mujoco import numpy as np
from mjcourse import control, kinematics, model_path, spatial
FAST = os.environ.get(“MJC_FAST”) == “1” DIRECTIONS = np.arange(2 if FAST else 8) * math.pi / (1 if FAST else 4) # error directions for the peg (rad) WN = 20.0 # joint-space bandwidth for approach moves (rad/s) K_NULL = 20.0 # null-space posture stiffness (N m/rad)
def min_jerk(s: float) -> float: s = min(max(s, 0.0), 1.0) return 10 * s3 - 15 * s4 + 6 * s**5
def contact_force(model, data, geoms_a: set[int], geoms_b: set[int]) -> np.ndarray: “"”World-frame force that geoms_a exert on geoms_b, summed over their contacts.””” total, f = np.zeros(3), np.zeros(6) for i in range(data.ncon): c = data.contact[i] if c.geom1 in geoms_a and c.geom2 in geoms_b: sign = 1.0 # the contact normal points from geom1 to geom2 elif c.geom2 in geoms_a and c.geom1 in geoms_b: sign = -1.0 else: continue mujoco.mj_contactForce(model, data, i, f) total += sign * (c.frame.reshape(3, 3).T @ f[:3]) return total
def geom_ids(model, names) -> set[int]: return {model.geom(n).id for n in names}
class Arm: “"”arm7 in one of the gripper scenes: joint-space approach moves, gripper commands, impedance steps.”””
def __init__(self, scene: str):
self.m = mujoco.MjModel.from_xml_path(str(model_path(scene)))
self.d = mujoco.MjData(self.m)
m, d = self.m, self.d
mujoco.mj_resetDataKeyframe(m, d, m.key("home").id)
mujoco.mj_forward(m, d)
mass = np.zeros((m.nv, m.nv))
mujoco.mj_fullM(m, d, mass)
self.kp, self.kd = np.diag(mass)[:7] * WN**2, 2 * np.diag(mass)[:7] * WN
self.target = d.qpos[:7].copy()
self.h = m.opt.timestep
self.down = spatial.mat_to_quat(d.site("gripper/tcp").xmat.reshape(3, 3))
def hold(self, n: int = 1) -> None:
m, d = self.m, self.d
for _ in range(n):
mujoco.mj_forward(m, d)
tau = self.kp * (self.target - d.qpos[:7]) - self.kd * d.qvel[:7] + d.qfrc_bias[:7]
d.ctrl[:7] = np.clip(tau, m.actuator_ctrlrange[:7, 0], m.actuator_ctrlrange[:7, 1])
mujoco.mj_step(m, d)
def move(self, pos: np.ndarray, quat: np.ndarray, seconds: float) -> None:
result, _ = kinematics.solve_ik_restarts(self.m, "gripper/tcp", pos, quat, q_init=self.d.qpos.copy(),
joints=list(range(7)), restarts=10, seed=0, tol_pos=5e-4, tol_rot=5e-3)
if not result.success:
raise RuntimeError(f"IK failed for {pos}")
start, goal, n = self.target.copy(), result.qpos[:7].copy(), round(seconds / self.h)
for k in range(n):
self.target = start + (goal - start) * min_jerk((k + 1) / n)
self.hold()
self.hold(round(0.3 / self.h))
def grip(self, command: float, seconds: float = 0.6) -> None:
self.d.ctrl[7] = command
self.hold(round(seconds / self.h))
def impedance(self, x_des, quat, k_pos, k_rot, q_rest) -> None:
mujoco.mj_forward(self.m, self.d)
self.d.ctrl[:7] = control.cartesian_impedance(self.m, self.d, "gripper/tcp", x_des, quat, k_pos, k_rot,
q_rest=q_rest, k_null=K_NULL)
mujoco.mj_step(self.m, self.d)
def tcp(self) -> np.ndarray:
return self.d.site("gripper/tcp").xpos.copy()
PEG_DELTA = 0.005 # the compliant target runs at most 5 mm below the tip PITCH, SEARCH_SPEED = 0.002, 0.01 # spiral pitch (m per turn) and path speed (m/s)
def insert(error: float, angle: float, mode: str, seconds: float = 10.0) -> dict:
“"”Insert the peg into a hole believed to be error m away (direction angle) from where it is.”””
m = mujoco.MjModel.from_xml_path(str(model_path(“peg_insert”)))
d = mujoco.MjData(m)
mujoco.mj_resetDataKeyframe(m, d, m.key(“home”).id)
mujoco.mj_forward(m, d)
tip, h = m.site(“tool/peg_tip”).id, m.opt.timestep
home, down = d.qpos.copy(), spatial.mat_to_quat(d.site_xmat[tip].reshape(3, 3))
top = d.site(“hole_top”).xpos.copy()
estimate = top[:2] + error * np.array([math.cos(angle), math.sin(angle)])
start, above = d.site_xpos[tip].copy(), np.r_[estimate, top[2] + 0.03]
peg, block = {m.geom(“tool/peg”).id}, geom_ids(m, (“hole_px”, “hole_nx”, “hole_py”, “hole_ny”, “hole_floor”))
xy, z_des, theta = estimate.copy(), above[2], 0.0
out = {“inserted”: None, “peak”: 0.0, “tilt”: 0.0, “stalled”: None, “dropped”: None, “late”: []}
for k in range(round(seconds / h)):
t = k * h
mujoco.mj_forward(m, d)
x = d.site_xpos[tip]
if t < 1.5: # to 3 cm above the estimate
target, k_pos = start + (above - start) * min_jerk(t / 1.2), 2000.0
elif mode == “stiff”:
z_des = max(z_des - 0.02 * h, 0.002) # descend at 2 cm/s to 2 mm below the floor
target, k_pos = np.r_[xy, z_des], 20000.0
else:
z_des = max(z_des - 0.02 * h, x[2] - PEG_DELTA, 0.002) # never more than PEG_DELTA below the tip
on_rim = x[2] > top[2] - 0.001
if mode == “search” and on_rim and z_des <= x[2] - PEG_DELTA + 1e-6 and out[“stalled”] is None:
out[“stalled”] = t
if mode == “search” and out[“stalled”] is not None and out[“dropped”] is None:
if on_rim: # Archimedean spiral around the estimate
r = PITCH * theta / (2 * math.pi)
theta += SEARCH_SPEED * h / max(r, 0.001)
xy = estimate + r * np.array([math.cos(theta), math.sin(theta)])
else:
out[“dropped”] = t
target, k_pos = np.r_[xy, z_des], np.array([2000.0, 2000.0, 1000.0])
d.ctrl[:] = control.cartesian_impedance(m, d, “tool/peg_tip”, target, down, k_pos, 300.0, q_rest=home, k_null=K_NULL)
mujoco.mj_step(m, d)
force = float(np.linalg.norm(contact_force(m, d, peg, block)))
out[“peak”] = max(out[“peak”], force)
out[“tilt”] = max(out[“tilt”], math.degrees(math.acos(min(1.0, -d.site_xmat[tip][8]))))
out[“late”].append(force)
if out[“inserted”] is None and d.site_xpos[tip][2] < 0.012:
out[“inserted”] = t
if out[“inserted”] is not None and t > out[“inserted”] + 1.0: # seated and settled
break
out[“final”] = float(np.mean(out[“late”][-round(0.5 / h):]))
out[“tip_z”] = d.site_xpos[tip][2]
return out
def outcome(r: dict) -> str: if r[“inserted”] is not None: return “inserted” if r[“tilt”] < 2.0 else “forced in, tilted” return “stalled on the rim” if r[“tilt”] < 2.0 else “jammed, tilted”
def peg_table() -> None: print(f” {‘controller’:<20}{‘e (mm)’:>7} {‘outcomes over directions’:<44}{‘peak (N)’:>9}{‘final (N)’:>10}{‘tilt (deg)’:>11}”) for mode, label in ((“stiff”, “stiff 20 kN/m”), (“compliant”, “compliant”), (“search”, “compliant + search”)): for e in (0.0, 1.5, 3.0, 5.0): runs = [insert(e / 1000, a, mode) for a in DIRECTIONS] names = [outcome(r) for r in runs] summary = “, “.join(f”{names.count(n)} {n}” for n in dict.fromkeys(names)) print(f” {label:<20}{e:>7.1f} {summary:<44}{np.median([r[‘peak’] for r in runs]):>9.1f}” f”{np.median([r[‘final’] for r in runs]):>10.1f}{max(r[‘tilt’] for r in runs):>11.1f}”)
def search_table() -> None: print(f” {‘e (mm)’:>7}{‘inserted’:>10}{‘searched’:>10}{‘mean search (s)’:>17}{‘max search (s)’:>16}{‘pi e^2/(p v) (s)’:>18}”) for e in ((3.0, 8.0) if FAST else (3.0, 5.0, 8.0, 12.0)): runs = [insert(e / 1000, a, “search”, seconds=30.0) for a in DIRECTIONS] times = [r[“dropped”] - r[“stalled”] for r in runs if r[“dropped”] is not None and r[“stalled”] is not None] bound = math.pi * (e / 1000) ** 2 / (PITCH * SEARCH_SPEED) print(f” {e:>7.1f}{sum(r[‘inserted’] is not None for r in runs):>7d}/{len(runs)}{len(times):>10d}” f”{np.mean(times) if times else float(‘nan’):>17.2f}{max(times) if times else float(‘nan’):>16.2f}{bound:>18.2f}”)
R_FRONT_BAR = np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]], float) # tool along +x, jaws close vertically R_FRONT_POST = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]], float) # tool along +x, jaws close horizontally PADS = (“gripper/pad_left”, “gripper/pad_right”, “gripper/finger_left”, “gripper/finger_right”)
def grasp_handle(arm: Arm, site: str, rotation: np.ndarray) -> np.ndarray: quat = spatial.mat_to_quat(rotation) handle = arm.d.site(site).xpos.copy() arm.grip(0.04, 0.1) arm.move(handle - np.array([0.08, 0, 0]), quat, 2.0) arm.move(handle, quat, 1.0) arm.grip(0.0, 0.8) return quat
def drawer(alpha_deg: float, k_pos: float, length: float = 0.15, seconds: float = 3.0) -> tuple[float, float, float]: arm = Arm(“articulated”) m, d = arm.m, arm.d quat = grasp_handle(arm, “drawer_handle”, R_FRONT_BAR) x0, q_rest, a = arm.tcp(), d.qpos[:7].copy(), math.radians(alpha_deg) pull = np.array([-math.cos(a), 0.0, math.sin(a)]) # believed direction; the slide is along -x pads, handle = geom_ids(m, PADS), geom_ids(m, (“drawer_handle”, “drawer_handle_l”, “drawer_handle_r”)) opening0, pried = d.ten_length[0], 0.0 n = round(seconds / arm.h) for k in range(n + round(1.0 / arm.h)): arm.impedance(x0 + length * min_jerk((k + 1) / n) * pull, quat, k_pos, 300.0, q_rest) pried = max(pried, d.ten_length[0] - opening0) across = abs(contact_force(m, d, pads, handle)[2]) return d.joint(“drawer”).qpos[0], across, pried
def drawer_table() -> None: print(f” {‘controller’:<16}{‘alpha (deg)’:>12}{‘opened (mm)’:>13}{‘force across slide (N)’:>24}{‘K L sin(alpha) (N)’:>20}{‘jaws pried (mm)’:>17}”) for label, k_pos in ((“stiff 20 kN/m”, 20000.0), (“compliant”, 500.0)): for alpha in (0, 5, 10, 20): opened, across, pried = drawer(alpha, k_pos) print(f” {label:<16}{alpha:>12d}{1000 * opened:>13.1f}{across:>24.1f}” f”{k_pos * 0.15 * math.sin(math.radians(alpha)):>20.1f}{1000 * pried:>17.1f}”)
DOOR_GOAL = -0.3 # rad; the door opens toward the robot, negative about +z
def door(mode: str, radius_error: float = 0.0, seconds: float = 3.0, lead: float = 0.02, goal: float = DOOR_GOAL) -> tuple[float, float, list[str]]: “"”Open the door with the hand’s orientation held fixed: the vertical bar turns between the pads, so the grasp acts as a hinge. Returns the door angle, the peak radial force on the handle and the arm’s self-contacts at the end.””” arm = Arm(“articulated”) m, d = arm.m, arm.d quat = grasp_handle(arm, “door_handle”, R_FRONT_POST) x0, q_rest = arm.tcp(), d.qpos[:7].copy() hinge_true = np.r_[d.body(“door”).xpos[:2], x0[2]] believed = x0 + (hinge_true - x0) * (1 + radius_error) # the hinge, as far off as the radius error says pads, handle = geom_ids(m, PADS), geom_ids(m, (“door_handle”, “door_handle_t”, “door_handle_b”)) k_pos = 20000.0 if mode == “stiff” else 500.0 direction, last, peak = np.array([-1.0, 0.0, 0.0]), x0.copy(), 0.0 n = round(seconds / arm.h) for k in range(n + round(1.0 / arm.h)): x = arm.tcp() if mode == “follow”: # pull along the handle’s own motion, no hinge model if np.linalg.norm((x - last)[:2]) > 2e-4: moved = np.r_[(x - last)[:2] / np.linalg.norm((x - last)[:2]), 0.0] if moved @ direction > 0.5: # a rebound is not a new direction: ignore reversals direction = moved last = x x_des = x + lead * direction if d.sensor(“door_angle”).data[0] > goal else x else: # track an arc about the believed hinge th = goal * min_jerk((k + 1) / n) rel = x0 - believed x_des = believed + np.array([math.cos(th) * rel[0] - math.sin(th) * rel[1], math.sin(th) * rel[0] + math.cos(th) * rel[1], rel[2]]) arm.impedance(x_des, quat, k_pos, 300.0, q_rest) radial = hinge_true - d.site(“door_handle”).xpos radial[2] = 0.0 peak = max(peak, abs(contact_force(m, d, pads, handle) @ radial) / np.linalg.norm(radial)) arm_bodies = {m.body(f”link{i}”).id for i in range(1, 8)} self_contacts = sorted({f”{m.geom(c.geom1).name}-{m.geom(c.geom2).name}” for c in d.contact[:d.ncon] if m.geom_bodyid[c.geom1] in arm_bodies and m.geom_bodyid[c.geom2] in arm_bodies}) return d.joint(“door”).qpos[0], peak, self_contacts
def door_table() -> None: print(f” goal {math.degrees(DOOR_GOAL):.1f} deg”) print(f” {‘controller’:<26}{‘radius error’:>13}{‘door (deg)’:>12}{‘peak radial force (N)’:>23}”) for mode, label in ((“stiff”, “stiff arc 20 kN/m”), (“compliant”, “compliant arc 500 N/m”)): for err in (0.0, -0.1, 0.1, -0.2, 0.2): angle, peak, _ = door(mode, err) print(f” {label:<26}{err:>+13.0%}{math.degrees(angle):>12.1f}{peak:>23.1f}”) angle, peak, _ = door(“follow”) print(f” {‘follow the motion 500 N/m’:<26}{‘no model’:>13}{math.degrees(angle):>12.1f}{peak:>23.1f}”) angle, _, contacts = door(“follow”, goal=-1.2, seconds=6.0) print(f” reach: following the motion toward 1.2 rad stops at {math.degrees(angle):.1f} deg; “ f”arm self-contacts then: {‘, ‘.join(contacts) or ‘none’}”)
R_PUCK, FACE_HALF_WIDTH, PUSHER_HALF, TCP_Z, PUSH_SPEED = 0.035, 0.016, 0.008, 0.03, 0.05
def push(puck_xy: np.ndarray, mu: float, closed_loop: bool, estimate_error: np.ndarray, delta: float = 0.003) -> float: “"”Push the puck to the goal with the closed fingers. The pushing face is 32 mm wide, perpendicular to the hand’s x axis; yaw turns it about the vertical.””” arm = Arm(“push”) m, d = arm.m, arm.d adr = m.jnt_qposadr[m.joint(“puck”).id] d.qpos[adr:adr + 3] = [*puck_xy, 0.02] for name in (“floor”, “puck”): m.geom_friction[m.geom(name).id, 0] = mu mujoco.mj_forward(m, d) goal = d.site(“goal”).xpos[:2].copy()
def puck() -> np.ndarray:
return d.body("puck").xpos[:2].copy()
def face(yaw: float) -> np.ndarray:
return spatial.quat_mul(np.array([math.cos(yaw / 2), 0, 0, math.sin(yaw / 2)]), arm.down)
estimate = puck() + estimate_error
u = (goal - estimate) / np.linalg.norm(goal - estimate)
yaw = math.atan2(u[1], u[0])
arm.grip(0.0, 0.3)
start = estimate - (R_PUCK + PUSHER_HALF + 0.03) * u
arm.move(np.r_[start, 0.12], face(yaw), 1.5)
arm.move(np.r_[start, TCP_Z], face(yaw), 1.0)
target, q_rest = arm.tcp()[:2], d.qpos[:7].copy()
distance, travelled = np.linalg.norm(goal - estimate) + 0.03 + delta, 0.0
for _ in range(round(8.0 / arm.h)):
if closed_loop:
p = puck()
if np.linalg.norm(goal - p) > 0.005:
to_goal = (goal - p) / np.linalg.norm(goal - p)
yaw = math.atan2(to_goal[1], to_goal[0]) # face normal from the puck to the goal
step = p - (R_PUCK + PUSHER_HALF - delta) * to_goal - target
if np.linalg.norm(step) > PUSH_SPEED * arm.h:
step *= PUSH_SPEED * arm.h / np.linalg.norm(step)
target = target + step
elif travelled < distance:
target, travelled = target + PUSH_SPEED * arm.h * u, travelled + PUSH_SPEED * arm.h
arm.impedance(np.r_[target, TCP_Z], face(yaw), 2000.0, 300.0, q_rest)
return float(np.linalg.norm(puck() - goal))
def push_table() -> None: n = 4 if FAST else 20 rng = np.random.default_rng(3) starts = np.c_[rng.uniform(0.42, 0.48, n), rng.uniform(-0.03, 0.03, n)] mus, errors = rng.uniform(0.3, 0.7, n), rng.normal(0.0, 0.01, (n, 2)) goal = mujoco.MjModel.from_xml_path(str(model_path(“push”))).site(“goal”).pos[:2] lateral = [] for s, e in zip(starts, errors): # component of the error across the push line u = (goal - s - e) / np.linalg.norm(goal - s - e) lateral.append(abs(e @ np.array([-u[1], u[0]]))) print(f” {n} starts in x 0.42-0.48, y -0.03-0.03 m; floor and puck friction 0.3-0.7; goal {goal} m”) print(f” {‘strategy’:<34}{‘median error (mm)’:>18}{‘max (mm)’:>10}{‘within 1 cm’:>13}”) results = {} for label, closed, errs in ((“open loop, exact start”, False, np.zeros((n, 2))), (“open loop, start sigma 10 mm”, False, errors), (“closed loop, start sigma 10 mm”, True, errors)): out = np.array([push(s, mu, closed, e) for s, mu, e in zip(starts, mus, errs)]) results[label] = out print(f” {label:<34}{1000 * np.median(out):>18.1f}{1000 * out.max():>10.1f}{(out < 0.01).sum():>10d}/{n}”) open_noisy = results[“open loop, start sigma 10 mm”] off_face = [i for i, lat in enumerate(lateral) if lat > FACE_HALF_WIDTH] print(f” open loop with noise: starts whose error across the push line exceeded the face half-width “ f”({1000 * FACE_HALF_WIDTH:.0f} mm): {len(off_face)}, final errors (mm) “ + “, “.join(f”{1000 * open_noisy[i]:.0f}” for i in off_face)) within = [i for i in range(n) if i not in off_face] if within: print(f” the others: final error minus start-estimate error, median {1000 * np.median([open_noisy[i] - np.linalg.norm(errors[i]) for i in within]):+.1f} mm”)
if name == “main”: print(f”MuJoCo {mujoco.version}; peg error directions: {len(DIRECTIONS)}”) print(“(1) peg insertion, 2 mm clearance, hole position known to within e”) peg_table() print(“(2) spiral search (pitch 2 mm, 10 mm/s) time against position error”) search_table() print(“(3) drawer pulled 15 cm along a direction pitched alpha above its slide”) drawer_table() print(“(4) door opened 0.3 rad”) door_table() print(“(5) pushing a puck to a goal”) push_table()
Output:
```text
MuJoCo 3.14.0; peg error directions: 8
(1) peg insertion, 2 mm clearance, hole position known to within e
controller e (mm) outcomes over directions peak (N) final (N) tilt (deg)
stiff 20 kN/m 0.0 8 inserted 39.9 39.9 0.1
stiff 20 kN/m 1.5 8 inserted 39.9 39.9 0.1
stiff 20 kN/m 3.0 1 forced in, tilted, 4 inserted, 3 jammed, tilted 125.1 42.3 9.5
stiff 20 kN/m 5.0 8 jammed, tilted 227.0 219.2 77.7
compliant 0.0 8 inserted 13.8 2.0 0.1
compliant 1.5 8 inserted 13.8 2.0 0.1
compliant 3.0 4 stalled on the rim, 4 inserted 13.4 3.5 0.1
compliant 5.0 8 stalled on the rim 12.9 5.0 0.1
compliant + search 0.0 8 inserted 13.8 2.0 0.1
compliant + search 1.5 8 inserted 13.8 2.0 0.1
compliant + search 3.0 8 inserted 13.8 2.0 0.1
compliant + search 5.0 8 inserted 13.8 2.0 0.1
(2) spiral search (pitch 2 mm, 10 mm/s) time against position error
e (mm) inserted searched mean search (s) max search (s) pi e^2/(p v) (s)
3.0 8/8 4 1.01 1.46 1.41
5.0 8/8 8 2.33 3.25 3.93
8.0 8/8 8 7.16 8.98 10.05
12.0 8/8 8 17.99 20.93 22.62
(3) drawer pulled 15 cm along a direction pitched alpha above its slide
controller alpha (deg) opened (mm) force across slide (N) K L sin(alpha) (N) jaws pried (mm)
stiff 20 kN/m 0 149.7 0.1 0.0 0.0
stiff 20 kN/m 5 149.1 16.9 261.5 11.5
stiff 20 kN/m 10 147.4 26.4 520.9 23.4
stiff 20 kN/m 20 140.7 100.5 1026.1 40.4
compliant 0 149.3 0.0 0.0 0.0
compliant 5 148.7 6.4 6.5 0.1
compliant 10 147.0 10.8 13.0 4.0
compliant 20 140.2 18.5 25.7 13.5
(4) door opened 0.3 rad
goal -17.2 deg
controller radius error door (deg) peak radial force (N)
stiff arc 20 kN/m +0% -17.1 4.8
stiff arc 20 kN/m -10% -15.3 9.6
stiff arc 20 kN/m +10% -19.0 10.4
stiff arc 20 kN/m -20% -13.6 10.6
stiff arc 20 kN/m +20% -20.8 13.3
compliant arc 500 N/m +0% -17.0 0.5
compliant arc 500 N/m -10% -15.4 1.1
compliant arc 500 N/m +10% -18.7 1.0
compliant arc 500 N/m -20% -13.7 1.8
compliant arc 500 N/m +20% -20.4 2.4
follow the motion 500 N/m no model -18.3 1.9
reach: following the motion toward 1.2 rad stops at -21.7 deg; arm self-contacts then: link2-link4
(5) pushing a puck to a goal
20 starts in x 0.42-0.48, y -0.03-0.03 m; floor and puck friction 0.3-0.7; goal [0.62 0.12] m
strategy median error (mm) max (mm) within 1 cm
open loop, exact start 2.9 3.5 20/20
open loop, start sigma 10 mm 10.0 140.6 10/20
closed loop, start sigma 10 mm 3.2 12.3 19/20
open loop with noise: starts whose error across the push line exceeded the face half-width (16 mm): 5, final errors (mm) 69, 141, 34, 47, 129
the others: final error minus start-estimate error, median -1.3 mm
The full run takes about three minutes on one CPU core; MJC_FAST=1 runs 2 error directions and 4 pushes.
Contact forces of hundreds of newtons in simulation. A stiff controller whose target has run far past an obstacle. Check the distance between target and measured position during contact; bound it (the $\delta$ clamp) or lower the stiffness along that direction. On hardware this is the failure that breaks parts.
The peg inserts in simulation only by tilting. A soft wrist (low rotational stiffness) let the off-centre rim force tip the peg into the hole. Check the tilt; the same insertion on a stiff wrist jams. Decide whether the compliance is real (a compliant wrist) or an artefact of the gains.
A grasped handle slips or the jaws open while pulling a mechanism. The pull direction is off the mechanism’s axis and the arm is stiffer than the grasp. Measure the force across the motion direction, lower the arm’s stiffness across it, or follow the measured motion.
A search never finds a hole it should. Pitch larger than twice the clearance, or stiffness across the surface too low for the friction drag, so the peg lags behind the spiral. Compute the lag $\mu F_z/K_\perp$ and keep it below the clearance.
The door or drawer stops early and the controller keeps pulling. Reach, not force: a joint limit or a self-collision. Print the arm’s joint values and contacts when the motion stalls.
A controller that follows the motion closes the door again. When the motion stalls, the handle rebounds slightly, and a follower that accepts any measured direction reads the rebound as the new direction of travel and pulls the door shut. Accept only directions within 60° of the current one, as the lesson’s script does.
Predict the worst-case search time for a 6 mm error with the lesson’s pitch and speed. Then set PITCH to 4 mm and to 6 mm, rerun part (2), and check whether any direction fails to find the hole within 30 s at the larger pitch.
Estimate the door’s hinge from the handle’s motion: fit a circle to the handle positions recorded during the first 0.1 rad of a follow-the-motion opening, then switch to arc tracking about the fitted hinge with the compliant controller. Report the hinge error, the final angle and the peak radial force against the three controllers of output (4).
[!research] Compliance as part of the action Learned manipulation policies usually output positions or position increments that a stiff controller tracks, which is what outputs (1) and (3) show to be dangerous at contact: the policy’s position error becomes force. Letting the policy choose the impedance as well as the target (variable impedance in end-effector space) improved learning and transfer in contact-rich tasks in the study of Martín-Martín et al. (2019). The classical picture is older: Whitney (1982) analysed how compliance in the support of a peg decides whether insertion jams or wedges, and remote-centre compliance devices build that answer into hardware. A simulated policy evaluated only on success rate can hide hundred-newton contact forces of the kind output (1) records (inference); logging peak contact force and tilt alongside success is cheap in MuJoCo and changes which policies look good.
{"id": "10.3-check", "title": "Knowledge check", "questions": [
{"kind": "numeric", "q": "An impedance controller has a vertical stiffness of 1500 N/m and never lets its target run more than 4 mm below the tool. What is the largest steady force (N) it presses on a surface with?",
"answer": 6.0, "tol": 0.05, "unit": "N",
"explain": "<p>$K\\delta = 1500 \\times 0.004$ = 6 N, whatever the target trajectory does.</p>"},
{"kind": "numeric", "q": "A spiral search with pitch 2 mm runs at 10 mm/s. Estimate the worst-case time (s) to find a hole 6 mm from the estimate.",
"answer": 5.65, "tol": 0.1, "unit": "s",
"explain": "<p>$\\pi e^2/(p v) = \\pi \\times 36 / (2 \\times 10)$ = 5.65 s.</p>"},
{"kind": "mcq", "q": "A stiff arm (20 kN/m) pulls a drawer 15 cm along a direction 10° off its slide. K L sin α predicts 521 N across the slide, but the handle sees 26 N. Why?",
"options": ["The slide joint has friction", "The gripper's jaws are the softest spring in series and give way, absorbing the misalignment", "MuJoCo's contacts are too soft", "The arm's torque limits cap it at 26 N"],
"answer": 1,
"explain": "<p>Output (3): the jaws were pried open by 23.4 mm. The misalignment goes to the softest element.</p>"},
{"kind": "mcq", "q": "Why does a flat pusher moving along its normal push a disc without turning it?",
"options": ["Friction between the floor and the disc is symmetric", "The contact normal of a circle passes through its centre, so the push acts through the centre", "The disc is lighter than the pusher", "MuJoCo ignores torsional friction"],
"answer": 1,
"explain": "<p>The contact force acts along the face normal and through the disc's centre, so it has no moment about the centre, as long as the contact stays on the face.</p>"}
]}
Level 11 turns to what a robot sees: cameras, depth and segmentation rendered from the same scenes, and the datasets built from them.