Robotics

Contents

• Introduction and overview
• 0. Review: least squares, rotations, integration and Gaussian conditioning
• 1. Modelling and simulation of robotic systems
• 2. Motion planning
• 3. Inverse problems for robot control
• 4. Optimal control and trajectory optimisation
• 5. Perception and estimation
• 6. Reinforcement learning for robotics
• 7. Legged locomotion
• 8. Responsible robotics
• 9. Research directions and open problems
• Exercises
• Projects and assessment
• References

Introduction and overview

Audience: a student with an M1 in mathematics, computer science or physics who has never modelled a robot, and who wants to understand how modelling, planning, control, estimation and learning fit together on a machine that moves in the physical world.

Empirical first: integrate the frictionless oscillator \ddot x = -x with the explicit Euler scheme and a step of 0.1 s, and after 1,000 steps its energy has been multiplied by about 21,000; the semi-implicit scheme, one line different, keeps it within 5.3 % (ch. 0). A humanoid whose centre of mass is 0.8 m high and which is pushed at 0.5 m/s must place its next foot about 14 cm ahead of its centre of mass, within a few tenths of a second, or fall (ch. 7).

Both facts are about dynamics, and dynamics is the thread of the course. A robot is a mechanical system whose state obeys Newton’s laws, whose sensors are noisy, and whose motions must avoid obstacles and respect contact. Each chapter turns one of these constraints into a mathematical problem — a least-squares problem, a graph search, an optimal control problem, a filtering problem, a stochastic optimisation — and into an algorithm that runs on a real robot.

The course leans on Convex Optimization and Applications in Machine Learning, which the official page recommends taking at the same time. Its estimation chapter meets 3D Computer Vision and Introduction to Probabilistic Graphical Models; its learning chapter opens the door to Reinforcement Learning in semester 2.

What you will be able to do

• Model a robot as a kinematic tree: compute its forward kinematics, its Jacobians and its Lagrangian dynamics, and simulate it with an integrator suited to mechanics.
• Plan collision-free paths with A*, probabilistic roadmaps and rapidly-exploring random trees, and bound the failure probability of a sampling-based planner.
• Solve inverse kinematics by damped least squares, exploit redundancy with null-space projections, and write whole-body control as a quadratic program.
• Design computed-torque and PD-plus-gravity controllers and prove their stability with a Lyapunov function.
• Derive the linear-quadratic regulator from dynamic programming, apply Pontryagin’s principle, and optimise trajectories with iLQR and model predictive control.
• Estimate the state of a robot with Kalman and extended Kalman filters, explain what invariant errors on Lie groups bring to them, and register two point clouds.
• Train a policy by policy gradient, relate it to optimal control, and quantify the simulation-to-reality gap.
• Generate a walking pattern with the linear inverted pendulum, the zero-moment point and the capture point.
• Build a safety filter with a control barrier function, and argue about the responsibilities of robotics research.

Official objectives and syllabus (2026–2027)

The course description on master-mva.com observes that a large part of the recent progress in robotics has come with advances in machine learning, optimisation and computer vision. The course introduces the general conceptual tools behind these advances, and shows how they let robots perceive the world and perform tasks that go beyond factory automation, from highly dynamic somersaults to mountain hikes.

It covers modelling and simulation of robotic systems, motion planning, inverse problems for motion control, optimal control and reinforcement learning. It includes practical exercises with state-of-the-art robotics libraries, and a broader reflection on our responsibilities when doing research and innovation in robotics.

master-mva.com organises the course in 8 sessions of 3 h and a 4 h final exam held as a student seminar, and lists seven themes. The Université Paris-Saclay course detail adds only the nature of the assessment; it lists no programme items. The table maps every official item to a section of this page.

Beyond the syllabus, this page adds control barrier functions as a mathematical tool of safety (ch. 8), the simulation lemma that quantifies the sim-to-real gap (ch. 6), and a chapter of research directions (ch. 9, self-study).

Where this course fits in the programme

Official prerequisites (master-mva.com): undergraduate-level linear algebra, and enough Python for the labs — functions, loops, recursion, data structures. The page recommends following “Convex optimization” by A. d’Aspremont in the same semester. The instructors listed are Justin Carpentier (INRIA), Silvère Bonnabel (Mines Paris), Pierre Brice Wieber (INRIA) and A. Sathya.

• Refreshers — Linear Algebra, Analysis and Optimization (least squares, eigenvalues, Lagrange multipliers), Probability and Statistics (Gaussian vectors, conditioning) and Scientific Python, NumPy and PyTorch (every code block of this page).
• Related courses of the same semester — Convex Optimization and Applications in Machine Learning (the quadratic programs of ch. 3, 4 and 7), 3D Computer Vision (camera models and registration, ch. 5), Object Recognition and Computer Vision, Introduction to Probabilistic Graphical Models and Deep Generative Models (state-space models), Computational Statistics (particle filters), Deep Learning, Geometric Data Analysis and Fundamentals of Reproducible Research and Free Software (ch. 8).
• Courses that build on it in semester 2 — Reinforcement Learning and Sequential Learning (ch. 6), Point Clouds and 3D Modelling (NPM3D) (registration and mapping, ch. 5), Geometry and Shape Spaces (Lie groups), Deep Learning in Practice / Advanced Deep Learning, Regulating AI, Responsible Machine Learning and Artificial Intelligence and the Environment (ch. 8).

Course outline

• Review: least squares, rotations, integration and Gaussian conditioning — the pseudo-inverse and its damped version, equality-constrained quadratic programs, Rodrigues’ formula, explicit and semi-implicit Euler, Gaussian conditioning.
• Modelling and simulation of robotic systems — configuration spaces, rigid motions, product of exponentials, Jacobians and statics, Lagrangian dynamics and its skew-symmetry, rigid-body algorithms and libraries, contact and time stepping.
• Motion planning — the configuration-space formulation, A* and its optimality, probabilistic roadmaps with a completeness bound, RRT and RRT*, from paths to trajectories.
• Inverse problems for robot control — inverse geometry by Levenberg–Marquardt, differential inverse kinematics and redundancy, computed torque, PD plus gravity compensation, task-space quadratic programs.
• Optimal control and trajectory optimisation — Bellman’s principle, the LQR, Pontryagin’s principle, direct transcription, iLQR and DDP, model predictive control and its stability.
• Perception and estimation — Bayes and Kalman filters, the EKF and observability, invariant errors on Lie groups, batch estimation and SLAM, cameras and point-cloud registration.
• Reinforcement learning for robotics — MDPs and their link with optimal control, the policy-gradient theorem, PPO and SAC, the simulation lemma and sim-to-real transfer, imitation learning.
• Legged locomotion — centroidal dynamics, the zero-moment point and the support polygon, the linear inverted pendulum, the capture point, MPC walking pattern generation, whole-body control.
• Responsible robotics — safety standards and control barrier functions, research integrity and statistics of robot trials, a responsible-innovation framework, dual use, environment and work.
• Research directions and open problems — differentiable contact simulation, whole-body MPC, learned locomotion and manipulation, Lie-group estimation, certified learning (self-study).

Session plan

28 h in class = 9 sessions (8 × 3 h + 1 × 4 h). The volume is that of the Université Paris-Saclay programme table (lectures 19 h, tutorials 9 h). master-mva.com organises it as eight sessions of 3 h — 19 h of lectures and 5 h of labs (TP) — and a 4 h final exam held as a student seminar. Counting the labs and the seminar as tutorials gives the 9 h of the table; this reading is ours. The plan below follows that organisation; the content of each session is a proposal.

Level 1 = foundations and first examples, 2 = core models and algorithms, 3 = advanced material close to research; the level never decreases.

Self-study, outside the 28 h (about 3 h in total, plus the project): ch. 9 Research directions and open problems (≈ 3 h, with Ex. 16).

How to study this module

• Read ch. 0 before session 1. Proposition 0.3 (damped least squares), Proposition 0.4 (KKT systems) and Proposition 0.9 (Gaussian conditioning) are used again in ch. 3, 5 and 8.
• After each session, do the exercises of the Practice column; the official weekly homework counts for 20 % of the grade, and the homework suggested in the plan has the right size.
• Rerun each code block and change one parameter at a time: the step of the integrator, the damping of the inverse kinematics, the horizon of the MPC, the exploration noise of the policy.
• Choose the project or the article study early (session 4 at the latest): it counts for 80 % and is presented at the final seminar.
• Python environment: NumPy and SciPy suffice for every block; all data are synthetic, with fixed seeds. For the projects, the open-source libraries Pinocchio, MuJoCo and Crocoddyl are the natural next step.

0. Review: least squares, rotations, integration and Gaussian conditioning

Four M1 tools carry most of the course. Least squares solves inverse kinematics and estimation problems; quadratic programs with equality constraints encode control; rotations describe every rigid body; numerical integration turns equations of motion into simulations.

This chapter states each tool in the form used later, with short proofs. The last section recalls Gaussian conditioning, on which the Kalman filter of ch. 5 rests.

Least squares and the pseudo-inverse

Robotics constantly solves linear systems that have too many or too few equations. A task in the plane constrains two coordinates of a seven-joint arm; twenty camera measurements constrain six pose parameters. The pseudo-inverse handles both cases at once.

Near a rank drop, some \sigma_i is tiny and c_i / \sigma_i explodes. For a robot this means huge joint velocities near a singular configuration (ch. 3). The cure is to penalise the norm of the solution.

The second form is the one used in practice: it inverts an m \times m matrix, and m (the task dimension) is usually smaller than n (the number of joints).

Quadratic programs with equality constraints

Whole-body control (ch. 3), the LQR (ch. 4) and walking pattern generation (ch. 7) all reduce to minimising a convex quadratic under linear constraints.

Inequality constraints (torque limits, friction cones, obstacle half-planes) require active-set or interior-point solvers; they are the subject of the Convex Optimization course and of chapter 16 of Nocedal and Wright’s textbook.

Rotations and the exponential map

With \omega constant, the solution starting at I is the matrix exponential of t\, [\omega]_\times. For skew-symmetric matrices, the exponential has a closed form.

Integrating equations of motion

A simulator advances the state (x, v) of a mechanical system with a fixed step h. Two first-order schemes look almost identical:

• explicit Euler: x_{k+1} = x_k + h v_k and v_{k+1} = v_k + h a(x_k);
• semi-implicit (symplectic) Euler: first v_{k+1} = v_k + h a(x_k), then x_{k+1} = x_k + h v_{k+1}.

For the semi-implicit scheme with h = 0.1 and (x_0, v_0) = (1, 0), the modified energy equals 1. The energy therefore stays, for ever, in the band 0.476 \leq \tfrac12 (x_k^2 + v_k^2) \leq 0.526 , that is within 5.3 % of its initial value 0.5.

For explicit Euler, 1,000 steps multiply the energy by 1.01^{1000} \approx 2.1 \times 10^4. This is the explosion announced in the introduction; the code below confirms both claims and checks Rodrigues’ formula against a general-purpose matrix exponential.

import numpy as np
from scipy.linalg import expm

def hat(w):
    """Skew-symmetric matrix [w]x, so that hat(w) @ v == np.cross(w, v)."""
    return np.array([[0, -w[2], w[1]], [w[2], 0, -w[0]], [-w[1], w[0], 0]])

def rodrigues(w):
    th = np.linalg.norm(w)
    if th < 1e-12:
        return np.eye(3) + hat(w)
    K = hat(w / th)
    return np.eye(3) + np.sin(th) * K + (1 - np.cos(th)) * K @ K

w = np.array([0.3, -0.5, 0.8])
R = rodrigues(w)
print("max |Rodrigues - expm| =", np.abs(R - expm(hat(w))).max())
print("R^T R = I:", np.allclose(R.T @ R, np.eye(3)), "| det R =", round(np.linalg.det(R), 12))

# Explicit vs semi-implicit Euler on x'' = -x, step h = 0.1, 1000 steps
h, n = 0.1, 1000
for scheme in ["explicit", "semi-implicit"]:
    x, v = 1.0, 0.0
    for _ in range(n):
        if scheme == "explicit":
            x, v = x + h * v, v - h * x
        else:
            v = v - h * x
            x = x + h * v
    print(f"{scheme:>13} Euler: energy after {n} steps = {0.5 * (x**2 + v**2):.4f} (initial 0.5)")

The output gives an energy of about 10,480 for explicit Euler and 0.52 for semi-implicit Euler. Physics engines therefore use semi-implicit schemes, which also extend naturally to contact (ch. 1).

Gaussian conditioning

1. Modelling and simulation of robotic systems

Before planning or controlling a robot, one needs its model: which configurations it can take, where its end effector is for a given configuration, and how torques produce accelerations. This chapter builds that model, from geometry to dynamics, and ends with simulation and contact.

Configuration space and rigid motions

Like SO(3), the group SE(3) has an exponential map. A twist \xi = (v, \omega) \in \mathbb{R}^6 describes an infinitesimal screw motion. Its matrix form is \hat\xi = \begin{pmatrix} [\omega]_\times & v \\ 0 & 0 \end{pmatrix} \in \mathbb{R}^{4 \times 4}, and e^{\hat\xi \theta} \in SE(3) is the rigid motion obtained by following the twist during a time \theta.

For a revolute joint of unit axis \omega through the point r, the twist is \xi = (-\omega \times r, \omega). Then e^{\hat\xi \theta} is the rotation of angle \theta about that axis.

Forward kinematics

The formula, due to Brockett, needs no frame per link; Murray, Li and Sastry (chapter 3) build their whole treatment of manipulators on it. For planar arms, elementary trigonometry gives the same result.

Jacobians, singularities and statics

For the arm of Example 1.4, a direct computation gives \det J(q) = l_1 l_2 \sin q_2 . The arm is singular when it is fully stretched (q_2 = 0) or fully folded (q_2 = \pi). At q = (0.4, 1.1) the determinant is 0.713.

The same transpose appears in dynamics: contact forces enter the equations of motion through J_c^\top \lambda, where J_c is the Jacobian of the contact point.

Lagrangian dynamics

Let T(q, \dot q) = \tfrac12 \dot q^\top M(q) \dot q be the kinetic energy of the robot and V(q) its potential energy. The matrix M(q) is the mass matrix. For a robot made of point masses m_i located at p_i = f_i(q), it is M(q) = \sum_i m_i\, J_i(q)^\top J_i(q), since the kinetic energy is \sum_i \tfrac12 m_i \|J_i \dot q\|^2. Rigid links add the rotational terms J_{\omega,i}^\top I_i J_{\omega,i} in the same way.

Proposition 1.8 says that the joints inject exactly the power \dot q^\top \tau. It is the key step of the stability proofs of ch. 3, and a sharp test for a simulator: with \tau = 0 the energy must stay constant.

Rigid-body algorithms and libraries

Forming M(q) and C(q, \dot q) symbolically is hopeless beyond a few joints. Three recursive algorithms over the kinematic tree, collected in Featherstone’s monograph, do the work instead:

• the recursive Newton–Euler algorithm computes the inverse dynamics \tau = \mathrm{ID}(q, \dot q, \ddot q) in O(n) operations — a forward pass for velocities and accelerations, a backward pass for forces;
• the composite rigid-body algorithm computes M(q) in O(n^2);
• the articulated-body algorithm computes the forward dynamics \ddot q in O(n), without forming or inverting M.

The open-source library Pinocchio (Carpentier and coauthors, 2019) implements these algorithms together with their analytical derivatives, which ch. 4 needs for trajectory optimisation. The MuJoCo simulator (Todorov, Erez and Tassa, 2012) adds contact. Both have Python bindings and are the natural tools for the projects.

The official course includes practical exercises with such state-of-the-art libraries. In Pinocchio’s Python interface, pinocchio.buildModelFromUrdf loads a robot description and model.createData() creates a workspace. The three algorithms above are then the functions rnea, crba and aba, which take the model, the workspace and the joint vectors. MuJoCo loads a model with mujoco.MjModel.from_xml_path and advances it by one time step with mujoco.mj_step. The code of this page re-implements the same computations in NumPy on small examples, so that every block runs without these libraries.

Contact and simulation

Walking and grasping are made of contacts that open and close. A contact is unilateral: the bodies cannot interpenetrate, and the ground can push but not pull.

The conditions are non-smooth: velocities jump at impacts. Time-stepping schemes (Stewart and Trinkle, 1996) therefore work with impulses over one step. At each step they solve a complementarity problem for the contact impulses, then update the velocities and the positions in the semi-implicit order of ch. 0. MuJoCo relaxes this problem into a convex one, which makes it fast and always solvable, at the price of small artificial penetrations.

Without contact, any good integrator works. The code below takes the two-link arm of Example 1.4 with point masses at the elbow and at the tip, and checks the Jacobian against finite differences. It builds M(q) = \sum_i m_i J_i^\top J_i, lets the arm fall freely for 5 s, and monitors the energy (Proposition 1.8).

import numpy as np

l1, l2, m1, m2, g = 1.0, 0.8, 1.0, 0.5, 9.81   # lengths (m), point masses (kg)

def fk(q):
    """Position of the end effector (angles measured from the x axis)."""
    return np.array([l1 * np.cos(q[0]) + l2 * np.cos(q[0] + q[1]),
                     l1 * np.sin(q[0]) + l2 * np.sin(q[0] + q[1])])

def jacobian(q):
    s1, c1 = np.sin(q[0]), np.cos(q[0])
    s12, c12 = np.sin(q[0] + q[1]), np.cos(q[0] + q[1])
    return np.array([[-l1 * s1 - l2 * s12, -l2 * s12],
                     [ l1 * c1 + l2 * c12,  l2 * c12]])

q = np.array([0.4, 1.1])
eps = 1e-6
J_fd = np.column_stack([(fk(q + eps * e) - fk(q - eps * e)) / (2 * eps) for e in np.eye(2)])
print("max |J - J_fd| =", f"{np.abs(jacobian(q) - J_fd).max():.1e}",
      "| det J =", round(np.linalg.det(jacobian(q)), 6), "| l1 l2 sin q2 =", round(l1 * l2 * np.sin(q[1]), 6))

def mass_matrix(q):
    J1 = np.array([[-l1 * np.sin(q[0]), 0.0], [l1 * np.cos(q[0]), 0.0]])  # elbow mass
    return m1 * J1.T @ J1 + m2 * jacobian(q).T @ jacobian(q)            # M = sum m_i J_i^T J_i

def bias(q, dq):
    """Coriolis, centrifugal and gravity terms h(q, dq)."""
    k = m2 * l1 * l2 * np.sin(q[1])
    cor = np.array([-k * (2 * dq[0] * dq[1] + dq[1] ** 2), k * dq[0] ** 2])
    grav = np.array([(m1 + m2) * g * l1 * np.cos(q[0]) + m2 * g * l2 * np.cos(q[0] + q[1]),
                     m2 * g * l2 * np.cos(q[0] + q[1])])
    return cor + grav

def energy(q, dq):
    return 0.5 * dq @ mass_matrix(q) @ dq + m1 * g * l1 * np.sin(q[0]) + m2 * g * fk(q)[1]

# Passive motion (tau = 0) during 5 s: semi-implicit Euler vs Runge-Kutta 4
def rhs(s):
    return np.concatenate([s[2:], np.linalg.solve(mass_matrix(s[:2]), -bias(s[:2], s[2:]))])

s0 = np.array([0.4, 1.1, 0.0, 0.0])
E0 = energy(s0[:2], s0[2:])
for h in [1e-3, 1e-4]:
    s, dev = s0.copy(), 0.0
    for _ in range(round(5 / h)):
        s[2:] += h * rhs(s)[2:]          # velocity first ...
        s[:2] += h * s[2:]               # ... then position with the new velocity
        dev = max(dev, abs(energy(s[:2], s[2:]) - E0) / E0)
    print(f"semi-implicit Euler, h = {h:g} s: max relative energy error {dev:.1e}")
s, h = s0.copy(), 1e-3
for _ in range(5000):
    k1 = rhs(s); k2 = rhs(s + h / 2 * k1); k3 = rhs(s + h / 2 * k2); k4 = rhs(s + h * k3)
    s = s + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
print(f"Runge-Kutta 4,       h = 0.001 s: relative energy error {abs(energy(s[:2], s[2:]) - E0) / E0:.1e}")

The finite differences agree with the analytic Jacobian to about 10^{-10}. The semi-implicit scheme keeps the energy within 3.6 % at h = 1 ms and within 0.35 % at h = 0.1 ms: the error is of first order, but bounded.

Runge–Kutta 4 conserves the energy to 2 \times 10^{-10}, which confirms that the Coriolis terms are right. Yet contact simulators keep first-order schemes, because high-order schemes lose their accuracy at every impact.

2. Motion planning

Motion planning finds a collision-free motion from a start configuration to a goal configuration. The key idea is to plan in the configuration space of ch. 1, where the robot is a point, rather than in the workspace, where it is a complicated body.

The planning problem

Complete planning is hard. Reif (1979) proved that the generalised “mover’s problem” is PSPACE-hard, and the exact algorithms that exist are exponential in the number of degrees of freedom. Practical planners give up exact completeness: they discretise the space (resolution completeness) or sample it (probabilistic completeness).

Search on a graph: A*

On a grid or a roadmap, planning is a shortest-path problem on a graph with non-negative edge costs c(u, v). Dijkstra’s algorithm expands vertices by increasing cost-to-come g(v); A* (Hart, Nilsson and Raphael, 1968) expands them by g(v) + h(v), where the heuristic h estimates the cost-to-go.

The Euclidean distance to the goal is a consistent heuristic on any geometric graph, by the triangle inequality. Grids, however, scale as (1/\delta)^d with the resolution \delta and the dimension d: a seven-joint arm needs another idea.

Sampling-based planning: PRM and RRT

The probabilistic roadmap (PRM; Kavraki, Švestka, Latombe and Overmars, 1996) samples N configurations uniformly in \mathcal{Q}_{\mathrm{free}}. It connects every pair at distance at most r whose straight segment is collision-free, and answers queries by graph search. Only two primitives are needed: a collision test for a configuration and one for a segment.

The rapidly-exploring random tree (RRT; LaValle, 1998; LaValle and Kuffner, 2001) grows a single tree from q_s. At each iteration it samples q_{\mathrm{rand}} and finds the nearest tree vertex q_{\mathrm{near}}. It moves from q_{\mathrm{near}} towards q_{\mathrm{rand}} by at most a step \eta, and adds the new vertex if the segment is free.

Large Voronoi cells attract samples, so the tree rushes into unexplored regions. RRT is probabilistically complete but not optimal. Karaman and Frazzoli (2011) proved that, with probability one, its solution does not converge to an optimal path. Their RRT* rewires the tree within a radius shrinking like (\log n / n)^{1/d}, and is asymptotically optimal.

From paths to trajectories

A sampled path zigzags. Shortcutting replaces a sub-path by a straight segment whenever that segment is free. Then a time scaling s(t) turns the geometric path \sigma(s) into a trajectory \sigma(s(t)) that respects velocity and torque limits.

For a rest-to-rest motion, the cubic scaling s(t) = 3 (t/T)^2 - 2 (t/T)^3 has peak speed 1.5 / T at mid-course. On a path of length L with a speed limit v_{\max}, it needs T \geq 1.5\, L / v_{\max}.

Time-optimal scaling under torque limits is a classical problem (Bobrow, Dubowsky and Gibson, 1985). Planning directly with dynamics — kinodynamic planning — merges with the optimal control of ch. 4. Artificial potential fields (Khatib, 1986) are a reactive alternative, fast but trapped by local minima.

The code below runs an RRT in a square with three discs. The straight line is blocked, and the gaps between the discs are narrower than the step, so the tree must go around. Shortcutting then shortens the path.

import numpy as np

rng = np.random.default_rng(0)
obstacles = [((0.5, 0.5), 0.2), ((0.25, 0.75), 0.15), ((0.75, 0.25), 0.15)]  # (centre, radius)
start, goal = np.array([0.05, 0.05]), np.array([0.95, 0.95])

def free(p):
    return all(np.linalg.norm(p - np.array(c)) > r for c, r in obstacles)

def segment_free(a, b, step=0.005):
    n = max(2, int(np.linalg.norm(b - a) / step) + 1)
    return all(free(a + t * (b - a)) for t in np.linspace(0, 1, n))

def rrt(eta=0.05, goal_bias=0.05, max_iter=5000):
    nodes, parent = [start], [-1]
    for it in range(max_iter):
        x_rand = goal if rng.random() < goal_bias else rng.random(2)
        i_near = int(np.argmin([np.linalg.norm(x_rand - v) for v in nodes]))
        d = x_rand - nodes[i_near]
        x_new = nodes[i_near] + d * min(1.0, eta / (np.linalg.norm(d) + 1e-12))
        if not segment_free(nodes[i_near], x_new):
            continue
        nodes.append(x_new); parent.append(i_near)
        if np.linalg.norm(x_new - goal) < eta and segment_free(x_new, goal):
            nodes.append(goal); parent.append(len(nodes) - 2)
            path, i = [], len(nodes) - 1
            while i >= 0:
                path.append(nodes[i]); i = parent[i]
            return path[::-1], it + 1, len(nodes)
    return None, max_iter, len(nodes)

def shortcut(path):
    """Greedy shortcutting: jump to the farthest vertex visible from the current one."""
    out, i = [path[0]], 0
    while i < len(path) - 1:
        j = len(path) - 1
        while j > i + 1 and not segment_free(path[i], path[j]):
            j -= 1
        out.append(path[j]); i = j
    return out

length = lambda p: sum(np.linalg.norm(b - a) for a, b in zip(p, p[1:]))
path, iters, n_nodes = rrt()
short = shortcut(path)
print(f"RRT: {iters} iterations, {n_nodes} nodes, path of {len(path)} vertices, length {length(path):.3f}")
print(f"after shortcutting: {len(short)} vertices, length {length(short):.3f}")
print(f"straight line {np.linalg.norm(goal - start):.3f} (blocked by the central disc: {not segment_free(start, goal)})")

With this seed, the tree reaches the goal after 500 iterations with 206 vertices. Its path has length 2.22; shortcutting reduces it to 1.71 with a single intermediate vertex, against 1.27 for the blocked straight line.

3. Inverse problems for robot control

A task is specified for the end effector — reach this point, follow this line, push with this force — but the robot is commanded through its joints. Each level of control is therefore an inverse problem: positions (inverse geometry), velocities (differential kinematics), accelerations and torques (inverse dynamics).

Inverse geometry

Closed forms exist only for special geometries. In general, inverse geometry is posed as nonlinear least squares, with e(q) = x^\star - f(q): \min_q \ \tfrac12 \| x^\star - f(q) \|^2 .

Linearising f at the current iterate and solving the damped problem of Proposition 0.3 gives the Levenberg–Marquardt step \Delta q = J^\top (J J^\top + \lambda I)^{-1} e . The damping is decreased after a successful step and increased after a failed one.

A large \lambda gives a short gradient step; a small one gives the fast Gauss–Newton step. Damped least squares in inverse kinematics was introduced by Wampler (1986) and by Nakamura and Hanafusa (1986).

Differential kinematics and redundancy

When the robot has more joints than the task has dimensions (n > m), the task does not determine the joint velocities.

The free vector z serves a secondary objective: avoid joint limits, stay away from singularities, keep a posture. The task-priority scheme (Nakamura, Hanafusa and Yoshikawa, 1987; Siciliano and Slotine, 1991) projects a second task J_2 \dot q = v_2 into the null space of the first: \dot q = J_1^+ v_1 + (J_2 P_1)^+ \big( v_2 - J_2 J_1^+ v_1 \big).

Integrating velocities lets the error drift. Closed-loop inverse kinematics adds a feedback term on the task error.

The code below solves inverse geometry for a three-link planar arm, first for a reachable target, then for an unreachable one. It then solves the reachable task again with a null-space term z = -q that pulls the joints towards zero.

import numpy as np

L = np.array([1.0, 0.8, 0.5])            # three-link planar arm, reach 2.3 m

def fk(q):
    a = np.cumsum(q)                      # absolute link angles
    return np.array([L @ np.cos(a), L @ np.sin(a)])

def jac(q):
    a = np.cumsum(q)
    return np.array([[-L[j:] @ np.sin(a[j:]) for j in range(3)],
                     [ L[j:] @ np.cos(a[j:]) for j in range(3)]])

def ik_lm(x_star, q0, lam=1e-2, tol=1e-12, max_iter=200):
    """Levenberg-Marquardt on 1/2 |x* - fk(q)|^2, damping adapted to progress."""
    q = q0.copy()
    for k in range(max_iter):
        e = x_star - fk(q)
        if e @ e < tol:
            break
        J = jac(q)
        q_try = q + J.T @ np.linalg.solve(J @ J.T + lam * np.eye(2), e)
        if np.linalg.norm(x_star - fk(q_try)) < np.linalg.norm(e):
            q, lam = q_try, lam / 2          # success: trust the Gauss-Newton model more
        else:
            lam = lam * 10                   # failure: shorter, gradient-like step
    return q, k

q0, x_star = np.array([0.3, 0.3, 0.3]), np.array([1.2, 1.0])
q, k = ik_lm(x_star, q0)
print(f"reachable target: {k} iterations, q = {np.round(q, 4)}, error {np.linalg.norm(x_star - fk(q)):.1e}")
q_lm = q
far = np.array([3.0, 0.0])
q, k = ik_lm(far, q0)
print(f"unreachable target: q = {np.round(q, 4) + 0.0}, residual {np.linalg.norm(far - fk(q)):.4f} (= 3.0 - 2.3)")

# Redundancy: same target, secondary task "stay close to q = 0" projected in the null space
q = q0.copy()
for _ in range(400):
    J = jac(q); Jp = np.linalg.pinv(J)
    dq = Jp @ (2.0 * (x_star - fk(q))) + (np.eye(3) - Jp @ J) @ (-q)
    q = q + 0.05 * dq
print(f"with a null-space task: q = {np.round(q, 4)}, |q| = {np.linalg.norm(q):.4f} "
      f"(Levenberg-Marquardt: {np.linalg.norm(q_lm):.4f}), error {np.linalg.norm(x_star - fk(q)):.1e}")

The reachable target is met in 6 iterations. For the unreachable one, the method converges to the stretched arm pointing at the target, at the best possible distance 0.7 m, instead of diverging. The null-space term reaches the same target with a smaller joint norm (1.513 against 1.530): the secondary task acts without disturbing the primary one.

Inverse dynamics and torque control

Kinematic control assumes that the joints follow their commands. Fast or heavy robots need torque control, which inverts the dynamics of Theorem 1.7. Write h(q, \dot q) = C(q, \dot q) \dot q + g(q).

Computed torque needs an exact model. A much simpler law needs only gravity.

Task-space inverse dynamics as a quadratic program

A humanoid must track several tasks while respecting its dynamics, its contacts and its torque limits. Modern controllers solve, at every control period (typically 1 ms), a quadratic program in (\ddot q, \tau, \lambda): \begin{aligned} \min_{\ddot q, \tau, \lambda} \ & \| J_t \ddot q + \dot J_t \dot q - \ddot x_t^\star \|^2 + \epsilon \|\ddot q\|^2 \\ \text{s.t.} \ & M \ddot q + h = S^\top \tau + J_c^\top \lambda, \quad J_c \ddot q + \dot J_c \dot q = 0, \\ & \lambda \in \text{linearised friction cones}, \quad \tau_{\min} \leq \tau \leq \tau_{\max} . \end{aligned}

Here S selects the actuated joints (the floating base has no motor), and the second equality keeps the contact points fixed. The reference \ddot x_t^\star comes from a PD law on the task, as in Proposition 3.5.

With equality constraints only, Proposition 0.4 solves the problem with one KKT system. With strict priorities between tasks, a hierarchy of such programs is solved, efficiently, by the hierarchical quadratic programming of Escande, Mansard and Wieber (2014).

4. Optimal control and trajectory optimisation

Inverse kinematics and computed torque track a motion that someone has designed. Optimal control designs the motion itself: among all control signals that respect the dynamics, it picks the one that minimises a cost — effort, time, distance to a goal.

Dynamic programming

The theorem returns a feedback law, not just a trajectory, but it requires a minimisation over u for every state: tabulating V_t is impossible beyond a few dimensions. One structure escapes this curse exactly.

The linear-quadratic regulator

Over a long horizon, P_t converges to the solution of the discrete algebraic Riccati equation when (A, B) is stabilisable and (A, Q^{1/2}) detectable. The stationary gain then stabilises the system.

Pontryagin’s maximum principle

In continuous time, with \dot x = f(x, u) and cost \int_0^T \ell(x, u)\, dt + \ell_T(x(T)), the necessary conditions take the form of a two-point boundary value problem. Define the Hamiltonian H(x, u, p) = \ell(x, u) + p^\top f(x, u).

Direct methods and differential dynamic programming

Robots have nonlinear dynamics, constraints and dozens of states. Practical solvers discretise first and optimise afterwards:

• single shooting optimises the controls only, the states being obtained by simulation; simple, but badly conditioned for unstable systems;
• multiple shooting and collocation also treat the states as variables, linked by the dynamics as equality constraints. The resulting large, sparse nonlinear program is solved by sequential quadratic programming or interior points.

Differential dynamic programming (DDP; Mayne, 1966) applies Bellman’s principle locally around a nominal trajectory (\bar x_t, \bar u_t). Its iterative-LQR variant (iLQR; Li and Todorov, 2004) linearises the dynamics, \delta x_{t+1} \approx A_t \delta x_t + B_t \delta u_t, and expands the cost to second order. It then solves the resulting LQ problem by the backward recursion Q_u = \ell_u + B_t^\top V'_x, \quad Q_{uu} = \ell_{uu} + B_t^\top V'_{xx} B_t, \quad Q_{ux} = \ell_{ux} + B_t^\top V'_{xx} A_t, where V' is the quadratic model of the value at time t + 1. The step is \delta u_t = k_t + K_t \delta x_t, with k_t = -Q_{uu}^{-1} Q_u and K_t = -Q_{uu}^{-1} Q_{ux}.

A forward simulation with a line search on k_t produces the next nominal trajectory. DDP also keeps the second derivatives of f, which iLQR drops. The Crocoddyl library (Mastalli and coauthors, 2020) implements DDP for multi-contact robots on top of Pinocchio.

The code computes the LQR gain of a double integrator by the Riccati recursion and checks it against SciPy. It then swings up a damped pendulum with iLQR, from rest hanging down to the upright position in 5 s.

import numpy as np
from scipy.linalg import solve_discrete_are

# (a) LQR for the double integrator, state (position, velocity), period dt = 0.1 s
dt = 0.1
A = np.array([[1.0, dt], [0.0, 1.0]]); B = np.array([[dt**2 / 2], [dt]])
Q, R = np.diag([1.0, 0.1]), np.array([[0.01]])
P = Q.copy()
for _ in range(500):                                   # Riccati recursion, backwards in time
    K = np.linalg.solve(R + B.T @ P @ B, B.T @ P @ A)
    P = Q + A.T @ P @ (A - B @ K)
P_are = solve_discrete_are(A, B, Q, R)
print("K from the recursion:", np.round(K, 4), "| max |P - P_are| =", f"{np.abs(P - P_are).max():.1e}")
print("closed-loop eigenvalue moduli:", np.round(np.abs(np.linalg.eigvals(A - B @ K)), 4))

# (b) iLQR swing-up of a damped pendulum (theta = 0 hanging down, target theta = pi)
m, l, g, b, h, N = 1.0, 1.0, 9.81, 0.1, 0.05, 100      # 100 steps of 50 ms
x_goal, r, w_T = np.array([np.pi, 0.0]), 1e-2, 1e3

def f(x, u):                                           # explicit Euler discretisation
    return np.array([x[0] + h * x[1], x[1] + h * (-g / l * np.sin(x[0]) - b * x[1] + u / (m * l**2))])

def f_x(x):
    return np.array([[1.0, h], [-h * g / l * np.cos(x[0]), 1.0 - h * b]])

f_u = np.array([0.0, h / (m * l**2)])

def cost(X, U):
    e = X[-1] - x_goal
    return r * U @ U + w_T * e @ e

U = np.zeros(N)
X = np.zeros((N + 1, 2))
for t in range(N):
    X[t + 1] = f(X[t], U[t])
