Robotics Simulation
& Software

Chapter 1: Rigid Body Dynamics — The Physics Engine Core

CONCEPT: Newton-Euler Equations

Every physics simulator solves the same fundamental problem: given forces acting on objects, compute how they move. The state of a rigid body is fully described by six quantities: position (x, y, z), orientation (how it's rotated), linear velocity (v_x, v_y, v_z), and angular velocity (ω_x, ω_y, ω_z). That's 13 numbers if you use quaternions for orientation (4 numbers instead of 3 Euler angles).

Why quaternions? Euler angles suffer from gimbal lock — a configuration where two rotation axes align and you lose a degree of freedom. Point your arm straight up and try to distinguish "turn left" from "roll" — they become the same motion. Quaternions avoid this entirely at the cost of one extra number and a normalization constraint (q_w² + q_x² + q_y² + q_z² = 1).

The equations of motion are Newton's second law applied to both translation and rotation:

The gyroscopic term ω × (I · ω) is what makes rotation non-intuitive. A spinning top stays upright because of this cross product. A tumbling satellite exhibits bizarre wobbling. This term is what separates "physics for games" (which often ignores it) from "physics for robotics" (which cannot).

DESIGN: The Simulation Loop

The simulation runs in a tight loop at a fixed timestep, typically 1kHz (dt = 0.001s) for robotics. Each step:

Timestep selection is the first design decision. Too large (dt=0.01s) and the simulation becomes unstable — objects fly through each other or energy grows without bound. Too small (dt=0.0001s) and the simulation is 10x slower than needed. The sweet spot for rigid body robotics: 1-4ms (250Hz-1kHz). For soft body or deformable objects, you may need 0.1ms.

Integration method matters enormously. Explicit Euler (v += a*dt, x += v*dt) is simple but adds energy over time — your simulation slowly "explodes." Semi-implicit Euler (update velocity first, then use new velocity for position) is nearly as simple but much more stable. Runge-Kutta 4 (RK4) is accurate but expensive — 4 force evaluations per step. Most physics engines use semi-implicit Euler because the stability-per-cost ratio is unbeatable.

CODE: 2D Rigid Body Simulator from Scratch

python
import numpy as np

class RigidBody2D:
    def __init__(self, mass, inertia, x, y, angle=0):
        self.mass = mass
        self.inertia = inertia  # scalar for 2D
        self.pos = np.array([x, y], dtype='float64')
        self.angle = angle
        self.vel = np.zeros(2)
        self.omega = 0.0       # angular velocity (scalar in 2D)
        self.force = np.zeros(2)
        self.torque = 0.0

    def apply_force(self, f, point=None):
        self.force += f
        if point is not None:
            r = point - self.pos
            self.torque += r[0]*f[1] - r[1]*f[0]  # 2D cross product

    def step(self, dt, gravity=np.array([0, -9.81])):
        # Semi-implicit Euler: update velocity first, then position
        self.force += self.mass * gravity
        a = self.force / self.mass
        alpha = self.torque / self.inertia

        self.vel += a * dt          # velocity update
        self.omega += alpha * dt    # angular velocity update
        self.pos += self.vel * dt   # position uses NEW velocity
        self.angle += self.omega * dt

        # Reset accumulators
        self.force = np.zeros(2)
        self.torque = 0.0

# Ground collision with restitution
def ground_collision(body, ground_y=0, restitution=0.6):
    if body.pos[1] < ground_y:
        body.pos[1] = ground_y          # project out of ground
        body.vel[1] *= -restitution     # bounce with energy loss
        body.omega *= 0.95              # friction damps rotation

# Run simulation
box = RigidBody2D(mass=1.0, inertia=0.1, x=0, y=5.0, angle=0.3)
dt = 0.001  # 1kHz
for _ in range(5000):  # 5 seconds
    ground_collision(box)
    box.step(dt)
    # Energy = 0.5*m*v^2 + 0.5*I*w^2 + m*g*h
    KE = 0.5*box.mass*np.dot(box.vel, box.vel) + 0.5*box.inertia*box.omega**2
    PE = box.mass * 9.81 * box.pos[1]
    # With semi-implicit Euler, total energy decays (stable)
    # With explicit Euler, it would GROW (unstable)

DEBUG: Common Physics Simulation Failures

Worked Example: Energy Conservation Across Integrators

Let's trace a single bouncing ball through three integration methods to see why the choice matters. Initial state: height = 5m, velocity = 0, mass = 1kg, g = 9.81 m/s², dt = 0.01s, restitution = 1.0 (perfectly elastic).

Explicit Euler (update position with OLD velocity, then update velocity):

Semi-implicit Euler (update velocity FIRST, then position with NEW velocity):

RK4: E stays within 0.001% of initial after 10 bounces — accurate but costs 4x per step. For robotics at 1kHz, semi-implicit Euler gives the best stability-per-compute ratio.

FRONTIER: Differentiable Physics

Differentiable physics engines compute gradients through the simulation. Instead of just running forward simulation, you can backpropagate through the physics to optimize control parameters, robot designs, or sim parameters to match real data. Key projects:

The practical impact: differentiable physics lets you do gradient-based system identification. Instead of grid-searching over friction values, compute d(sim_trajectory)/d(friction) and use gradient descent to match real trajectories. This converts a combinatorial search (exponential in number of parameters) into a smooth optimization (linear in iterations).

The frontier is learning simulation parameters from real data. Record a real robot dropping an object, then backpropagate through the differentiable sim to find the friction and restitution coefficients that make the sim match reality. This is system identification via gradient descent — dramatically faster than manual tuning.

Chapter 2: Contact & Collision — Where Simulation Gets Hard

CONCEPT: Collision Detection Pipeline

Contact is the hardest problem in physics simulation. Two objects touching creates a discontinuity — velocity can change instantaneously. The simulation must detect when objects collide, compute the contact forces, and resolve the interpenetration, all within one timestep.

The collision detection pipeline has two phases. Broad phase quickly eliminates pairs that definitely don't collide using cheap bounding volume tests. Narrow phase computes exact contact points for the remaining candidates.

Broad phase uses AABB (axis-aligned bounding boxes). Each object gets a box aligned to the world axes. Two AABBs overlap only if they overlap on ALL three axes — a trivial comparison of six numbers. For N objects, a naive check is O(N²), but a sweep-and-prune algorithm (sort AABBs along each axis) reduces this to O(N log N). A bin of 100 objects has 4,950 potential pairs, but broad phase culls this to ~200.

Narrow phase uses GJK (Gilbert-Johnson-Keerthi) for convex shapes. GJK tests whether the Minkowski difference of two shapes contains the origin. If it does, they intersect. EPA (Expanding Polytope Algorithm) then finds the penetration depth and contact normal. For non-convex shapes (meshes), the engine decomposes them into convex hulls first.

DESIGN: Contact Resolution Strategies

Once we detect a contact, we need to resolve it. There are two fundamentally different approaches:

Penalty methods are the simplest. When two objects overlap by depth d along normal n, apply a spring force F = k · d · n. This pushes them apart. The problem: to prevent visible penetration, you need a very stiff spring (large k), which requires a very small timestep to remain stable. It's a tradeoff between visual quality and simulation speed.

Constraint-based methods (used by MuJoCo, Drake, Bullet) formulate contacts as velocity-level constraints. Instead of "apply a force proportional to penetration," they say "find the minimum force that prevents further penetration." This is a Linear Complementarity Problem (LCP): either the contact force is zero (objects separating) or the gap is zero (objects touching). Never both positive. MuJoCo goes further with a convex optimization formulation that always has a unique solution.

Friction follows the Coulomb model: tangential friction force |f_t| ≤ μ · f_n, where μ is the friction coefficient and f_n is the normal force. The friction force lives inside a cone. Below the cone limit, the contact is sticking (static friction). At the limit, it's sliding (kinetic friction). Simulating this accurately is critical for grasping — without proper friction, a robot can't hold anything.

CODE: AABB Collision + Penalty Response

python
import numpy as np

def aabb_overlap(a_min, a_max, b_min, b_max):
    """Check if two AABBs overlap. O(1) per pair."""
    return (a_min[0] <= b_max[0] and a_max[0] >= b_min[0] and
            a_min[1] <= b_max[1] and a_max[1] >= b_min[1])

def circle_collision(p1, r1, p2, r2):
    """Narrow phase for circles: penetration depth + normal."""
    diff = p2 - p1
    dist = np.linalg.norm(diff)
    if dist < r1 + r2 and dist > 1e-8:
        normal = diff / dist
        depth = (r1 + r2) - dist
        return depth, normal
    return 0, np.zeros(2)

def penalty_force(depth, normal, v_rel, k=5000, b=50):
    """Spring-damper penalty force."""
    if depth <= 0: return np.zeros(2)
    f_spring = k * depth * normal          # pushes objects apart
    f_damp = -b * np.dot(v_rel, normal) * normal  # damps oscillation
    return f_spring + f_damp

# Why penalty is bad for robotics:
# k=5000 requires dt < 2*sqrt(m/k) ~ 0.03s for stability
# k=50000 (for stiff contact) requires dt < 0.009s
# k=500000 (realistic steel) requires dt < 0.003s
# Real engines use constraint-based methods instead

DEBUG: Contact Simulation Failures

FRONTIER: Differentiable Contact and Learned Models

Differentiable contact is the holy grail. Contact creates discontinuities (velocity jumps), which break gradients. Approaches: (1) Randomized smoothing (Dojo) — average over random perturbations to smooth out the discontinuity. (2) Soft contact models (Drake's hydroelastic) — replace hard contacts with smooth force fields. (3) Learned contact models — train a neural network to predict contact forces from geometry and velocities. The frontier is combining physics-based priors with learned residuals.

Chapter 3: Simulation Platforms — MuJoCo, Isaac Sim, Drake, PyBullet

CONCEPT: What Each Simulator Does Well

Choosing the right simulator is a high-leverage decision. Each has a philosophy that shapes what it's good at:

MuJoCo (Multi-Joint dynamics with Contact) was created by Emo Todorov at the University of Washington. Its core insight: formulate contact as a convex optimization problem, which always has a unique solution and is fast to solve. MuJoCo runs at 10-50 million timesteps per second on a single CPU thread. It's the default for reinforcement learning research because you need millions of episodes, and speed is everything. Since DeepMind acquired it (now open-source), it has become the standard.

Isaac Sim (NVIDIA) is built on PhysX 5 and Omniverse. Its killer feature: GPU-accelerated parallel simulation. Instead of running one environment fast, run 4,096 environments simultaneously on a single GPU. Combined with photorealistic rendering via ray tracing, it's unbeatable for (1) massive RL training and (2) visual sim-to-real transfer where you need realistic images.

Drake (MIT Robot Locomotion Group) emphasizes mathematical rigor. Every algorithm has correctness guarantees. Its contact model (hydroelastic) produces smooth, physically meaningful contact patches. Drake includes optimization-based planners (SNOPT, IPOPT) and controllers (LQR, MPC) out of the box. Choose Drake when you need provably correct behavior — like designing a controller for a legged robot that must never fall.

PyBullet wraps the Bullet physics engine in Python. Simple API, good documentation, fast to prototype. But its contact model is less accurate than MuJoCo's, and it lacks GPU acceleration. Use PyBullet for quick experiments and educational prototypes, then graduate to MuJoCo or Isaac for serious work.

DESIGN: Choosing a Simulator

CODE: MuJoCo Hello World

python
import mujoco
import numpy as np

# Define a simple robot arm in MJCF (MuJoCo's XML format)
XML = """
<mujoco>
  <option timestep="0.002" gravity="0 0 -9.81"/>
  <worldbody>
    <light pos="0 0 3"/>
    <geom type="plane" size="2 2 0.1"/>
    <body name="link1" pos="0 0 0.5">
      <joint name="j1" type="hinge" axis="0 1 0"/>
      <geom type="capsule" size="0.04" fromto="0 0 0 0 0 0.4" rgba="1 .5 .3 1"/>
      <body name="link2" pos="0 0 0.4">
        <joint name="j2" type="hinge" axis="0 1 0"/>
        <geom type="capsule" size="0.03" fromto="0 0 0 0 0 0.3" rgba=".3 .8 .7 1"/>
      </body>
    </body>
  </worldbody>
  <actuator>
    <motor joint="j1" ctrlrange="-2 2"/>
    <motor joint="j2" ctrlrange="-2 2"/>
  </actuator>
</mujoco>
"""

model = mujoco.MjModel.from_xml_string(XML)
data = mujoco.MjData(model)

# Simulate 1000 steps
for i in range(1000):
    data.ctrl[:] = [0.5, -0.3]  # apply torques to joints
    mujoco.mj_step(model, data)
    if i % 100 == 0:
        print(f"t={data.time:.2f}s  j1={data.qpos[0]:.3f}rad  j2={data.qpos[1]:.3f}rad")

DEBUG: Simulator-Specific Issues

FRONTIER: Next-Generation Simulators

Chapter 4: Robot Kinematics & Dynamics

CONCEPT: Forward and Inverse Kinematics

A robot arm is a chain of links connected by joints. Each joint has an angle θ. Given all the joint angles, where is the end-effector (the tool at the tip)? That's forward kinematics (FK) — a straightforward chain of matrix multiplications. Given a desired end-effector position, what joint angles achieve it? That's inverse kinematics (IK) — a much harder problem because it's typically underdetermined (multiple solutions) or overconstrained (no solution).

For a 2-link planar arm (the simplest case), FK uses trigonometry:

The Jacobian J is the matrix that maps joint velocities to end-effector velocities: v = J · θ̇. It's computed by taking partial derivatives of the FK equations with respect to each joint angle. The Jacobian is the central object in robotics — it tells you how the end-effector moves when you wiggle each joint.

where s₁ = sin(θ₁), s₁₂ = sin(θ₁+θ₂), etc.

IK via the Jacobian pseudoinverse: Δθ = J⁺ · Δx. This iteratively adjusts joint angles to move the end-effector toward the target. When the Jacobian becomes singular (loses rank), IK fails. This happens at singularities — configurations where the arm is fully extended or folded. The fix is damped least squares: Δθ = J^T(JJ^T + λ²I)^-1 · Δx. The damping λ prevents the solution from blowing up near singularities.

DESIGN: The Control Hierarchy

Robot control operates in three coordinate spaces, forming a hierarchy:

A 7-DOF arm (like the Franka Panda) solving a 6-DOF task (3D position + 3D orientation) has one extra degree of freedom. This redundancy creates a null space — a set of joint motions that don't change the end-effector pose. You can use this null space to optimize a secondary objective: avoid joint limits, keep the elbow away from obstacles, or minimize energy. The null space projector is N = I - J⁺J.

CODE: Forward Kinematics and IK for a 2-Link Arm

python
import numpy as np

def forward_kinematics(theta1, theta2, L1=1.0, L2=0.8):
    """FK: joint angles -> end-effector position."""
    x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
    y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
    return np.array([x, y])

def jacobian(theta1, theta2, L1=1.0, L2=0.8):
    """Jacobian: d(x,y)/d(theta1, theta2)."""
    s1, c1 = np.sin(theta1), np.cos(theta1)
    s12, c12 = np.sin(theta1+theta2), np.cos(theta1+theta2)
    return np.array([
        [-L1*s1 - L2*s12, -L2*s12],
        [ L1*c1 + L2*c12,  L2*c12]
    ])

def ik_damped(target, theta_init, L1=1.0, L2=0.8, lam=0.1, steps=100):
    """Damped least-squares IK. Converges even near singularities."""
    theta = np.array(theta_init, dtype='float64')
    for _ in range(steps):
        pos = forward_kinematics(theta[0], theta[1], L1, L2)
        error = target - pos
        if np.linalg.norm(error) < 1e-4: break
        J = jacobian(theta[0], theta[1], L1, L2)
        # Damped pseudoinverse: J^T (J J^T + lambda^2 I)^-1
        JJT = J @ J.T + lam**2 * np.eye(2)
        dtheta = J.T @ np.linalg.solve(JJT, error)
        theta += dtheta * 0.5  # step size for stability
    return theta

# Example: reach target at (1.2, 0.5)
target = np.array([1.2, 0.5])
angles = ik_damped(target, [0.5, 0.5])
result = forward_kinematics(angles[0], angles[1])
print(f"Target: {target}, Reached: {result}, Error: {np.linalg.norm(target-result):.6f}")

DEBUG: Kinematics Failures

FRONTIER: Learned Kinematics

IKFlow uses normalizing flows to learn the full IK solution manifold — given a target, it samples from ALL valid joint configurations, not just one. This is useful for motion planning where you need diverse IK solutions to find collision-free paths. Differentiable FK enables end-to-end learning: backpropagate through the kinematic chain to optimize grasp poses jointly with control parameters.

Chapter 5: Motion Planning — Getting from A to B

CONCEPT: Configuration Space and Sampling-Based Planners

The robot arm lives in configuration space (C-space): the space of all possible joint angles. A 6-DOF arm has a 6-dimensional C-space. An obstacle in the 3D workspace becomes a complex, high-dimensional forbidden region in C-space. Motion planning finds a path through C-space that avoids these forbidden regions.

Why not just plan in workspace (3D)? Because the arm has width. Two workspace paths that look identical may differ dramatically in C-space — one requires the elbow to pass through a shelf, the other doesn't. C-space captures the full geometry of the arm, not just its tip.

RRT (Rapidly-exploring Random Trees) is the workhorse of robotics planning. It grows a tree from the start configuration by repeatedly: (1) sample a random configuration, (2) find the nearest node in the tree, (3) extend toward the sample by a step size, (4) check the extension for collisions, (5) if collision-free, add the new node. RRT is probabilistically complete — given enough samples, it will find a path if one exists.

RRT-Connect (bidirectional RRT) grows trees from BOTH the start and goal. When the trees meet, you have a path. This is dramatically faster for narrow-passage problems because both trees are exploring simultaneously.

Optimization-based planners (CHOMP, TrajOpt, STOMP) start with an initial trajectory (e.g., straight line in C-space) and iteratively deform it to avoid obstacles while minimizing smoothness/energy cost. These produce smoother paths than RRT but can get stuck in local minima.

DESIGN: The Planning Hierarchy

Typical pipeline: plan a coarse path with RRT (~100 waypoints, 200ms), smooth it with trajectory optimization (~20 waypoints, 50ms), then track it with a PD controller at 1kHz. The planner doesn't need to produce a perfect trajectory — just a collision-free waypoint sequence. The optimizer handles smoothness.

CODE: RRT from Scratch

python
import numpy as np

class RRT:
    def __init__(self, start, goal, bounds, obstacles, step=0.3):
        self.start = np.array(start)
        self.goal = np.array(goal)
        self.bounds = bounds       # [(lo, hi), (lo, hi)]
        self.obstacles = obstacles # list of (center, radius)
        self.step = step
        self.nodes = [self.start.copy()]
        self.parents = [-1]

    def sample(self):
        if np.random.rand() < 0.1:  # 10% goal bias
            return self.goal.copy()
        return np.array([np.random.uniform(lo, hi) for lo, hi in self.bounds])

    def nearest(self, point):
        dists = [np.linalg.norm(n - point) for n in self.nodes]
        return np.argmin(dists)

    def steer(self, from_pt, to_pt):
        diff = to_pt - from_pt
        dist = np.linalg.norm(diff)
        if dist < self.step: return to_pt.copy()
        return from_pt + diff / dist * self.step

    def collision_free(self, p1, p2, n_checks=10):
        for t in np.linspace(0, 1, n_checks):
            pt = p1 + t * (p2 - p1)
            for center, radius in self.obstacles:
                if np.linalg.norm(pt - center) < radius:
                    return False
        return True

    def plan(self, max_iter=2000):
        for _ in range(max_iter):
            rand = self.sample()
            near_idx = self.nearest(rand)
            new = self.steer(self.nodes[near_idx], rand)
            if self.collision_free(self.nodes[near_idx], new):
                self.nodes.append(new)
                self.parents.append(near_idx)
                if np.linalg.norm(new - self.goal) < self.step:
                    return self._trace(len(self.nodes)-1)
        return None  # failed

    def _trace(self, idx):
        path = []
        while idx != -1:
            path.append(self.nodes[idx])
            idx = self.parents[idx]
        return path[::-1]

DEBUG: Planning Failures

Worked Example: RRT vs RRT-Connect Performance

Consider a 2D workspace with a narrow passage (10% of workspace width). How many samples does each algorithm need?

RRT-Connect is 3-8x faster in narrow passages because both trees grow toward the gap from opposite sides, effectively halving the passage length each tree must discover. This is why bidirectional search is the default in production systems (OMPL defaults to RRT-Connect).

After finding a path, shortcutting tries connecting non-adjacent waypoints. For each pair (i, j) where j > i+1, check if the direct connection is collision-free. If so, remove all intermediate waypoints. This typically reduces path length by 30-50% and removes the characteristic "zigzag" of RRT paths.

FRONTIER: Neural and Diffusion-Based Planning

Diffusion Policy treats planning as denoising: start with a random trajectory, iteratively denoise it into a collision-free, task-completing path. It naturally handles multi-modality (multiple valid plans). MPNet learns a sampling distribution from successful plans, making RRT find paths faster in familiar environments. STORM uses parallel trajectory optimization on GPU to evaluate thousands of candidate trajectories simultaneously.

Chapter 6: Control — Making the Robot Move Precisely

CONCEPT: PID and Beyond

PID control is the foundation. The controller computes a torque based on three error terms: Proportional (how far from target), Integral (accumulated past error), Derivative (how fast the error is changing):

P alone gives fast response but overshoots. D damps the oscillation. I eliminates steady-state error (e.g., gravity pulling the arm down). The art is tuning: too much P = oscillation, too much D = sluggish, too much I = windup and overshoot on step changes.

Impedance control makes the robot behave like a mass-spring-damper system in task space. Instead of commanding a position, you command a dynamic relationship between force and displacement:

where M_d, B_d, K_d are the desired inertia, damping, and stiffness. Low stiffness = compliant (good for contact tasks like wiping a table). High stiffness = stiff (good for precise positioning). This is critical for safe human-robot interaction — a stiff controller will hurt people, a compliant one will give way.

Computed torque (inverse dynamics) uses the robot's dynamic model to cancel out nonlinear effects (gravity, Coriolis, centrifugal forces), then applies a simple linear controller on top:

This gives perfect tracking IF the model (M, C, g) is perfect. In practice, the model is always wrong — which is why robust and adaptive control exist.

DESIGN: Control Hierarchy for Contact Tasks

CODE: PID Controller with Anti-Windup

python
class PIDController:
    def __init__(self, kp, ki, kd, max_integral=10.0, max_output=50.0):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.max_integral = max_integral  # anti-windup clamp
        self.max_output = max_output
        self.integral = 0.0
        self.prev_error = 0.0

    def compute(self, error, dt):
        # Proportional
        P = self.kp * error

        # Integral with anti-windup
        self.integral += error * dt
        self.integral = np.clip(self.integral, -self.max_integral, self.max_integral)
        I = self.ki * self.integral

        # Derivative (on error, not on measurement to avoid setpoint kick)
        D = self.kd * (error - self.prev_error) / dt if dt > 0 else 0
        self.prev_error = error

        output = P + I + D
        return np.clip(output, -self.max_output, self.max_output)

# Simulate a 1-DOF joint with PID
pid = PIDController(kp=20, ki=5, kd=8)
theta, omega = 0.0, 0.0  # current angle, angular velocity
target = 1.0  # desired angle (radians)
inertia = 0.5  # kg*m^2
dt = 0.001     # 1kHz

for i in range(3000):  # 3 seconds
    error = target - theta
    torque = pid.compute(error, dt)
    alpha = torque / inertia - 0.5 * omega  # + viscous friction
    omega += alpha * dt
    theta += omega * dt

DEBUG: Control Failures

Worked Example: Tuning PID for a Robot Joint

A common tuning procedure (Ziegler-Nichols): (1) Set K_i = 0, K_d = 0. (2) Increase K_p until the system oscillates at a constant amplitude — this is the ultimate gain K_u. (3) Measure the oscillation period T_u. (4) Set gains according to the table:

For our simulated joint (inertia = 0.5 kg m², friction = 0.5): K_u ≈ 45, T_u ≈ 0.45s. Ziegler-Nichols PID: K_p = 27, K_i = 120, K_d = 1.5. This gives aggressive response with ~15% overshoot. For robotics, we usually want less overshoot (safer), so use 70% of K_p and 150% of K_d: K_p = 19, K_i = 120, K_d = 2.3.

FRONTIER: Learned Controllers

Residual policy learning combines a classical controller with a learned correction: τ = τ_PID + π_θ(state). The classical controller handles the known dynamics, and the neural network learns to correct for model mismatch and unmodeled effects. This converges much faster than learning from scratch because the residual is small. Adaptive control estimating parameters online is being replaced by learned adaptation — a meta-learned policy that adapts to new dynamics in a few timesteps.

Chapter 7: Perception for Robotics — Seeing the World

CONCEPT: Depth Sensing and Object Understanding

A robot needs to know what's in front of it and where things are in 3D. Depth cameras provide this, but each type has different tradeoffs:

For tabletop manipulation (the Apple SPG use case), structured light (Intel RealSense D435) or stereo gives dense depth at 30fps with sub-millimeter accuracy at close range.

The perception pipeline for manipulation:

Latency budget: the entire pipeline must run in under 100ms to keep up with the control loop. Perception at 30Hz, planning at 10Hz, control at 1kHz.

CODE: Point Cloud Processing Pipeline

python
import open3d as o3d
import numpy as np

# 1. Load point cloud from depth image
pcd = o3d.io.read_point_cloud("scene.ply")

# 2. Downsample with voxel grid (0.005m = 5mm voxels)
pcd = pcd.voxel_down_sample(0.005)

# 3. Remove table plane with RANSAC
plane, inliers = pcd.segment_plane(distance_threshold=0.01,
                                    ransac_n=3, num_iterations=1000)
objects = pcd.select_by_index(inliers, invert=True)

# 4. Cluster objects with DBSCAN
labels = np.array(objects.cluster_dbscan(eps=0.02, min_points=50))
n_clusters = labels.max() + 1
print(f"Found {n_clusters} objects")

# 5. Get bounding box for each cluster
for i in range(n_clusters):
    cluster = objects.select_by_index(np.where(labels == i)[0])
    bbox = cluster.get_axis_aligned_bounding_box()
    center = bbox.get_center()
    extent = bbox.get_extent()
    print(f"Object {i}: center={center}, size={extent}")

DEBUG: Perception Pipeline Failures

FRONTIER: Foundation Models for Robot Perception

SAM2 (Segment Anything Model 2) gives zero-shot segmentation — point at any object and get a perfect mask, no training needed. FoundationPose estimates 6-DOF pose from a single reference image, no CAD model required. SpatialVLM grounds language in 3D space: "pick up the red cup on the left" becomes a 3D coordinate. These foundation models are replacing the hand-crafted perception pipelines above with general-purpose perception that works out of the box.

Chapter 8: Sim-to-Real Transfer — SHOWCASE

This is THE critical skill. Most robot learning happens in simulation. Getting it to work on real hardware is where careers are made. The reality gap — the difference between simulated and real physics, perception, and dynamics — determines whether your policy transfers or falls flat.

CONCEPT: Sources of the Reality Gap

DESIGN: The Sim-to-Real Pipeline

CODE: Domain Randomization Implementation

python
import numpy as np
import mujoco

class DomainRandomizer:
    """Randomize simulation parameters each episode for robust transfer."""
    def __init__(self, model):
        self.model = model
        # Store default values
        self.default_friction = model.geom_friction.copy()
        self.default_mass = model.body_mass.copy()
        self.default_damping = model.dof_damping.copy()

    def randomize(self):
        # Friction: multiply by uniform [0.5, 1.5]
        scale = np.random.uniform(0.5, 1.5, self.default_friction.shape)
        self.model.geom_friction[:] = self.default_friction * scale

        # Mass: +/- 20%
        scale = np.random.uniform(0.8, 1.2, self.default_mass.shape)
        self.model.body_mass[:] = self.default_mass * scale

        # Joint damping: multiply by [0.5, 2.0]
        scale = np.random.uniform(0.5, 2.0, self.default_damping.shape)
        self.model.dof_damping[:] = self.default_damping * scale

        # Actuator delay: 0-20ms (added in step function)
        self.action_delay = np.random.randint(0, 21)  # ms

        # Observation noise: Gaussian with std 0-0.02 rad
        self.obs_noise_std = np.random.uniform(0, 0.02)

        # Recompute derived quantities after changing parameters
        mujoco.mj_setConst(self.model, mujoco.MjData(self.model))

    def add_obs_noise(self, obs):
        return obs + np.random.randn(*obs.shape) * self.obs_noise_std

# Usage: call randomizer.randomize() at each episode reset
# The policy trained over thousands of randomized envs
# learns to be robust to parameter variation

DEBUG: Sim-to-Real Failures (The Systematic Approach)

Worked Example: Diagnosing the Reality Gap

You trained a reaching policy in MuJoCo with 95% success. On the real Franka Panda, it achieves 35%. Here's the systematic debugging trace:

The key insight: each fix addressed a specific measured gap, not a generic "add more randomization." System identification (steps 1-3) moved the sim center closer to reality. Domain randomization (steps 4-5) covered the remaining uncertainty. This ordered approach converges in 3-5 iterations, while blind DR tuning can take 20+ iterations.

FRONTIER: Closing the Gap Automatically

Auto-DR (Automatic Domain Randomization) uses a secondary RL agent to learn the randomization ranges that maximize transfer. Instead of hand-tuning "friction ∈ [0.5, 1.5]", the system discovers the right ranges automatically. Real-to-Sim-to-Real scans the real environment (with NeRF or 3D Gaussian Splatting), builds a digital twin, trains a policy in the twin, and deploys. Foundation models (pre-trained visual features like DINOv2) transfer better than raw pixels because they've already learned to be invariant to visual domain shifts.

python
import numpy as np
from multiprocessing import Process, Pipe

class VectorizedEnv:
    def __init__(self, env_fn, num_envs):
        self.num_envs = num_envs
        self.envs = [env_fn() for _ in range(num_envs)]
        # Get obs/action dims from first env
        self.obs_dim = self.envs[0].observation_space.shape[0]

    def reset(self):
        obs = np.stack([env.reset()[0] for env in self.envs])
        return obs  # (num_envs, obs_dim)

    def step(self, actions):
        obs_list, rew_list, done_list, info_list = [], [], [], []
        for i, (env, act) in enumerate(zip(self.envs, actions)):
            obs, rew, terminated, truncated, info = env.step(act)
            done = terminated or truncated
            if done:
                # Store terminal obs BEFORE reset
                info['terminal_obs'] = obs.copy()
                obs = self._auto_reset(i)
            obs_list.append(obs)
            rew_list.append(rew)
            done_list.append(done)
            info_list.append(info)
        return (np.stack(obs_list), np.array(rew_list),
                np.array(done_list), info_list)

    def _auto_reset(self, idx):
        obs, _ = self.envs[idx].reset()
        return obs

Chapter 9: Robot Learning — RL, Imitation, Diffusion Policies

CONCEPT: Three Paradigms for Teaching Robots

Reinforcement learning (RL) learns from trial and error. The robot tries random actions, receives a reward signal, and gradually learns which actions lead to high rewards. For robotics, RL happens in simulation (millions of episodes, each taking seconds) because real robots are too slow and expensive. The dominant algorithm is PPO (Proximal Policy Optimization) — stable, scalable, and well-understood.

Imitation learning learns from demonstrations. A human teleoperates the robot (or records hand motions), and the policy learns to map observations to actions that mimic the expert. Behavior cloning (BC) is the simplest form: supervised learning on (observation, action) pairs. The problem: compounding errors. A small deviation from the demonstration puts the robot in a state it never saw during training, leading to further deviation. DAgger fixes this by interactively querying the expert in states the policy visits.

Diffusion policies model the action distribution as a denoising diffusion process. Starting from Gaussian noise, the policy iteratively denoises to produce an action trajectory. The key advantage: multi-modality. If there are multiple valid ways to fold a shirt (left-first or right-first), BC averages them (producing garbage), while diffusion policies sample from one mode or the other.

Action chunking predicts N future actions at once (typically 8-32 timesteps). This reduces compounding errors in BC and provides temporal consistency in diffusion policies. ACT (Action Chunking with Transformers) combines a transformer encoder with a CVAE decoder to produce action chunks.

DESIGN: When to Use What

DESIGN: The Reward Engineering Problem

Designing good reward functions is the hardest part of RL for robotics. A sparse reward (+1 when the task succeeds, 0 otherwise) is easy to define but almost impossible to learn from — the robot must randomly stumble into success before it can learn anything. A dense reward provides signal at every timestep but can lead to reward hacking (the robot finds unexpected shortcuts that maximize reward without actually completing the task).

The practical approach is reward shaping — adding intermediate rewards that guide the robot toward the goal without specifying exactly how to get there:

python
def reaching_reward(obs, goal):
    dist = np.linalg.norm(obs["ee_pos"] - goal)
    # Dense: negative distance (closer = better)
    r = -dist
    # Bonus for getting close (but not for staying close)
    if dist < 0.05: r += 1.0
    # Penalty for high velocity at contact (don't slam into things)
    if dist < 0.1: r -= 0.1 * np.linalg.norm(obs["ee_vel"])
    # Penalty for joint limits (stay in safe range)
    r -= 0.01 * np.sum(np.maximum(0, np.abs(obs["q"]) - 0.9 * obs["q_lim"]))
    return r

Common reward engineering pitfalls: (1) Reward for being close to the goal instead of reaching it → the robot hovers near the goal forever. (2) No penalty for collisions → the robot takes the shortest path through obstacles. (3) Reward for grasping force → the robot crushes delicate objects. (4) No time penalty → the robot moves infinitely slowly to maximize precision.

CODE: PPO Training Loop for a Reaching Task

python
import torch
import torch.nn as nn
import numpy as np

class Policy(nn.Module):
    def __init__(self, obs_dim, act_dim):
        super().__init__()
        self.net = nn.Sequential(
            nn.Linear(obs_dim, 64), nn.Tanh(),
            nn.Linear(64, 64), nn.Tanh())
        self.mean = nn.Linear(64, act_dim)
        self.log_std = nn.Parameter(torch.zeros(act_dim))

    def forward(self, obs):
        h = self.net(obs)
        mean = self.mean(h)
        std = self.log_std.exp()
        return torch.distributions.Normal(mean, std)

def ppo_update(policy, optimizer, states, actions, advantages, old_log_probs,
               clip_eps=0.2, epochs=4):
    for _ in range(epochs):
        dist = policy(states)
        log_probs = dist.log_prob(actions).sum(-1)
        ratio = (log_probs - old_log_probs).exp()
        # PPO clipped objective
        surr1 = ratio * advantages
        surr2 = ratio.clamp(1 - clip_eps, 1 + clip_eps) * advantages
        loss = -torch.min(surr1, surr2).mean()
        optimizer.zero_grad(); loss.backward(); optimizer.step()

# Reward function for reaching: -distance to target
def reward(end_effector_pos, target_pos):
    dist = np.linalg.norm(end_effector_pos - target_pos)
    if dist < 0.02: return 10.0   # bonus for reaching target
    return -dist  # shaped reward: closer = better

DEBUG: Learning Failures

FRONTIER: Foundation Policies

RT-2 (Robotic Transformer 2) is a VLA (Vision-Language-Action) model: a 55B parameter model that takes images and language instructions and outputs robot actions. Trained on internet-scale data + robot demonstrations. pi-0 uses flow matching (continuous-time analog of diffusion) for action generation, achieving state-of-the-art dexterous manipulation. Octo is an open-source generalist policy pre-trained on the Open X-Embodiment dataset (1M+ robot episodes). LeRobot (Hugging Face) provides a unified framework for training and deploying robot learning algorithms.

Chapter 10: Systems Integration — ROS2, Middleware, Testing

CONCEPT: ROS2 Architecture

ROS2 (Robot Operating System 2) is the middleware that connects all robot software components. It's not an operating system — it's a communication framework. Each component runs as a node (a process) that communicates via topics (pub/sub), services (request/reply), or actions (long-running tasks with feedback).

Key concepts:

QoS (Quality of Service) profiles determine message delivery guarantees. For sensor data: BEST_EFFORT (drop old messages if subscriber is slow). For control commands: RELIABLE (guarantee delivery). For safety signals: RELIABLE + TRANSIENT_LOCAL (new subscribers get last message). Getting QoS wrong is one of the most common causes of mysterious ROS2 bugs.

DESIGN: The Real-Time Control Stack

The 1kHz control loop runs on a real-time OS (PREEMPT_RT Linux or Xenomai) in a dedicated thread. It communicates with ROS2 via lock-free shared memory. The ROS2 nodes run in a separate executor. This separation is critical: ROS2 is NOT real-time by default. A garbage collection pause in a Python node must never affect the safety-critical control loop.

CODE: ROS2 Control Node (C++)

cpp
// ROS2 lifecycle node: subscribes to joint states, publishes torques
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>

class ArmController : public rclcpp::Node {
public:
  ArmController() : Node("arm_controller") {
    // Subscribe to joint states (RELIABLE for control)
    auto qos = rclcpp::QoS(10).reliable();
    sub_ = create_subscription<sensor_msgs::msg::JointState>(
      "joint_states", qos,
      [this](const sensor_msgs::msg::JointState::SharedPtr msg) {
        // PD control at 1kHz
        std::vector<double> torques(msg->position.size());
        for (size_t i = 0; i < torques.size(); i++) {
          double err = target_[i] - msg->position[i];
          double derr = -msg->velocity[i];
          torques[i] = kp_ * err + kd_ * derr;
        }
        auto cmd = std_msgs::msg::Float64MultiArray();
        cmd.data = torques;
        pub_->publish(cmd);
      });
    pub_ = create_publisher<std_msgs::msg::Float64MultiArray>(
      "torque_commands", qos);
  }
private:
  double kp_ = 20.0, kd_ = 5.0;
  std::vector<double> target_ = {0, 0.5, -0.3, 0, 0, 0};
  // subscribers and publishers
  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr sub_;
  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr pub_;
};

DEBUG: ROS2 Integration Issues

Worked Example: ROS2 Launch File for a Manipulation System

python
# launch/manipulation.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        # Camera driver (30Hz, BEST_EFFORT)
        Node(package="realsense2_camera", executable="realsense2_camera_node",
             parameters=[{"depth_module.profile": "640x480x30"}]),
        # Perception (subscribes to camera, publishes object_poses)
        Node(package="perception", executable="pose_estimator",
             parameters=[{"model": "foundation_pose", "gpu": 0}]),
        # Motion planner (action server, RELIABLE)
        Node(package="planning", executable="motion_planner",
             parameters=[{"algorithm": "rrt_connect", "timeout_ms": 500}]),
        # Controller (1kHz real-time, RELIABLE)
        Node(package="control", executable="impedance_controller",
             parameters=[{"rate": 1000, "stiffness": 1000.0}]),
        # Safety monitor (1kHz, RELIABLE + TRANSIENT_LOCAL)
        Node(package="safety", executable="force_monitor",
             parameters=[{"max_force_N": 30.0, "rate": 1000}]),
    ])

