13.The Robot as an Environment: Kinematics, Dynamics & Control

13.1 Configuration, pose, and the two spaces

A robot's configuration $q \in \mathbb{R}^n$ lists its joint variables — for Reacher, two angles $q = (q_1, q_2)$. Its pose is where the end-effector actually is, an element of $SE(2)$ in the plane (position plus orientation) or $SE(3)$ in space.

Everything in robot control lives in the tension between these two descriptions. You command joint space, because that is where the motors are. You care about task space, because that is where the mug is.

Forward kinematics maps one to the other. For Reacher with link lengths $\ell_1, \ell_2$:

This is always well defined, cheap, and unique. Inverse kinematics goes the other way and is where the difficulties live. Squaring and adding:

$x^2 + y^2 = \ell_1^2 + \ell_2^2 + 2\ell_1\ell_2 \cos q_2 \quad\Longrightarrow\quad \cos q_2 = \frac{x^2+y^2-\ell_1^2-\ell_2^2}{2\ell_1\ell_2},$

so

$q_2 = \pm\arccos\left(\frac{x^2+y^2-\ell_1^2-\ell_2^2}{2\ell_1\ell_2}\right), \qquad q_1 = \operatorname{atan2}(y,x) - \operatorname{atan2}\big(\ell_2 \sin q_2,\; \ell_1 + \ell_2\cos q_2\big).$

13.2 The Jacobian: velocities, forces, and singularities

Differentiating forward kinematics gives the Jacobian $J(q)$, mapping joint velocities to end-effector velocity, $\dot{p} = J(q)\,\dot q$. For Reacher:

$J(q) = \begin{bmatrix} -\ell_1 s_1 - \ell_2 s_{12} & -\ell_2 s_{12} \\ \ell_1 c_1 + \ell_2 c_{12} & \ell_2 c_{12} \end{bmatrix},$

with $s_1 = \sin q_1$, $s_{12} = \sin(q_1+q_2)$ and so on.

The Jacobian does three jobs at once. It maps velocities forward. It maps forces backward — the static force relation $\tau = J^\top F$ says the joint torques needed to exert an end-effector force $F$ are the Jacobian transpose applied to it, which is how force control is implemented. And it diagnoses singularities.

For Reacher, $\det J = \ell_1 \ell_2 \sin q_2$, which vanishes at $q_2 = 0$ (arm fully extended) and $q_2 = \pi$ (fully folded). At those configurations the arm loses the ability to move in one direction entirely, and any controller that inverts $J$ demands infinite joint velocity.

The manipulability ellipsoid — the image of the unit joint-velocity ball under $J$ — visualizes this: it is round where the arm is dexterous and flattens to a line at a singularity.

13.3 Dynamics: the manipulator equation

Kinematics ignores mass. To command torques we need dynamics, and the systematic route is Lagrangian mechanics.

Form the Lagrangian $\mathcal{L} = T - V$ (kinetic minus potential energy) and apply the Euler–Lagrange equation:

$\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot q_i} - \frac{\partial \mathcal{L}}{\partial q_i} = \tau_i.$

For any serial manipulator this always produces the same structure — the manipulator equation:

$M(q)\,\ddot q + C(q,\dot q)\,\dot q + g(q) = \tau.$

Each term has a clean physical reading. $M(q)$ is the mass matrix: symmetric, positive definite, and configuration-dependent — an extended arm has more rotational inertia than a folded one, which is why a controller tuned in one configuration misbehaves in another. $C(q,\dot q)\dot q$ collects Coriolis and centrifugal effects, the velocity-dependent coupling that makes fast motion qualitatively harder than slow motion. $g(q)$ is gravity, the term that is usually largest and, helpfully, the easiest to cancel exactly.

For Reacher, writing $\alpha = m_1 \ell_{c1}^2 + m_2(\ell_1^2 + \ell_{c2}^2) + I_1 + I_2$, $\beta = m_2 \ell_1 \ell_{c2}$, $\delta = m_2 \ell_{c2}^2 + I_2$:

$M(q) = \begin{bmatrix} \alpha + 2\beta \cos q_2 & \delta + \beta\cos q_2 \\ \delta + \beta \cos q_2 & \delta \end{bmatrix}, \qquad C(q,\dot q) = \begin{bmatrix} -\beta \dot q_2 \sin q_2 & -\beta(\dot q_1 + \dot q_2)\sin q_2 \\ \beta \dot q_1 \sin q_2 & 0 \end{bmatrix}.$

A useful structural fact: $\dot M - 2C$ is skew-symmetric, which encodes energy conservation and underpins the stability proofs of most classical robot controllers.

13.4 Classical control: the baselines RL must beat

Three controllers, in increasing order of how much they know about the robot.

PID knows nothing. It drives the error to zero using proportional, integral and derivative terms:

$\tau = K_p e + K_i \int e\, dt + K_d \dot e.$

It is model-free, ubiquitous, and remarkably effective for slow, well-geared, fully actuated systems. It struggles with coupling, gravity that varies with configuration, and anything fast.

Computed torque knows the model and cancels it. Choosing

$\tau = M(q)\big(\ddot q_d + K_d \dot e + K_p e\big) + C(q,\dot q)\dot q + g(q)$

makes the closed-loop error dynamics exactly linear: $\ddot e + K_d \dot e + K_p e = 0$. This is feedback linearization, and it is beautiful when the model is right. Its weakness is precisely its strength — it depends entirely on model accuracy, and Chapter 15 is about how wrong models actually are.

LQR is optimal control for a linearized system, and it deserves special attention here because it is exactly the machinery of Chapter 5 in continuous form. For dynamics $x_{k+1} = A x_k + B u_k$ and cost $\sum_k (x_k^\top Q x_k + u_k^\top R u_k)$, the optimal policy is linear state feedback $u = -Kx$ with $K = (R + B^\top P B)^{-1} B^\top P A$, where $P$ solves the discrete algebraic Riccati equation

$P = Q + A^\top P A - A^\top P B\,(R + B^\top P B)^{-1} B^\top P A.$

use nalgebra::{Matrix2, Vector2};
 
pub struct ReacherParams {
    pub m1: f64, pub m2: f64,
    pub l1: f64, pub l2: f64,
    pub lc1: f64, pub lc2: f64,
    pub i1: f64, pub i2: f64,
    pub g: f64,
}
 
impl ReacherParams {
    /// Configuration-dependent mass matrix M(q) — symmetric, positive definite.
    pub fn mass_matrix(&self, q: &Vector2<f64>) -> Matrix2<f64> {
        let alpha = self.m1 * self.lc1.powi(2)
            + self.m2 * (self.l1.powi(2) + self.lc2.powi(2))
            + self.i1 + self.i2;
        let beta = self.m2 * self.l1 * self.lc2;
        let delta = self.m2 * self.lc2.powi(2) + self.i2;
        let c2 = q[1].cos();
 
        Matrix2::new(
            alpha + 2.0 * beta * c2, delta + beta * c2,
            delta + beta * c2,       delta,
        )
    }
 
    /// Coriolis/centrifugal matrix C(q, q̇).
    pub fn coriolis(&self, q: &Vector2<f64>, dq: &Vector2<f64>) -> Matrix2<f64> {
        let beta = self.m2 * self.l1 * self.lc2;
        let s2 = q[1].sin();
        Matrix2::new(
            -beta * dq[1] * s2, -beta * (dq[0] + dq[1]) * s2,
             beta * dq[0] * s2,  0.0,
        )
    }
 
    /// Forward dynamics: q̈ = M⁻¹(τ − C q̇ − g).
    pub fn accel(&self, q: &Vector2<f64>, dq: &Vector2<f64>, tau: &Vector2<f64>) -> Vector2<f64> {
        let m = self.mass_matrix(q);
        let c = self.coriolis(q, dq);
        let grav = self.gravity(q);
        m.try_inverse().expect("M(q) is positive definite") * (tau - c * dq - grav)
    }
}
 
/// Discrete LQR by iterating the Riccati recursion to its fixed point.
/// This is Chapter 5's value iteration, specialized to quadratic value functions.
pub fn dlqr(a: &Matrix2<f64>, b: &Matrix2<f64>, q: &Matrix2<f64>, r: &Matrix2<f64>)
    -> Matrix2<f64>
{
    let mut p = *q;
    for _ in 0..500 {
        let s = r + b.transpose() * p * b;
        let k = s.try_inverse().unwrap() * b.transpose() * p * a;
        let p_next = q + a.transpose() * p * (a - b * k);
        if (p_next - p).norm() < 1e-12 { p = p_next; break; }
        p = p_next;
    }
    (r + b.transpose() * p * b).try_inverse().unwrap() * b.transpose() * p * a
}

13.5 Where learning actually enters

Given all this machinery, what is left for reinforcement learning? Four distinct entry points, and confusing them is a common source of wasted effort.

Learning the policy — replace the controller entirely. Correct when the dynamics are too complex or contact-rich to model, as in legged locomotion over rough terrain. Usually wasteful when a classical controller already works.

Learning the model — keep classical control, learn the dynamics it needs. This is Chapter 12's territory and Chapter 15's system identification. Often the highest-value option, because a small correction to a mostly-right model goes a long way.

Learning a residual — run a hand-designed controller and let a policy learn a correction on top, $\tau = \tau_{\text{classical}} + \tau_{\text{learned}}$. The classical part guarantees baseline competence and safety; the learned part handles what the model missed. Chapter 17 builds this properly, and it is frequently the pragmatic answer for real deployments.

Learning to tune — treat controller gains as the action space and let RL adapt them online. Modest, low-risk, and underused.

13.6 State estimation, and Chapter 4's confession revisited

One more piece of reality. The controllers above assume you know $q$ and $\dot q$. You measure encoder counts and integrate noisy signals.

The extended Kalman filter is the standard answer: maintain a Gaussian belief over the state, propagate it through linearized dynamics (predict), and correct it with each measurement (update). It is Chapter 4's belief update, specialized to Gaussians and linearized dynamics.

This is where Chapter 4's confession becomes concrete. The observation is not the state; a filter produces an estimate with real error; and a policy trained on ground-truth simulator state will encounter estimation error it has never seen. Chapter 15's teacher–student method exists precisely to handle this, and Chapter 19's recurrent policies learn to do the filtering themselves.

13.7 Chapter bridge

The black box is open. Reacher is a mechanism with derivable kinematics, a Jacobian that says what it can and cannot do, dynamics that follow from the Lagrangian, and classical controllers that already solve a respectable fraction of the problem. We know where learning belongs — and, equally useful, where it does not.

Chapter 14 asks what makes robot RL hard in ways that have nothing to do with algorithms. Kober's four curses: the dimensionality we quantified in Chapter 5, the sample cost that shaped Chapter 11, the under-modelling that Chapter 12 fought, and the one we have so far avoided entirely — goal specification. Reward functions are the interface between human intent and an optimizer that will exploit any gap in your phrasing, and the results are a catalogue of instructive disasters.

References