J = cost(X, U)
for it in range(200):
    Vx, Vxx = 2 * w_T * (X[-1] - x_goal), 2 * w_T * np.eye(2)    # backward pass
    k_ff, K_fb = np.zeros(N), np.zeros((N, 2))
    for t in reversed(range(N)):
        At = f_x(X[t])
        Qx, Qu = At.T @ Vx, 2 * r * U[t] + f_u @ Vx
        Qxx, Quu, Qux = At.T @ Vxx @ At, 2 * r + f_u @ Vxx @ f_u, f_u @ Vxx @ At
        k_ff[t], K_fb[t] = -Qu / Quu, -Qux / Quu
        Vx = Qx + Qux * k_ff[t]                                   # value of the optimal step
        Vxx = Qxx - np.outer(Qux, Qux) / Quu
    for alpha in [1.0, 0.5, 0.25, 0.1, 0.03, 0.01]:               # forward pass + line search
        Xn, Un = np.zeros_like(X), np.zeros(N)
        Xn[0] = X[0]
        for t in range(N):
            Un[t] = U[t] + alpha * k_ff[t] + K_fb[t] @ (Xn[t] - X[t])
            Xn[t + 1] = f(Xn[t], Un[t])
        if cost(Xn, Un) < J:
            break
    if J - cost(Xn, Un) < 1e-9 * J:
        break
    X, U, J = Xn, Un, cost(Xn, Un)
print(f"iLQR: {it} iterations, cost {J:.3f}, final state {np.round(X[-1], 4)}, max |u| = {np.abs(U).max():.2f} N m")

The recursion matches SciPy to 10^{-13} and both closed-loop poles have modulus 0.76. iLQR converges in 46 iterations to a final angle of 3.1405 rad with a peak torque of 2.66 N m. That is about a quarter of the torque m g l = 9.81 N m needed to hold the pendulum horizontal, so the optimiser has discovered that it must pump energy by swinging.

Model predictive control

An open-loop trajectory fails at the first disturbance. Model predictive control (MPC) closes the loop by re-optimising.

This argument, surveyed by Mayne, Rawlings, Rao and Scokaert (2000), underlies the walking controllers of ch. 7, which re-solve a small MPC problem at every control period.

5. Perception and estimation

A controller needs the state of the robot — its pose, its velocity, the position of the objects around it — but sensors give only noisy, partial measurements. Estimation fuses a model of the motion with these measurements; perception extracts geometric information from cameras and range sensors.

Sensors and the filtering problem

Robots measure themselves with joint encoders, force sensors and inertial measurement units (gyroscopes and accelerometers). They measure the world with cameras, depth cameras and LiDAR. Both kinds of measurement are summarised by a state-space model: x_{k+1} = f(x_k, u_k) + w_k, \qquad y_k = h(x_k) + v_k , with process noise w_k and measurement noise v_k.

The Bayes filter propagates the distribution of x_k given y_{0:k} in two steps. The prediction integrates the motion model against the previous posterior; the update multiplies by the likelihood p(y_{k+1} \mid x_{k+1}) and renormalises. Two cases are tractable: linear-Gaussian models (Kalman filter) and low dimensions, or many particles (particle filters, see Computational Statistics).

The Kalman filter

The Riccati recursion of the covariance is the time-reversed twin of the LQR recursion of ch. 4: estimation and control are dual problems, as Kalman already noted.

Nonlinear models: the EKF and observability

The extended Kalman filter (EKF) applies the same recursion to the linearisations F_k = \partial f / \partial x and H_k = \partial h / \partial x at the current estimate. It is the workhorse of robot navigation, but it has no optimality guarantee. Linearising at a wrong estimate can make it inconsistent, that is, overconfident.

Before filtering, one should ask whether the measurements determine the state at all.

Estimation on Lie groups: invariant errors

Orientation lives in SO(3), not in a vector space, and the EKF’s additive error R - \hat R is not a rotation. Bonnabel, Martin and Rouchon (2008) proposed instead errors defined with the group operation. Their behaviour can be remarkable.

The additive error, by contrast, rotates with \omega(t), and a linearisation of it depends on the trajectory. Barrau and Bonnabel (2017) generalised the observation to group-affine systems, a class that includes inertial navigation. For these systems, the invariant error obeys an equation that does not depend on the estimate, and its logarithm obeys an exactly linear equation.

The resulting invariant EKF inherits convergence properties of the linear Kalman filter that the standard EKF lacks. Hartley and coauthors (2020) built a contact-aided invariant EKF for legged robots on this basis.

Batch estimation, SLAM and registration

Filtering processes measurements once. Batch estimation computes the maximum a posteriori trajectory given all measurements, as a nonlinear least-squares problem: \min_{x_{0:K}} \ \| x_0 - m_0 \|^2_{P_0^{-1}} + \sum_k \| x_{k+1} - f(x_k, u_k) \|^2_{Q^{-1}} + \sum_k \| y_k - h(x_k) \|^2_{R^{-1}} , where \mathcal{N}(m_0, P_0) is the prior on the initial state.

Gauss–Newton solves it with sparse linear algebra, because each term involves one or two states. In the linear-Gaussian case, the solution coincides with the Kalman smoother. When the landmarks of a map join the unknowns, the problem becomes simultaneous localisation and mapping (SLAM). Factor graphs (Dellaert and Kaess, 2017) represent the sparsity; Barfoot’s textbook develops the whole approach.

Cameras enter through the pinhole model. A point (X, Y, Z) in the camera frame projects to the pixel (f X / Z + c_x, \ f Y / Z + c_y), and minimising reprojection errors is bundle adjustment; see 3D Computer Vision. Depth sensors give 3D points directly, and the basic problem becomes aligning two point clouds.

When correspondences are unknown, the iterative closest point algorithm (Besl and McKay, 1992) alternates nearest-neighbour matching and Theorem 5.5. Each step decreases the cost, so ICP converges, but only to a local minimum: it needs a good initial guess.

The code runs a Kalman filter on a constant-velocity target observed in position only, then registers two noisy point clouds.

import numpy as np
from scipy.spatial.transform import Rotation

rng = np.random.default_rng(1)
# (a) Kalman filter: constant-velocity target, only the position is measured
dt, n, sa, sr = 0.1, 200, 0.5, 1.0          # period, steps, acceleration std, measurement std
F, H = np.array([[1.0, dt], [0.0, 1.0]]), np.array([[1.0, 0.0]])
Qn = sa**2 * np.array([[dt**4 / 4, dt**3 / 2], [dt**3 / 2, dt**2]])
Rn = np.array([[sr**2]])
x, xs, ys = np.array([0.0, 1.0]), [], []
for _ in range(n):
    x = F @ x + rng.multivariate_normal(np.zeros(2), Qn)
    xs.append(x); ys.append(H @ x + sr * rng.standard_normal(1))
xs, ys = np.array(xs), np.array(ys)

m, P, est = np.zeros(2), 10.0 * np.eye(2), []
for y in ys:
    m, P = F @ m, F @ P @ F.T + Qn                        # predict
    S = H @ P @ H.T + Rn
    K = P @ H.T @ np.linalg.inv(S)                        # Kalman gain
    m, P = m + K @ (y - H @ m), (np.eye(2) - K @ H) @ P   # update
    est.append(m)
est = np.array(est)
rmse = lambda u, v: np.sqrt(np.mean((u - v) ** 2))
print(f"position RMSE: raw measurements {rmse(ys[:, 0], xs[:, 0]):.3f}, Kalman {rmse(est[:, 0], xs[:, 0]):.3f}")
print(f"velocity RMSE (never measured): {rmse(est[:, 1], xs[:, 1]):.3f}; final gain K = {np.round(K.ravel(), 3)}")

# (b) Registration of two point sets with known correspondences (Theorem 5.5)
R_true = Rotation.from_rotvec([0.2, -0.4, 0.9]).as_matrix()
t_true = np.array([0.5, -1.0, 2.0])
p = rng.standard_normal((100, 3))
q = p @ R_true.T + t_true + 0.01 * rng.standard_normal((100, 3))
pc, qc = p.mean(0), q.mean(0)
U, s, Vt = np.linalg.svd((q - qc).T @ (p - pc))           # W = sum (q_i - qc)(p_i - pc)^T
D = np.diag([1.0, 1.0, np.sign(np.linalg.det(U @ Vt))])
R_hat = U @ D @ Vt
t_hat = qc - R_hat @ pc
angle = np.degrees(np.arccos(np.clip((np.trace(R_hat.T @ R_true) - 1) / 2, -1, 1)))
print(f"rotation error {angle:.3f} deg, translation error {np.linalg.norm(t_hat - t_true):.4f}")

The filter divides the position error by almost three (0.35 against 0.98 for the raw measurements). It also estimates the velocity, which is never measured, with an error of 0.27, as Proposition 5.3 promised. The registration recovers the rotation to 0.05 degree and the translation to 2 mm.

6. Reinforcement learning for robotics

Optimal control needs a model of the dynamics and of the cost. Reinforcement learning (RL) optimises a policy from trials instead — in simulation, most of the time. It has produced the most robust legged controllers of recent years; this chapter gives its mathematical core and the conditions under which it transfers to real robots.

Markov decision processes and optimal control

With a known model, this is the optimal control problem of ch. 4, and Bellman’s recursion applies with an expectation over x'. RL methods differ in what they learn: value functions (Q-learning), policies (policy gradient), or both (actor–critic). The Reinforcement Learning course of semester 2 develops them in depth. Here, the policy-gradient family matters most, because robot actions are continuous.

The policy-gradient theorem

The estimator only needs to sample trajectories and differentiate the log-policy: no model, no derivative of the dynamics. Its variance, however, is large; the baseline reduces it without bias.

The code below runs this REINFORCE estimator, with the time-wise average of G_t as baseline, on x_{t+1} = x_t + 0.5 u_t with cost x^2 + 0.1 u^2 over 30 steps. It compares the learned gain with the LQR gain of Example 4.4.

import numpy as np

rng = np.random.default_rng(0)
a, b, qc, rc, T = 1.0, 0.5, 1.0, 0.1, 30       # x' = a x + b u, cost q x^2 + r u^2, horizon T

p = qc                                          # stationary scalar Riccati equation (ch. 4)
for _ in range(1000):
    p = qc + a * a * p - (a * b * p) ** 2 / (rc + b * b * p)
K_lqr = a * b * p / (rc + b * b * p)

def rollouts(theta, sigma, n):
    """n episodes of the Gaussian policy u = -theta x + sigma eps; returns states, noises, costs."""
    x = rng.standard_normal(n)
    X, E, C = np.zeros((T, n)), np.zeros((T, n)), np.zeros((T, n))
    for t in range(T):
        eps = rng.standard_normal(n)
        u = -theta * x + sigma * eps
        X[t], E[t], C[t] = x, eps, qc * x**2 + rc * u**2
        x = a * x + b * u
    return X, E, C

theta, sigma, lr = 0.0, 0.5, 0.003
for it in range(501):
    X, E, C = rollouts(theta, sigma, 500)
    G = np.cumsum(C[::-1], axis=0)[::-1]              # cost-to-go from each time step
    G = G - G.mean(axis=1, keepdims=True)             # baseline: average cost-to-go at time t
    score = -E * X / sigma                            # d/dtheta log pi(u_t | x_t)
    grad = np.mean(np.sum(score * G, axis=0))         # REINFORCE estimate of dJ/dtheta
    theta -= lr * grad
    if it % 100 == 0:
        print(f"iteration {it:3d}: theta = {theta:.3f}, average cost {C.sum(0).mean():.2f}")
print(f"LQR gain K = {K_lqr:.3f}")

After 501 iterations of 500 episodes each, the learned gain is 1.533, against 1.531 for the LQR. RL has rediscovered the optimal controller, but it used about 250,000 episodes where the Riccati equation needed one line. That ratio is the price of ignoring the model, and the reason why robot RL is done in simulation.

Actor–critic methods: PPO and SAC

Practical algorithms replace G_t - b(x_t) by an estimate of the advantage A(x_t, u_t) = Q(x_t, u_t) - V(x_t), given by a learned critic. Proximal policy optimisation (PPO; Schulman and coauthors, 2017) maximises a clipped surrogate objective over a batch collected with the previous policy: \mathbb{E}\Big[ \min\big( \rho_t A_t, \ \mathrm{clip}(\rho_t, 1 - \epsilon, 1 + \epsilon) A_t \big) \Big], \qquad \rho_t = \frac{\pi_\theta(u_t \mid x_t)}{\pi_{\theta_{\mathrm{old}}}(u_t \mid x_t)}, written for rewards (negative costs). The clipping keeps the new policy close to the one that collected the data. Soft actor–critic (SAC; Haarnoja and coauthors, 2018) adds an entropy bonus to the reward and learns off-policy from a replay buffer, which saves samples.

From simulation to real robots

A policy trained in simulation meets a different robot: other masses, friction, motor dynamics, delays and sensor noise. How much does a model error cost?

The error is amplified by the square of the effective horizon 1 / (1 - \gamma): small per-step errors add up along long trajectories. Three remedies dominate practice.

• Domain randomisation (Tobin and coauthors, 2017) trains on a distribution of simulators — random masses, frictions, delays — so that the real robot looks like one more sample.
• Better simulators, such as a learned model of the actuators (Hwangbo and coauthors, 2019).
• Privileged learning: a teacher policy trained with simulator-only information is distilled into a student that sees only onboard sensors (Lee and coauthors, 2020). Miki and coauthors (2022) extended this approach to perceptive locomotion, and their quadruped completed a hike in the mountains.

Learning from demonstrations

When a human can demonstrate the task, behaviour cloning fits the policy by supervised learning on the demonstrated state–action pairs. Its weakness is distribution shift: a small error leads to states never demonstrated, where errors grow.

Ross and Bagnell (2010) showed that the cost of behaviour cloning can grow quadratically with the horizon. The DAgger algorithm (Ross, Gordon and Bagnell, 2011) queries the expert on the states visited by the learned policy, and recovers a linear growth.

7. Legged locomotion

A legged robot is not attached to the ground. It can move its body only by pushing on the ground through its feet, and the ground can only push. This chapter turns that observation into the zero-moment point, the linear inverted pendulum and the capture point. It then generates walking motions with the MPC of ch. 4.

Floating base and centroidal dynamics

Write the configuration as q = (q_b, q_j), with the floating-base pose q_b \in SE(3) and the joint angles q_j. In the dynamics of Theorem 1.7, the torque vector is S^\top \tau, where S selects the actuated joints: the first six equations, those of the base, contain no motor torque at all.

Without contact, the CoM follows a parabola, whatever the joints do. Walking is therefore the art of choosing contact forces — and the ground constrains them.

The zero-moment point and the support polygon

The moment balance of Proposition 7.1, taken about z instead of c, relates the ZMP to the motion of the CoM. With L constant and the CoM at constant height h, it gives, for the horizontal coordinates, z = c - \frac{h}{g}\, \ddot c . Balance is then a constraint on the CoM acceleration: c - (h/g) \ddot c must stay inside the support polygon, or the foot starts to tip over its edge.

The linear inverted pendulum and the capture point

This is the previous relation solved for \ddot c. The CoM is repelled by the ZMP: if z stays fixed, the solution c(t) - z = (c_0 - z) \cosh \omega t + \frac{\dot c_0}{\omega} \sinh \omega t diverges exponentially unless the initial conditions are just right.

Proposition 7.5 also explains why walking needs preview. The only bounded solution of the unstable equation is \xi(t) = \omega \int_t^{\infty} e^{-\omega (s - t)} z(s)\, ds , so the current state must anticipate the future ZMP, that is the future footsteps.

Walking pattern generation by MPC

Kajita and coauthors (2003) generated walking by preview control of the ZMP; Wieber (2006) recast it as linear MPC. Discretise the CoM with period T, state \hat x = (c, \dot c, \ddot c) and the jerk as control. Over a horizon of N periods, the future ZMPs are affine in the jerks U: Z = P_x \hat x + P_u U .

The pattern generator minimises a weighted sum of \|Z - Z_{\mathrm{ref}}\|^2, where Z_{\mathrm{ref}} follows the planned footsteps, and of \|U\|^2. Without inequality constraints, the minimiser is explicit; with the support polygons as constraints, it is a small QP.

Herdt and coauthors (2010) made the footstep positions decision variables of the same QP. Only the first jerk is applied, and the problem is re-solved at the next period (Definition 4.7). The code below generates a lateral sway for steps alternating between y = \pm 0.1 m every 0.8 s.

import numpy as np

g, hc, T, N = 9.81, 0.8, 0.1, 16          # gravity, CoM height (m), period (s), horizon (1.6 s)
omega = np.sqrt(g / hc)
# Lateral CoM state (y, y', y''), input = jerk; ZMP z = y - (hc/g) y''
A = np.array([[1, T, T**2 / 2], [0, 1, T], [0, 0, 1]])
B = np.array([T**3 / 6, T**2 / 2, T])
C = np.array([1.0, 0.0, -hc / g])
Px = np.array([C @ np.linalg.matrix_power(A, i + 1) for i in range(N)])
Pu = np.zeros((N, N))
for i in range(N):
    for j in range(i + 1):
        Pu[i, j] = C @ np.linalg.matrix_power(A, i - j) @ B

# Footsteps alternate left/right every 0.8 s (y = +-0.1 m); foot half-width 0.05 m
steps = 120
z_ref = np.where((np.arange(steps + N) // 8) % 2 == 0, 0.1, -0.1)   # 8 periods = 0.8 s
z_ref[:8] = 0.0                            # start in double support, between the feet
R_jerk = 1e-6
Hq = Pu.T @ Pu + R_jerk * np.eye(N)        # least squares in the jerks (Wieber, 2006)

x, zs, ys = np.zeros(3), [], []
for k in range(steps):
    jerks = np.linalg.solve(Hq, Pu.T @ (z_ref[k + 1:k + 1 + N] - Px @ x))
    x = A @ x + B * jerks[0]               # receding horizon: apply the first jerk only
    zs.append(C @ x); ys.append(x[0])
zs, ys = np.array(zs), np.array(ys)
dev = np.abs(zs[8:] - z_ref[1:steps + 1][8:])
print(f"omega = {omega:.3f} 1/s; max |ZMP - foot centre| = {dev.max():.3f} m (foot half-width 0.05 m)")
print(f"CoM lateral sway: {ys[40:].min():+.3f} m to {ys[40:].max():+.3f} m, while the feet are at +-0.100 m")
xi = 0.0 + 0.5 / omega                     # capture point after a 0.5 m/s push from rest
print(f"capture point after a 0.5 m/s push: {xi:.3f} m ahead of the CoM")

The ZMP stays within 7 mm of the foot centres, well inside the 5 cm half-width of the feet. The CoM sways between -0.053 m and +0.053 m: it never goes above the feet, and it starts moving towards the next foot before the step, as the preview formula requires.

Whole-body control and learned locomotion

On a real humanoid, the layers of this course stack up:

• a footstep planner, often a graph search or an optimisation (ch. 2);
• a CoM pattern generator by MPC, as above;
• a whole-body QP that tracks the CoM and foot tasks under the full dynamics, contact and torque limits (ch. 3), at about 1 kHz;
• a state estimator that fuses joint encoders, inertial measurements and contact information (ch. 5).

The control of the Atlas humanoid described by Kuindersma and coauthors (2016) follows this optimisation-based scheme; Wensing and coauthors (2024) survey it. The alternative of ch. 6 trains a single neural policy by RL in simulation, from proprioception and sometimes perception to joint targets. Combining the guarantees of the first approach with the robustness of the second is an open question (ch. 9).

8. Responsible robotics

The last lecture of the official programme steps back from algorithms. It asks what responsibilities researchers and engineers take on when they build machines that act in the physical world. This chapter gives one mathematical tool, one statistical tool, and a framework for the questions that have no formula.

What is specific to robots

Many questions of AI ethics apply to robots, but several take a sharper form:

• physical harm: a wrong output is a collision, not a wrong label, and failures can injure;
• diluted responsibility: when a learned controller fails, responsibility is spread between the designer, the data, the integrator and the operator;
• sensing: cameras and microphones in homes, hospitals and streets record people who never consented;
• dual use: the controller that lets a quadruped climb rubble after an earthquake works just as well on a military platform; lethal autonomous weapons are debated within the United Nations Convention on Certain Conventional Weapons;
• work: automation transforms jobs, and who benefits from the productivity gains is a political question;
• environment: robots contain batteries, motors and electronics, with their mining and recycling problems, and training large policies consumes energy.

Safety by design and control barrier functions

Industrial practice relies on standards: ISO 10218 for industrial robots, ISO/TS 15066 for collaborative robots that share space with humans, and ISO 13482 for personal care robots. In the European Union, the Machinery Regulation (EU) 2023/1230 and the AI Act, Regulation (EU) 2024/1689, add legal obligations; the Regulating AI course studies them.

Standards ask for risk analysis and protective measures. Control theory offers one precise kind of protective measure: a filter that keeps the state in a safe set, whatever the nominal controller — hand-designed, optimised or learned — asks for.

The safety filter chooses the admissible input closest to the nominal one: u^\star(x) = \arg\min_u \ \|u - u_{\mathrm{nom}}(x)\|^2 \quad \text{s.t.} \quad \nabla h^\top (f + g u) \geq -\alpha h . With one constraint, this QP is the projection of u_{\mathrm{nom}} onto a half-space, which has a closed form (Exercise 12).

The code applies the filter to a robot \dot x = u in the plane, driven to a goal by a proportional controller, past a disc of radius 0.5 that lies across the straight path.

import numpy as np

# Single integrator x' = u in the plane; the nominal controller drives the robot to the goal
c_obs, r_obs = np.array([0.0, 0.1]), 0.5          # disc to avoid, slightly off the straight line
goal, k_nom, dt = np.array([2.0, 0.0]), 1.0, 0.01

def h(x):                                          # barrier: h >= 0 outside the disc
    return (x - c_obs) @ (x - c_obs) - r_obs**2

def safety_filter(x, u_nom, alpha):
    """argmin |u - u_nom|^2 s.t. grad h(x) . u >= -alpha h(x): one half-plane, closed form."""
    a = 2 * (x - c_obs)
    slack = a @ u_nom + alpha * h(x)
    return u_nom if slack >= 0 else u_nom - slack * a / (a @ a)

for alpha in [None, 1.0, 5.0]:                     # None = no filter
    x, h_min, active = np.array([-2.0, 0.0]), np.inf, 0
    for _ in range(1500):                          # 15 s, explicit Euler
        u_nom = -k_nom * (x - goal)
        u = u_nom if alpha is None else safety_filter(x, u_nom, alpha)
        active += int(np.any(u != u_nom))
        x = x + dt * u
        h_min = min(h_min, h(x))
    print(f"alpha = {alpha}: min h = {h_min:+.3f}, final distance to goal "
          f"{np.linalg.norm(x - goal):.1e}, filter active during {active * dt:.2f} s")

Without the filter, the robot crosses the disc (h reaches -0.24). With the filter, h stays positive and the goal is still reached.

A small rate \alpha = 1 keeps a wide margin (h \geq 0.48); a larger rate \alpha = 5 lets the robot graze the obstacle (h \geq 0.016).

The guarantee is only as good as the model. Unmodelled dynamics, delays and estimation errors (ch. 5) erode it, and a robot can also get stuck in front of an obstacle. Robust and learned variants of barrier functions are an active research topic (ch. 9).

Research integrity: claims, videos and trials

Robotics papers are often judged on videos and success rates, which invites selective reporting. A few habits make claims verifiable:

• report the number of trials, the failures and the conditions, not only the best run;
• release code, simulator configurations and hardware details, in line with the reproducible research course;
• give confidence intervals: a handful of real-robot trials says less than it seems.

A framework for responsible innovation

Stilgoe, Owen and Macnaghten (2013) propose four dimensions of responsible innovation, which translate into questions for a robotics project:

• anticipation — who will use this robot, for what, and what could go wrong, including uses the authors do not intend;
• reflexivity — which assumptions are built into the objective, the data and the benchmark;
• inclusion — have the people who will work next to the robot, or be watched by it, been consulted;
• responsiveness — can the project change course when the answers are uncomfortable, up to not publishing a component.

These questions have no algorithmic answer. They belong in the report of every project of this course (see the rubric in Projects and assessment), and they link this page to Responsible Machine Learning and Artificial Intelligence and the Environment.

9. Research directions and open problems

This chapter is for self-study. It points from each chapter of the course to active research questions, citing only well-established work, and ends with open problems that make good project or internship topics.

Differentiable and contact-rich simulation

Trajectory optimisation and learning both need derivatives of the dynamics. Carpentier and Mansard (2018) derived the analytical derivatives of the recursive rigid-body algorithms of ch. 1, which made second-order solvers such as the DDP of ch. 4 fast enough for humanoids.

Contacts remain the hard part. The complementarity conditions of Definition 1.9 make the dynamics non-smooth, and every simulator chooses a compromise between physical accuracy, convexity and differentiability. Todorov (2014) described the convex formulation behind MuJoCo; how to obtain informative gradients through contact switches is an open question.

Optimisation-based whole-body control

Whole-body MPC — the full dynamics of ch. 1, the contacts and the tasks in one receding-horizon problem — is now possible in real time on some robots. The survey of Wensing and coauthors (2024) maps the field: centroidal versus whole-body models, contact sequences fixed or optimised, and the solvers that make them run.

Strict task priorities (ch. 3) solved by hierarchical quadratic programming (Escande, Mansard and Wieber, 2014) raise their own questions. How should priorities be combined with the horizon of MPC, and with inequality constraints that switch on and off?

Learned locomotion and manipulation

Policies trained by RL in simulation now walk on rough terrain (Lee and coauthors, 2020; Miki and coauthors, 2022). In manipulation, imitation learning has progressed with expressive policy classes: Chi and coauthors (2023) represent the policy as a diffusion model over action sequences.

Brohan and coauthors (2023) fine-tune vision-language models to output robot actions. How such large models should be evaluated, and how their failures can be bounded, is largely open.

Estimation on Lie groups

The invariant EKF of ch. 5 (Barrau and Bonnabel, 2017) has spread from inertial navigation to legged robots (Hartley and coauthors, 2020). Solà, Deray and Atchuthan (2018) give a compact introduction to the Lie-group calculus that these filters use. Questions remain for systems that are not group-affine, and for the fusion of learned perception with filters that come with guarantees.

Safety of learned controllers

Control barrier functions (ch. 8; Ames and coauthors, 2017, 2019) certify safety for a known model. Making them robust to model errors and to estimation errors, learning them from data, and combining them with RL policies without destroying performance are active topics.

Open problems to think about

1. Can a simulator be differentiable, physically consistent at impacts and fast, all at once? Which of the three can be relaxed for which task?
2. The simulation lemma (Proposition 6.4) is pessimistic. Which structural properties of a robot make policies transfer much better than the bound predicts?
3. How many real-robot trials are needed to rank two controllers with confidence (Example 8.3)? Can simulation reduce that number without biasing the comparison?
4. Can an invariant filter be built for a legged robot whose feet slip, with a consistent uncertainty?
5. What would a responsible-innovation assessment (ch. 8) of a general-purpose humanoid look like, and who should carry it out?

Exercises

Levels: ★ apply the course, ★★ combine results or prove a short statement, ★★★ open-ended or research-flavoured. The Practice column of the session plan says when to do each one.

1. ★ Kinematics of the two-link arm. For the arm of Example 1.4 (l_1 = 1, l_2 = 0.8) at q = (0, \pi/2), compute the position of the end effector, the Jacobian and its determinant. Which joint velocities move the end effector straight up at 0.1 m/s? List all singular configurations.

   Solution: the end effector is at (1, 0.8), and J = \begin{pmatrix} -0.8 & -0.8 \\ 1 & 0 \end{pmatrix}, \qquad \det J = 0.8 = l_1 l_2 \sin q_2 .

   Solving J \dot q = (0, 0.1) gives \dot q_1 = 0.1 and \dot q_2 = -0.1 rad/s: the shoulder lifts and the elbow compensates to keep x fixed.

   The singular configurations are q_2 = 0 and q_2 = \pi, for any q_1.

2. ★ A rotation that permutes the axes. With Rodrigues’ formula (Proposition 0.6), show that the rotation of angle 2\pi/3 about u = (1, 1, 1)/\sqrt{3} sends e_x to e_y. Without computation, where does it send e_y?

   Solution: [u]_\times e_x = u \times e_x = (0, 1, -1)/\sqrt{3} and [u]_\times^2 e_x = u (u^\top e_x) - e_x = (-2/3, 1/3, 1/3). With \sin(2\pi/3) = \sqrt{3}/2 and 1 - \cos(2\pi/3) = 3/2: R e_x = (1, 0, 0) + \tfrac12 (0, 1, -1) + (-1, \tfrac12, \tfrac12) = (0, 1, 0). The rotation commutes with the cyclic permutation of the axes, which fixes u, so it also sends e_y to e_z and e_z to e_x.

3. ★ One Kalman step by hand. A constant-velocity model with \Delta t = 1 and no process noise has prior mean (0, 1) and covariance I. The position is measured as y = 1.5 with noise variance 1. Compute the predicted and updated means and covariances.

   Solution: prediction, m^- = (1, 1) and P^- = F F^\top = \begin{pmatrix} 2 & 1 \\ 1 & 1 \end{pmatrix}.

   Update: S = 3, K = (2/3, 1/3), innovation 0.5, hence m = (1.333, 1.167) and P = \begin{pmatrix} 2/3 & 1/3 \\ 1/3 & 2/3 \end{pmatrix}. The velocity estimate moved, although only the position was measured: the prediction correlated the two. Repeated over time, this is how the velocity becomes observable (Proposition 5.3).

4. ★ How many samples for a roadmap? In a free space of area 0.9 in the plane, a path of length L = 2 has clearance r = 0.05. With Theorem 2.3, how many uniform samples guarantee a failure probability of at most 1 %? What if the clearance is halved?

   Solution: m = 80 and p = \pi (0.0125)^2 / 0.9 \approx 5.45 \times 10^{-4}. The condition 80\, e^{-N p} \leq 0.01 gives N \geq \ln(8000) / p \approx 16{,}500.

   With r = 0.025, m doubles and p is divided by four, so N \approx 71{,}000: more than four times as many samples. Narrow passages dominate the cost of sampling-based planning.

5. ★ Push recovery. A humanoid has its CoM at h = 1 m. It stands still with its ZMP able to move at most 0.12 m ahead of the CoM. Can it stop without stepping after a push to 0.3 m/s? To 0.5 m/s? How long does it have before the divergent component has grown by a factor e?

   Solution: \omega = \sqrt{9.81} \approx 3.13 s^{-1}. After 0.3 m/s the capture point is 0.3 / 3.13 \approx 0.096 m ahead, inside the reach of the ZMP: the ankles suffice (Proposition 7.5).

   After 0.5 m/s it is 0.16 m ahead, beyond 0.12 m: the robot must step. The time constant is 1/\omega \approx 0.32 s.

6. ★★ Holding a load. The arm of Example 1.4, with massless links, holds a 1 kg mass at the tip in the configuration q = (0.4, 1.1). Compute the joint torques with Proposition 1.6, and interpret each one as a weight times a lever arm. Which configurations of the same tip height minimise the shoulder torque?

   Solution: the arm must push up with f = (0, 9.81) N, so \tau = J^\top f = 9.81 \begin{pmatrix} l_1 \cos q_1 + l_2 \cos(q_1 + q_2) \\ l_2 \cos(q_1 + q_2) \end{pmatrix} \approx \begin{pmatrix} 9.59 \\ 0.555 \end{pmatrix} \text{N m}.

   Each torque is the weight times the horizontal distance from the joint to the load: \tau_1 = 9.81\, x. The shoulder torque is therefore minimal, and zero, when the load is directly above the shoulder (x = 0), if the tip height is reachable there.

7. ★★ Two tasks without interference. Let P_1 = I - J_1^+ J_1 and \dot q = J_1^+ v_1 + (J_2 P_1)^+ (v_2 - J_2 J_1^+ v_1), with J_1 of full row rank. Prove that the first task is always met exactly, and that the second one is met too when J_2 P_1 has full row rank.

   Solution: the range of (J_2 P_1)^+ is the range of (J_2 P_1)^\top = P_1 J_2^\top, contained in the range of P_1, which is \ker J_1 (Proposition 3.3). Hence J_1 (J_2 P_1)^+ = 0 and J_1 \dot q = J_1 J_1^+ v_1 = v_1.

   For the second task, P_1 (J_2 P_1)^+ = (J_2 P_1)^+ because P_1 is a projector onto a space containing that range. So J_2 (J_2 P_1)^+ = J_2 P_1 (J_2 P_1)^+ = I under the rank assumption, and J_2 \dot q = J_2 J_1^+ v_1 + v_2 - J_2 J_1^+ v_1 = v_2.

8. ★★ The scalar Riccati equation. For x_{t+1} = a x_t + b u_t with a = 1.2, b = 1, q = r = 1, solve the stationary Riccati equation and give the gain and the closed-loop pole.

   Then show that when r \to \infty (expensive control) and |a| > 1, the closed-loop pole tends to 1/a.

   Solution: multiplying the equation by 1 + p gives p^2 - 1.44 p - 1 = 0, so p \approx 1.952, K = 1.2 p / (1 + p) \approx 0.794, and the pole is a - b K \approx 0.406.

   For large r, write p = r s and divide by r. In the limit, s = a^2 s - a^2 b^2 s^2 / (1 + b^2 s). Since s \neq 0 for an unstable system, 1 + b^2 s = a^2, hence K \to \frac{a^2 - 1}{a b}, \qquad a - b K \to \frac{1}{a} . The cheapest stabilising controller mirrors the unstable pole inside the unit circle; here 1/1.2 \approx 0.833.

9. ★★ Minimum-energy transfer, general case. Redo Example 4.6 for a transfer of length D in time T. How does the optimal cost scale with T? What does that say about fast motions?

   Solution: the same steps give u(t) = a + b t, and the four boundary conditions yield u(t) = \frac{6 D}{T^2} \Big( 1 - \frac{2 t}{T} \Big), \qquad \int_0^T u^2\, dt = \frac{12 D^2}{T^3} . Halving the duration multiplies the energy by eight, and the peak acceleration 6 D / T^2 by four: fast motions quickly hit torque limits.

10. ★★ The best constant baseline. For the estimator \hat g = s\, (G - b), where s = \nabla_\theta \log \pi_\theta is a scalar score and b a constant, show that \hat g is unbiased for every b.

    Find the b that minimises its variance, and check that \mathbb{E}[s] = 0 for the policy of Example 6.3.

    Solution: \mathbb{E}[s\, b] = b\, \mathbb{E}[s] = 0 by the identity in the proof of Theorem 6.2, so the mean does not depend on b. The second moment \mathbb{E}[s^2 (G - b)^2] is a quadratic in b, minimal at b^\star = \frac{\mathbb{E}[s^2 G]}{\mathbb{E}[s^2]} , a score-weighted average of the costs. For Example 6.3, \mathbb{E}[-\varepsilon x / \sigma] = 0 because \varepsilon is centred and independent of x.

11. ★★ Why walking needs preview. Prove that the only solution of \dot \xi = \omega (\xi - z) that stays bounded for a bounded ZMP z is \xi(t) = \omega \int_t^\infty e^{-\omega (s - t)} z(s)\, ds. Apply it to a ZMP that jumps from 0 to d at time t_1 (a step), and interpret.

    Solution: the difference of two solutions is C e^{\omega t}, bounded only if C = 0. The given formula is bounded by \sup |z|, and differentiating it gives \dot \xi = -\omega z(t) + \omega \xi(t).

    For the step, \xi(t) = d\, e^{-\omega (t_1 - t)} for t < t_1. The divergent component, hence the CoM, starts moving towards the new foot before the step, with a lead time of order 1/\omega — what the code of ch. 7 shows.

12. ★★ The safety filter in closed form. Show that the solution of \min_u \|u - u_{\mathrm{nom}}\|^2 subject to a^\top u \geq \beta, with a \neq 0, is u^\star = u_{\mathrm{nom}} + \max\big(0, \ \beta - a^\top u_{\mathrm{nom}}\big) \frac{a}{\|a\|^2} . Identify a and \beta in the code of ch. 8.

    Solution: if u_{\mathrm{nom}} is feasible, it is optimal. Otherwise the constraint is active at the optimum (a convex problem whose unconstrained minimiser is infeasible). Proposition 0.4 with the single equality a^\top u = \beta gives u = u_{\mathrm{nom}} + \nu a with \nu = (\beta - a^\top u_{\mathrm{nom}}) / \|a\|^2 > 0.

    In the code, a = \nabla h(x) = 2 (x - c) and \beta = -\alpha h(x), and slack is a^\top u_{\mathrm{nom}} - \beta.

13. ★★ Damped least squares near a singularity (Python). Put the two-link arm almost stretched, q = (0, 0.01), and ask the tip to move towards the base at 0.1 m/s. Compare the joint velocities and the task error given by the pseudo-inverse and by damped least squares with \lambda = 10^{-4} and 10^{-2}. Explain the results with Proposition 0.3.

    Hints and expected result: the smallest singular value of J is about 0.004. The pseudo-inverse asks for about 25 rad/s, physically impossible, to track the task exactly.

    With \lambda = 10^{-4}, the joint speed drops to about 3.5 rad/s and the task is tracked only partially. With \lambda = 10^{-2}, the arm barely moves. The factor \sigma / (\sigma^2 + \lambda) replaces 1/\sigma: damping trades accuracy for bounded speeds.

    import numpy as np
    
    l1, l2 = 1.0, 0.8
    
    def jac(q):
        s1, c1, s12, c12 = np.sin(q[0]), np.cos(q[0]), np.sin(q.sum()), np.cos(q.sum())
        return np.array([[-l1 * s1 - l2 * s12, -l2 * s12], [l1 * c1 + l2 * c12, l2 * c12]])
    
    q = np.array([0.0, 0.01])            # arm almost stretched along the x axis
    v = np.array([-0.1, 0.0])            # move the tip towards the base: the nearly singular direction
    J = jac(q)
    print("singular values of J:", np.round(np.linalg.svd(J, compute_uv=False), 5))
    for lam in [0.0, 1e-4, 1e-2]:
        dq = J.T @ np.linalg.solve(J @ J.T + lam * np.eye(2), v)
        print(f"lambda = {lam:g}: |dq| = {np.linalg.norm(dq):7.3f} rad/s, "
              f"task velocity error {np.linalg.norm(J @ dq - v):.4f} m/s")

14. ★★ Is my Kalman filter consistent? (Python). For a consistent filter, the normalised estimation error squared e^\top P^{-1} e, with e = x - m, has mean equal to the state dimension. Check it by Monte Carlo on the constant-velocity model of ch. 5. Then give the filter a process-noise covariance one hundred times too small.

    Hints and expected result: with the right noise model, the average is 2.00. With the underestimated noise, it is about 72: the filter is overconfident, and in a real robot it would reject valid measurements as outliers. This test is the standard way to tune Q and R.

    import numpy as np
    
    rng = np.random.default_rng(2)
    dt, n, runs, sa, sr = 0.1, 100, 500, 0.5, 1.0
    F, H = np.array([[1.0, dt], [0.0, 1.0]]), np.array([[1.0, 0.0]])
    G = np.array([dt**2 / 2, dt])                    # a random acceleration enters through G
    Q = sa**2 * np.outer(G, G)
    
    def average_nees(Q_filter):
        """Average over runs of e^T P^-1 e at the last step (should be about 2 if consistent)."""
        x = rng.standard_normal((runs, 2))           # true initial states, prior N(0, I)
        m, P = np.zeros((runs, 2)), np.eye(2)
        for _ in range(n):
            x = x @ F.T + sa * np.outer(rng.standard_normal(runs), G)
            y = x[:, 0] + sr * rng.standard_normal(runs)
            m, P = m @ F.T, F @ P @ F.T + Q_filter
            S = P[0, 0] + sr**2
            K = P[:, 0] / S
            m = m + np.outer(y - m[:, 0], K)
            P = P - np.outer(K, P[0, :])
        e = x - m
        return np.mean(np.einsum("ri,ij,rj->r", e, np.linalg.inv(P), e))
    
    print(f"correct Q:        NEES = {average_nees(Q):.2f}  (dimension 2)")
    print(f"Q divided by 100: NEES = {average_nees(Q / 100):.2f}  (overconfident filter)")

15. ★★★ Constrained walking pattern generation with a push (Python). Extend the code of ch. 7: add the constraints |z_k - z_{\mathrm{ref}, k}| \leq 0.05 m (the foot) and solve the QP at each period, for instance with scipy.optimize.minimize (method SLSQP) or a small active-set solver. Apply a lateral push (a jump of the CoM velocity) at t = 4 s and find the largest push the generator can absorb without violating the constraints. Then let the next footstep position be a decision variable, as in Herdt and coauthors (2010).

    Hints and expected result: with fixed footsteps, the QP becomes infeasible beyond a push for which the capture point leaves the reachable ZMP region (Proposition 7.5). Report this threshold as a function of the foot half-width. With a free footstep, the admissible push grows markedly. A clean figure of the CoM, ZMP and footsteps with and without the push is the expected deliverable.

16. ★★★ Invariant versus standard EKF (self-study, ch. 5 and ch. 9). Simulate a unicycle in the plane, \dot \theta = \omega and \dot p = R(\theta) (v, 0), measured by noisy odometry (v, \omega) and noisy GPS positions.

    Implement the standard EKF on (\theta, p) and an invariant EKF on SE(2), choosing between the errors \hat g\, g^{-1} and g^{-1} \hat g according to the form of the GPS measurement, as Barrau and Bonnabel (2017) explain. Compare the two filters from an initial heading error of 90 degrees.

    Hints and expected result: the dynamics are group-affine, so the invariant error propagates independently of the estimate. Expect the invariant EKF to converge from large heading errors where the standard EKF can become inconsistent — its covariance shrinks while its error does not. Plot the error and the normalised error of ex. 14 for both filters over 100 runs.

17. ★★★ A responsible-robotics case study (ch. 8). A team plans to deploy a sidewalk delivery robot in a city. Write a two-page analysis organised by the four dimensions of Stilgoe and coauthors (2013). Add one quantitative element: how many independent trials without a single failure are needed to claim, with 95 % confidence, a failure probability below 1 %?

    Hints and expected result: with zero failures in n trials, the one-sided bound requires 0.99^n \leq 0.05, that is n \geq 299 — the “rule of three”, 3/n. The analysis should name the stakeholders (pedestrians, people with reduced mobility, couriers, the city) and the sensing and privacy issues of the cameras. It should also say which safety guarantees are model-based (barrier functions, speed limits) and which are only statistical.

Projects and assessment

The official assessment (master-mva.com) combines weekly homework (20 %) with a project or an article study (80 %). The final exam is a 4 h student seminar where projects and reports are presented; the Université Paris-Saclay course detail calls this a non-integral continuous assessment. The subjects below are sized for the 80 % component, and each ends with a talk for the seminar. The homework suggested in the session plan fits the 20 % component. The written exam is only a self-assessment proposal: the official assessment includes none.

Solo projects

1. [Solo · ≈ 20 h] Paper study: the capture point. Reproduce the capture-point analysis of Pratt, Carff, Drakunov and Goswami (2006) on the linear inverted pendulum. Compute the capture region for a finite foot and a minimum step time, then simulate push recovery with ankle control and with stepping.
   • Extension: compare with the divergent-component controller of Englsberger, Ott and Albu-Schäffer (2015).
   • Success criterion: the largest recoverable push as a function of foot size and step time, matching Proposition 7.5.
   • Deliverables: code, 4–6 page report, 10-minute talk at the final seminar. Chapters 7 and 4.
2. [Solo · ≈ 20 h] Paper study: the invariant extended Kalman filter. Study Barrau and Bonnabel (2017): restate their stability theorem for group-affine systems and check its hypotheses on a synthetic navigation problem — a vehicle with gyroscope, odometry and position measurements. Implement the standard and the invariant EKF.
   • Success criterion: over 100 runs with random initial headings up to 180 degrees, the invariant EKF brings the heading error below 5 degrees within 60 s in at least 95 runs, with an average normalised error (Exercise 14) in the 95 % chi-square band. A figure shows where the standard EKF fails either test.
   • Deliverables: code, 4–6 page report, 10-minute talk. Chapters 0 and 5.
3. [Solo · ≈ 20 h] Asymptotic optimality of RRT*. Implement RRT and RRT* (Karaman and Frazzoli, 2011) in the plane and in a three-dimensional configuration space with obstacles. Compute the optimal cost independently, by a visibility graph in the plane or a fine grid with A* in 3D.
   • Success criterion: a plot of the cost against the number of samples over 50 seeds, showing the RRT plateau above the optimum. In the plane, the median RRT* cost is within 5 % of the visibility-graph optimum at 10,000 samples. A second plot shows the effect of the rewiring radius.
   • Deliverables: code, 4–6 page report, 10-minute talk. Chapter 2.

Group projects

1. [Group of 2–3 · ≈ 30 h each] Cart-pole: optimal control against reinforcement learning. Derive the cart-pole dynamics with Theorem 1.7 and write a simulator with an energy test. Swing up with iLQR and stabilise with LQR and MPC under a force limit. Train a policy by REINFORCE or PPO in the same simulator, with and without randomisation of the masses.
   • Split: member A writes the model and the simulator; B the optimal control and MPC; C the learning and the robustness study.
   • Success criterion: swing-up from rest in at most 5 s under the force limit; success rate against mass mismatch for both controllers, with confidence intervals (Example 8.3); number of simulated samples used by each.
   • Deliverables: shared repository, 8–10 page report, 20-minute talk. Chapters 1, 4, 6 and 8.
2. [Group of 2–3 · ≈ 30 h each] A walking pattern generator with whole-body kinematics. Implement the linear MPC of Wieber (2006) with ZMP constraints as a QP, then the automatic footstep placement of Herdt and coauthors (2010). Track the sagittal CoM and foot trajectories on a planar five-link biped with task-priority inverse kinematics.
   • Split: A writes the MPC and the QP solver; B the kinematic model and the inverse kinematics; C the evaluation (pushes, constraint checks, visualisation).
   • Success criterion: ten steps without ZMP violation, with a tracking error below 1 cm. Report the largest forward push absorbed with fixed and with free footsteps; the free footsteps must absorb a strictly larger one.
   • Deliverables: shared repository, 8–10 page report, 20-minute talk. Chapters 3, 4 and 7.
3. [Group of 3 · ≈ 30 h each] A safe mobile robot: estimate, plan, control and filter. In a simulated 2D world with static obstacles and moving pedestrians, localise a unicycle by EKF from odometry and range–bearing landmarks. Plan with A* or RRT*, track the path by MPC, and protect it with a control-barrier safety filter.
   • Split: one member per layer (estimation, planning and control, safety), with shared interfaces fixed in the first week.
   • Success criterion: zero collisions in 300 randomised trials — which supports a failure probability below 1 % with 95 % confidence (Exercise 17) — and the arrival rate and time, with and without the filter.
   • Deliverables: shared repository, 8–10 page report including a responsible-innovation section (ch. 8), 20-minute talk. Chapters 2, 4, 5 and 8.

Written exam (proposal)

Format: 3 hours, one handwritten A4 sheet allowed, no electronic devices, four exercises. Not part of the official assessment.

1. Question (4 points). Define the zero-moment point of a robot standing on flat ground, and prove that it lies in the support polygon. Explain why keeping the ZMP inside the polygon is not sufficient for a robot to come to rest, using the linear inverted pendulum.

   Expected: the definition as the barycentre of the contact points weighted by the normal forces; the convex-combination argument; the equation \dot \xi = \omega (\xi - z), showing that the CoM comes to rest only if the ZMP can reach the capture point (Proposition 7.5).

2. Question (6 points). A planar two-link arm has l_1 = l_2 = 1 m. At q = (\pi/2, -\pi/2): (a) give the position of the end effector and the Jacobian; (b) list all singular configurations of the arm; (c) compute the joint torques that exert the force f = (1, 0) N; (d) find the joint velocities that move the end effector up at 1 m/s.

   Expected: (a) position (1, 1) and J = \begin{pmatrix} -1 & 0 \\ 1 & 1 \end{pmatrix};

   2. q_2 \in \{0, \pi\}, from \det J = \sin q_2; (c) \tau = J^\top f = (-1, 0) N m, each torque being the force times the vertical lever arm; (d) \dot q = (0, 1) rad/s.

3. Question (5 points). State and prove the global asymptotic stability of PD control with gravity compensation for a fully actuated robot. Say exactly where the skew-symmetry of \dot M - 2C is used, and what happens without gravity compensation.

   Expected: the Lyapunov function \tfrac12 \dot q^\top M \dot q + \tfrac12 e^\top K_p e, the derivative -\dot q^\top K_d \dot q after the skew-symmetric term cancels, then LaSalle (Theorem 3.6). Without compensation, the equilibrium satisfies K_p e = g(q): a steady-state error that a larger K_p reduces but does not remove.

4. Question (5 points). For x_{t+1} = x_t + u_t with stage cost x^2 + u^2, compute the stationary LQR gain and the closed-loop pole. Then, for the estimation problem x_{k+1} = x_k + w_k and y_k = x_k + v_k with unit noise variances, compute the stationary predicted variance and the Kalman gain. Comment.

   Expected: the Riccati equation is p^2 - p - 1 = 0, so p \approx 1.618, K = p / (1 + p) \approx 0.618 and the pole is 0.382. The predicted variance solves the same equation, giving the same gain 0.618: an instance of the duality between control and estimation (ch. 5).

Grading rubric (proposal)

References

Official references (master-mva.com)

• Murray, R. M., Li, Z., & Sastry, S. S. (2017). A Mathematical Introduction to Robotic Manipulation. CRC Press.
• Liberzon, D. (2011). Calculus of Variations and Optimal Control Theory: A Concise Introduction. Princeton University Press.
• Barfoot, T. D. (2017). State Estimation for Robotics. Cambridge University Press.
• Kajita, S., Hirukawa, H., Harada, K., & Yokoi, K. (2014). Introduction to Humanoid Robotics. Springer.

Textbooks

• Bertsekas, D. P. (2017). Dynamic Programming and Optimal Control, Vol. I (4th ed.). Athena Scientific.
• Featherstone, R. (2008). Rigid Body Dynamics Algorithms. Springer.
• Hartley, R., & Zisserman, A. (2004). Multiple View Geometry in Computer Vision (2nd ed.). Cambridge University Press.
• Khalil, H. K. (2002). Nonlinear Systems (3rd ed.). Prentice Hall.
• LaValle, S. M. (2006). Planning Algorithms. Cambridge University Press.
• Lynch, K. M., & Park, F. C. (2017). Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press.
• Nocedal, J., & Wright, S. J. (2006). Numerical Optimization (2nd ed.). Springer.
• Rawlings, J. B., Mayne, D. Q., & Diehl, M. (2017). Model Predictive Control: Theory, Computation, and Design (2nd ed.). Nob Hill Publishing.
• Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.
• Sutton, R. S., & Barto, A. G. (2018). Reinforcement Learning: An Introduction (2nd ed.). MIT Press.
• Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.

Papers

• Ames, A. D., Xu, X., Grizzle, J. W., & Tabuada, P. (2017). Control barrier function based quadratic programs for safety critical systems. IEEE Transactions on Automatic Control.
• Ames, A. D., Coogan, S., Egerstedt, M., Notomista, G., Sreenath, K., & Tabuada, P. (2019). Control barrier functions: Theory and applications. European Control Conference (ECC).
• Barrau, A., & Bonnabel, S. (2017). The invariant extended Kalman filter as a stable observer. IEEE Transactions on Automatic Control.
• Besl, P. J., & McKay, N. D. (1992). A method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence.
• Bobrow, J. E., Dubowsky, S., & Gibson, J. S. (1985). Time-optimal control of robotic manipulators along specified paths. The International Journal of Robotics Research.
• Bonnabel, S., Martin, P., & Rouchon, P. (2008). Symmetry-preserving observers. IEEE Transactions on Automatic Control.
• Brockett, R. W. (1984). Robotic manipulators and the product of exponentials formula. In Mathematical Theory of Networks and Systems, Lecture Notes in Control and Information Sciences 58, Springer.
• Brohan, A., et al. (2023). RT-2: Vision-language-action models transfer web knowledge to robotic control. Conference on Robot Learning (CoRL).
• Carpentier, J., & Mansard, N. (2018). Analytical derivatives of rigid body dynamics algorithms. Robotics: Science and Systems (RSS).
• Carpentier, J., Saurel, G., Buondonno, G., Mirabel, J., Lamiraux, F., Stasse, O., & Mansard, N. (2019). The Pinocchio C++ library: A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives. IEEE/SICE International Symposium on System Integration (SII).
• Chi, C., et al. (2023). Diffusion policy: Visuomotor policy learning via action diffusion. Robotics: Science and Systems (RSS).
• Dellaert, F., & Kaess, M. (2017). Factor graphs for robot perception. Foundations and Trends in Robotics.
• Englsberger, J., Ott, C., & Albu-Schäffer, A. (2015). Three-dimensional bipedal walking control based on divergent component of motion. IEEE Transactions on Robotics.
• Escande, A., Mansard, N., & Wieber, P.-B. (2014). Hierarchical quadratic programming: Fast online humanoid-robot motion generation. The International Journal of Robotics Research.
• Haarnoja, T., Zhou, A., Abbeel, P., & Levine, S. (2018). Soft actor-critic: Off-policy maximum entropy deep reinforcement learning with a stochastic actor. International Conference on Machine Learning (ICML).
• Hart, P. E., Nilsson, N. J., & Raphael, B. (1968). A formal basis for the heuristic determination of minimum cost paths. IEEE Transactions on Systems Science and Cybernetics.
• Hartley, R., Ghaffari, M., Eustice, R. M., & Grizzle, J. W. (2020). Contact-aided invariant extended Kalman filtering for robot state estimation. The International Journal of Robotics Research.
• Herdt, A., Diedam, H., Wieber, P.-B., Dimitrov, D., Mombaur, K., & Diehl, M. (2010). Online walking motion generation with automatic footstep placement. Advanced Robotics.
• Hwangbo, J., Lee, J., Dosovitskiy, A., Bellicoso, D., Tsounis, V., Koltun, V., & Hutter, M. (2019). Learning agile and dynamic motor skills for legged robots. Science Robotics.
• Kajita, S., Kanehiro, F., Kaneko, K., Yokoi, K., & Hirukawa, H. (2001). The 3D linear inverted pendulum mode: A simple modeling for a biped walking pattern generation. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).
• Kajita, S., Kanehiro, F., Kaneko, K., Fujiwara, K., Harada, K., Yokoi, K., & Hirukawa, H. (2003). Biped walking pattern generation by using preview control of zero-moment point. IEEE International Conference on Robotics and Automation (ICRA).
• Kalman, R. E. (1960). A new approach to linear filtering and prediction problems. Journal of Basic Engineering.
• Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research.
• Kavraki, L. E., Švestka, P., Latombe, J.-C., & Overmars, M. H. (1996). Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation.
• Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots. The International Journal of Robotics Research.
• Kuindersma, S., Deits, R., Fallon, M., Valenzuela, A., Dai, H., Permenter, F., Koolen, T., Marion, P., & Tedrake, R. (2016). Optimization-based locomotion planning, estimation, and control design for the Atlas humanoid robot. Autonomous Robots.
• LaValle, S. M. (1998). Rapidly-exploring random trees: A new tool for path planning. Technical Report TR 98-11, Computer Science Department, Iowa State University.
• LaValle, S. M., & Kuffner, J. J. (2001). Randomized kinodynamic planning. The International Journal of Robotics Research.
• Lee, J., Hwangbo, J., Wellhausen, L., Koltun, V., & Hutter, M. (2020). Learning quadrupedal locomotion over challenging terrain. Science Robotics.
• Li, W., & Todorov, E. (2004). Iterative linear quadratic regulator design for nonlinear biological movement systems. International Conference on Informatics in Control, Automation and Robotics (ICINCO).
• Mastalli, C., Budhiraja, R., Merkt, W., Saurel, G., Hammoud, B., Naveau, M., Carpentier, J., Righetti, L., Vijayakumar, S., & Mansard, N. (2020). Crocoddyl: An efficient and versatile framework for multi-contact optimal control. IEEE International Conference on Robotics and Automation (ICRA).
• Mayne, D. (1966). A second-order gradient method for determining optimal trajectories of non-linear discrete-time systems. International Journal of Control.
• Mayne, D. Q., Rawlings, J. B., Rao, C. V., & Scokaert, P. O. M. (2000). Constrained model predictive control: Stability and optimality. Automatica.
• Miki, T., Lee, J., Hwangbo, J., Wellhausen, L., Koltun, V., & Hutter, M. (2022). Learning robust perceptive locomotion for quadrupedal robots in the wild. Science Robotics.
• Nakamura, Y., & Hanafusa, H. (1986). Inverse kinematic solutions with singularity robustness for robot manipulator control. Journal of Dynamic Systems, Measurement, and Control.
• Nakamura, Y., Hanafusa, H., & Yoshikawa, T. (1987). Task-priority based redundancy control of robot manipulators. The International Journal of Robotics Research.
• Pratt, J., Carff, J., Drakunov, S., & Goswami, A. (2006). Capture point: A step toward humanoid push recovery. IEEE-RAS International Conference on Humanoid Robots.
• Reif, J. H. (1979). Complexity of the mover’s problem and generalizations. IEEE Symposium on Foundations of Computer Science (FOCS).
• Ross, S., & Bagnell, D. (2010). Efficient reductions for imitation learning. International Conference on Artificial Intelligence and Statistics (AISTATS).
• Ross, S., Gordon, G., & Bagnell, D. (2011). A reduction of imitation learning and structured prediction to no-regret online learning. International Conference on Artificial Intelligence and Statistics (AISTATS).
• Schulman, J., Wolski, F., Dhariwal, P., Radford, A., & Klimov, O. (2017). Proximal policy optimization algorithms. arXiv:1707.06347.
• Siciliano, B., & Slotine, J.-J. E. (1991). A general framework for managing multiple tasks in highly redundant robotic systems. International Conference on Advanced Robotics (ICAR).
• Solà, J., Deray, J., & Atchuthan, D. (2018). A micro Lie theory for state estimation in robotics. arXiv:1812.01537.
• Stewart, D. E., & Trinkle, J. C. (1996). An implicit time-stepping scheme for rigid body dynamics with inelastic collisions and Coulomb friction. International Journal for Numerical Methods in Engineering.
• Stilgoe, J., Owen, R., & Macnaghten, P. (2013). Developing a framework for responsible innovation. Research Policy.
• Takegaki, M., & Arimoto, S. (1981). A new feedback method for dynamic control of manipulators. Journal of Dynamic Systems, Measurement, and Control.
• Tobin, J., Fong, R., Ray, A., Schneider, J., Zaremba, W., & Abbeel, P. (2017). Domain randomization for transferring deep neural networks from simulation to the real world. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).
• Todorov, E., Erez, T., & Tassa, Y. (2012). MuJoCo: A physics engine for model-based control. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).
• Todorov, E. (2014). Convex and analytically-invertible dynamics with contacts and constraints: Theory and implementation in MuJoCo. IEEE International Conference on Robotics and Automation (ICRA).
• Umeyama, S. (1991). Least-squares estimation of transformation parameters between two point patterns. IEEE Transactions on Pattern Analysis and Machine Intelligence.
• Vukobratović, M., & Borovac, B. (2004). Zero-moment point — thirty five years of its life. International Journal of Humanoid Robotics.
• Wampler, C. W. (1986). Manipulator inverse kinematic solutions based on vector formulations and damped least-squares methods. IEEE Transactions on Systems, Man, and Cybernetics.
• Wensing, P. M., Posa, M., Hu, Y., Escande, A., Mansard, N., & Del Prete, A. (2024). Optimization-based control for dynamic legged robots. IEEE Transactions on Robotics.
• Wieber, P.-B. (2006). Trajectory free linear model predictive control for stable walking in the presence of strong perturbations. IEEE-RAS International Conference on Humanoid Robots.
• Williams, R. J. (1992). Simple statistical gradient-following algorithms for connectionist reinforcement learning. Machine Learning.