Key design decisions in the launch file: perception and planning are separate processes (crash isolation — if perception crashes, the controller still holds position). The safety monitor runs at 1kHz with TRANSIENT_LOCAL QoS so that new subscribers (like a UI) immediately get the last safety status. All parameters are exposed as ROS2 parameters for runtime tuning without recompilation.

FRONTIER: Foundation Models Changing the Stack

VLA models (Vision-Language-Action) like RT-2 and pi-0 are collapsing the traditional perception→planning→control pipeline into a single neural network. Instead of separate nodes for each, one model takes images and language instructions and outputs motor commands directly. This changes the ROS2 architecture: fewer nodes, simpler data flow, but new challenges around real-time inference (running a 7B model at 10Hz requires an onboard GPU).

Chapter 11: Full Pipeline Simulation — SHOWCASE #2

This is the capstone. An end-to-end robotics pipeline running in your browser: perception detects objects, the planner finds a path, the controller executes it, and the physics engine simulates the result. You control every stage and can break any part to see how failures cascade.

The Pipeline

Try Breaking It

The simulation starts with reasonable defaults that produce successful reaching. Your job is to understand each failure mode:

Chapter 12: Interview Arsenal

1. Cheat Sheet Table

2. System Design Questions

3. Coding Drills

4. Debugging Scenarios

5. Classical vs Learned Table

6. Recommended Reading

Book: Robotics: Modelling, Planning and Control by Siciliano, Sciavicco, Villani, Oriolo. The bible for kinematics, dynamics, and control. Chapters 3-7 cover everything in this lesson's Ch 1, 4, 5, 6.

Papers:

Repos:

7. Related Lessons

Continue your preparation with these related Gleam lessons: