A differential-drive robot has two independently actuated wheels mounted on the same axle. We assume: (i) planar motion on a flat ground, (ii) pure rolling (no longitudinal slip), (iii) no lateral slip at the wheel contact points (introduced in Lesson 1 as rolling constraints), and (iv) rigid body chassis.

The robot pose is \( \mathbf{q} = [x,\;y,\;\theta]^T \), where \( (x,y) \) is the position of a body-fixed reference point (typically the midpoint of the axle), and \( \theta \) is the yaw angle measured from the world \( x \)-axis.

Let wheel radius be \( r \) and axle length (wheel separation) be \( L \). Wheel angles are \( \phi_L, \phi_R \), with angular rates \( \dot{\phi}_L, \dot{\phi}_R \).

flowchart TD
  A["Wheel rates: phi_dot_L, phi_dot_R"] --> B["Wheel linear speeds: v_L = r*phi_dot_L, v_R = r*phi_dot_R"]
  B --> C["Body twist: v = (v_R+v_L)/2, w = (v_R-v_L)/L"]
  C --> D["Pose rates: x_dot = v*cos(theta), y_dot = v*sin(theta), theta_dot = w"]
  D --> E["Discrete update over dt (exact for constant v,w)"]
        

From Lesson 1, pure rolling implies that the component of the wheel contact-point velocity along the rolling direction equals the wheel rim speed, and the lateral component is zero. For differential drive, the wheels are fixed (not steerable), so the wheel planes are parallel to the body \( x_b \)-axis.

Define the body-frame unit vectors: \( \mathbf{e}_{x_b} = [\cos\theta,\;\sin\theta]^T \), \( \mathbf{e}_{y_b} = [-\sin\theta,\;\cos\theta]^T \). The axle midpoint velocity is \( \mathbf{v}_c = [\dot{x},\;\dot{y}]^T \).

The no-lateral-slip constraint at the axle midpoint is:

\[ \mathbf{e}_{y_b}^T \mathbf{v}_c = 0 \;\;\Longleftrightarrow\;\; [-\sin\theta,\;\cos\theta] \begin{bmatrix}\dot{x}\\ \dot{y}\end{bmatrix} = 0 \;\;\Longleftrightarrow\;\; -\sin\theta\,\dot{x} + \cos\theta\,\dot{y} = 0. \]

Interpretation. The chassis instantaneous velocity at the axle midpoint lies along the body forward axis: \( \mathbf{v}_c = v\,\mathbf{e}_{x_b} \) for some scalar forward speed \( v \). This is the key nonholonomic restriction: the robot cannot instantaneously move sideways without slip.

Each wheel’s longitudinal (rolling-direction) speed equals its rim speed: \( v_L = r\dot{\phi}_L \), \( v_R = r\dot{\phi}_R \). We now connect these to the rigid-body motion of the chassis.

Let the body angular velocity about the vertical axis be \( \omega = \dot{\theta} \). For a rigid body in planar motion, the velocity of any point is: \( \mathbf{v}(\mathbf{p}) = \mathbf{v}_c + \omega\,\mathbf{k}\times(\mathbf{p}-\mathbf{p}_c) \), where \( \mathbf{k} \) is the unit vector out of the plane.

The left and right wheel contact points lie at lateral offsets \( \pm L/2 \) from the axle midpoint. In the body frame, their position vectors (relative to the midpoint) are: \( \mathbf{p}_L = [0,\; +L/2]^T \), \( \mathbf{p}_R = [0,\; -L/2]^T \).

The forward (body \( x_b \)) component of each contact-point velocity must equal \( v_L \) and \( v_R \). Using rigid-body kinematics, the forward speeds become:

\[ v_L = v - \omega\frac{L}{2},\qquad v_R = v + \omega\frac{L}{2}. \]

Derivation (rigorous). The instantaneous velocity at a lateral offset adds (or subtracts) the tangential speed due to rotation. With rotation rate \( \omega \), the tangential speed magnitude is \( |\omega|\cdot(L/2) \). Signs differ because the wheels are on opposite sides of the rotation center.

Solving the two linear equations for \( v \) and \( \omega \) yields:

\[ v = \frac{v_R+v_L}{2},\qquad \omega = \frac{v_R - v_L}{L}. \]

Substituting \( v_L = r\dot{\phi}_L \) and \( v_R = r\dot{\phi}_R \) gives the canonical differential-drive forward kinematics:

\[ v = \frac{r}{2}\left(\dot{\phi}_R + \dot{\phi}_L\right),\qquad \omega = \frac{r}{L}\left(\dot{\phi}_R - \dot{\phi}_L\right). \]

Finally, the pose-rate model in the world frame is:

\[ \dot{x} = v\cos\theta,\qquad \dot{y} = v\sin\theta,\qquad \dot{\theta} = \omega. \]

Proof that the nonholonomic constraint holds. Substitute the pose rates into the constraint \( -\sin\theta\,\dot{x} + \cos\theta\,\dot{y} = 0 \):

\[ -\sin\theta\,(v\cos\theta) + \cos\theta\,(v\sin\theta) = -v\sin\theta\cos\theta + v\cos\theta\sin\theta = 0. \]

Thus the differential-drive model enforces no instantaneous lateral motion at the axle midpoint under the rolling assumptions.

It is often convenient to express the mapping from wheel rates to pose rates as a configuration-dependent Jacobian:

\[ \begin{bmatrix} \dot{x}\\ \dot{y}\\ \dot{\theta} \end{bmatrix} = \underbrace{ \begin{bmatrix} \frac{r}{2}\cos\theta & \frac{r}{2}\cos\theta\\ \frac{r}{2}\sin\theta & \frac{r}{2}\sin\theta\\ -\frac{r}{L} & \frac{r}{L} \end{bmatrix}}_{\mathbf{J}(\theta)} \begin{bmatrix} \dot{\phi}_L\\ \dot{\phi}_R \end{bmatrix}. \]

Since \( \mathbf{J}(\theta) \) is a \( 3\times 2 \) matrix, it is not invertible as a square matrix: wheel rates cannot arbitrarily realize all \( (\dot{x},\dot{y},\dot{\theta}) \) due to the nonholonomic constraint. Instead, we invert the reduced map from wheel rates to body twist \( (v,\omega) \).

Define the linear map: \( [v,\;\omega]^T = \mathbf{A}[\dot{\phi}_L,\;\dot{\phi}_R]^T \) with

\[ \mathbf{A} = \begin{bmatrix} \frac{r}{2} & \frac{r}{2}\\ -\frac{r}{L} & \frac{r}{L} \end{bmatrix}. \]

Invertibility (proof). The determinant is:

\[ \det(\mathbf{A}) = \frac{r}{2}\cdot\frac{r}{L} - \frac{r}{2}\cdot\left(-\frac{r}{L}\right) = \frac{r^2}{L}. \]

Therefore, if \( r \neq 0 \) and \( L \neq 0 \) (physically necessary), then \( \det(\mathbf{A}) \neq 0 \), and the mapping between wheel rates and body twist is bijective.

The inverse kinematics (desired \( v,\omega \) to wheel rates) is:

\[ \dot{\phi}_R = \frac{1}{r}\left(v + \frac{L}{2}\omega\right),\qquad \dot{\phi}_L = \frac{1}{r}\left(v - \frac{L}{2}\omega\right). \]

Feasibility with actuator limits. If wheel speed bounds are \( |\dot{\phi}_L| \le \dot{\phi}_{\max} \) and \( |\dot{\phi}_R| \le \dot{\phi}_{\max} \), then admissible commands must satisfy: \( \left|v \pm \frac{L}{2}\omega\right| \le r\dot{\phi}_{\max} \). This yields a diamond-shaped admissible region in the \( (v,\omega) \) plane.

For planar rigid motion with forward speed \( v \) and yaw rate \( \omega \), if \( \omega \neq 0 \) the chassis rotates about an instantaneous center of curvature (ICC) at distance \( R \) from the axle midpoint along the lateral axis: \( R = v/\omega \). Substituting the differential-drive relations yields:

\[ R = \frac{v}{\omega} = \frac{\frac{v_R+v_L}{2}}{\frac{v_R-v_L}{L}} = \frac{L}{2}\cdot\frac{v_R+v_L}{v_R-v_L} = \frac{L}{2}\cdot\frac{\dot{\phi}_R+\dot{\phi}_L}{\dot{\phi}_R-\dot{\phi}_L}. \]

The path curvature \( \kappa \) (signed) is: \( \kappa = \omega/v \) when \( v \neq 0 \). Using wheel speeds:

\[ \kappa = \frac{\omega}{v} = \frac{\frac{v_R-v_L}{L}}{\frac{v_R+v_L}{2}} = \frac{2}{L}\cdot\frac{v_R-v_L}{v_R+v_L}. \]

Special cases.

• Straight motion: \( v_R = v_L \Rightarrow \omega = 0 \Rightarrow R \) is infinite.
• Pure rotation in place: \( v_R = -v_L \Rightarrow v = 0 \) while \( \omega = 2v_R/L \), so curvature is unbounded (the path is a point).

In software, we typically sample wheel rates (or encoder increments) and assume the body twist \( (v,\omega) \) is constant over a small interval \( \Delta t \). Under this assumption, the continuous model: \( \dot{x} = v\cos\theta \), \( \dot{y} = v\sin\theta \), \( \dot{\theta} = \omega \) admits a closed-form solution.

Case 1: \( \omega \neq 0 \). Since \( \theta(t) = \theta_k + \omega t \), integrate: \( x_{k+1} - x_k = \int_0^{\Delta t} v\cos(\theta_k+\omega t)\,dt \) and similarly for \( y \).

\[ \begin{aligned} x_{k+1} &= x_k + \frac{v}{\omega}\left(\sin(\theta_k+\omega\Delta t) - \sin\theta_k\right),\\ y_{k+1} &= y_k - \frac{v}{\omega}\left(\cos(\theta_k+\omega\Delta t) - \cos\theta_k\right),\\ \theta_{k+1} &= \theta_k + \omega\Delta t. \end{aligned} \]

Proof sketch (integration). Use \( \int \cos(\theta_k+\omega t)\,dt = \frac{1}{\omega}\sin(\theta_k+\omega t) \) and \( \int \sin(\theta_k+\omega t)\,dt = -\frac{1}{\omega}\cos(\theta_k+\omega t) \), evaluated at \( t=0 \) and \( t=\Delta t \).

Case 2: \( \omega = 0 \). The heading is constant and the motion is a straight line:

\[ x_{k+1} = x_k + v\Delta t\cos\theta_k,\qquad y_{k+1} = y_k + v\Delta t\sin\theta_k,\qquad \theta_{k+1} = \theta_k. \]

Numerical note. Implementations commonly branch on \( |\omega| \) being small to avoid catastrophic cancellation in \( v/\omega \). A practical threshold is \( |\omega| < \varepsilon \) for some small \( \varepsilon \).

In many systems, wheel encoders provide increments \( \Delta\phi_L, \Delta\phi_R \) over a sampling period. Convert to arc-length increments: \( \Delta s_L = r\Delta\phi_L \), \( \Delta s_R = r\Delta\phi_R \). Then define: \( \Delta s = (\Delta s_R+\Delta s_L)/2 \), \( \Delta\theta = (\Delta s_R-\Delta s_L)/L \).

Under the same constant-twist assumption over the interval, the exact incremental pose update mirrors Section 6 with substitutions \( v\Delta t \mapsto \Delta s \) and \( \omega\Delta t \mapsto \Delta\theta \).

\[ \begin{aligned} x_{k+1} &= x_k + \frac{\Delta s}{\Delta\theta}\left(\sin(\theta_k+\Delta\theta) - \sin\theta_k\right),\\ y_{k+1} &= y_k - \frac{\Delta s}{\Delta\theta}\left(\cos(\theta_k+\Delta\theta) - \cos\theta_k\right),\\ \theta_{k+1} &= \theta_k + \Delta\theta, \end{aligned} \quad \text{for } |\Delta\theta| > 0, \]

and for \( |\Delta\theta| \approx 0 \) use: \( x_{k+1}=x_k+\Delta s\cos\theta_k \), \( y_{k+1}=y_k+\Delta s\sin\theta_k \).

flowchart TD
  S["Start (x_k, y_k, theta_k)"] --> E["Read encoders: dphi_L, dphi_R"]
  E --> C["Convert: ds_L=r*dphi_L, ds_R=r*dphi_R"]
  C --> T["Compute: ds=(ds_R+ds_L)/2, dtheta=(ds_R-ds_L)/L"]
  T --> Q{"Is |dtheta| small?"}
  Q -->|yes| L1["Update straight: \nx+=ds*cos(theta), y+=ds*sin(theta)"]
  Q -->|no| A1["Update arc:\n x+= (ds/dtheta)*(sin(theta+dtheta)-sin(theta))"]
  A1 --> A2["y+= -(ds/dtheta)*\n(cos(theta+dtheta)-cos(theta))"]
  L1 --> H["theta += dtheta"]
  A2 --> H
  H --> F["Finish (x_{k+1}, y_{k+1}, theta_{k+1})"]
        

This update is purely kinematic; it becomes inaccurate when the rolling assumptions fail (e.g., skid, slip, uneven terrain), which will be treated later in dynamics and odometry error chapters.

The following reference implementations provide forward/inverse kinematics and exact discrete integration. Typical ecosystem integrations include:

• Python: NumPy; optional integration with ROS/ROS2 (e.g., publishing nav_msgs/Odometry).
• C++: Standard library + optional Eigen; typical deployment via ROS2 rclcpp.
• Java: Standard library; optional ROSJava for robotics middleware integration.
• MATLAB/Simulink: Robotics System Toolbox (robotics.DifferentialDriveKinematics) and block-based integration.
• Wolfram Mathematica: Symbolic ODE solving and exact update verification.

Chapter2_Lesson2.py

"""
Chapter 2 - Lesson 2: Differential Drive Kinematics

University-level reference implementation of:
  1) Forward kinematics: wheel rates -> body twist -> pose update
  2) Inverse kinematics: desired (v, w) -> wheel rates
  3) Exact discrete-time integration under piecewise-constant (v, w)

Assumptions:
  - Planar motion (x, y, theta)
  - Pure rolling, no lateral slip
  - Wheel radius r and axle length L are known constants
"""

from __future__ import annotations
from dataclasses import dataclass
import numpy as np


@dataclass(frozen=True)
class DiffDriveParams:
    r: float  # wheel radius [m]
    L: float  # axle length / wheel separation [m]


def body_twist_from_wheels(phi_dot_l: float, phi_dot_r: float, p: DiffDriveParams) -> tuple[float, float]:
    """Map wheel angular rates (rad/s) to body twist (v [m/s], w [rad/s])."""
    v_l = p.r * phi_dot_l
    v_r = p.r * phi_dot_r
    v = 0.5 * (v_r + v_l)
    w = (v_r - v_l) / p.L
    return float(v), float(w)


def wheels_from_body_twist(v: float, w: float, p: DiffDriveParams) -> tuple[float, float]:
    """Inverse mapping: desired (v,w) to wheel angular rates (phi_dot_l, phi_dot_r)."""
    phi_dot_r = (v + 0.5 * p.L * w) / p.r
    phi_dot_l = (v - 0.5 * p.L * w) / p.r
    return float(phi_dot_l), float(phi_dot_r)


def integrate_pose_exact(pose: np.ndarray, v: float, w: float, dt: float) -> np.ndarray:
    """Exact integration over dt for constant (v,w)."""
    x, y, th = float(pose[0]), float(pose[1]), float(pose[2])
    if abs(w) < 1e-12:
        return np.array([x + v * dt * np.cos(th),
                         y + v * dt * np.sin(th),
                         th], dtype=float)

    th2 = th + w * dt
    x2 = x + (v / w) * (np.sin(th2) - np.sin(th))
    y2 = y - (v / w) * (np.cos(th2) - np.cos(th))
    return np.array([x2, y2, th2], dtype=float)


def step_from_wheels(pose: np.ndarray, phi_dot_l: float, phi_dot_r: float, p: DiffDriveParams, dt: float) -> np.ndarray:
    v, w = body_twist_from_wheels(phi_dot_l, phi_dot_r, p)
    return integrate_pose_exact(pose, v, w, dt)


def update_from_encoder_increments(pose: np.ndarray, dphi_l: float, dphi_r: float, p: DiffDriveParams) -> np.ndarray:
    """Pose update from wheel encoder increments over a sampling interval."""
    x, y, th = float(pose[0]), float(pose[1]), float(pose[2])
    ds_l = p.r * dphi_l
    ds_r = p.r * dphi_r
    ds = 0.5 * (ds_r + ds_l)
    dth = (ds_r - ds_l) / p.L

    if abs(dth) < 1e-12:
        return np.array([x + ds * np.cos(th),
                         y + ds * np.sin(th),
                         th], dtype=float)

    th2 = th + dth
    x2 = x + (ds / dth) * (np.sin(th2) - np.sin(th))
    y2 = y - (ds / dth) * (np.cos(th2) - np.cos(th))
    return np.array([x2, y2, th2], dtype=float)


def simulate_constant_wheels(pose0: np.ndarray, phi_dot_l: float, phi_dot_r: float,
                             p: DiffDriveParams, dt: float, N: int) -> np.ndarray:
    pose = np.array(pose0, dtype=float)
    traj = np.zeros((N + 1, 3), dtype=float)
    traj[0] = pose
    for k in range(N):
        pose = step_from_wheels(pose, phi_dot_l, phi_dot_r, p, dt)
        traj[k + 1] = pose
    return traj


if __name__ == "__main__":
    p = DiffDriveParams(r=0.05, L=0.30)
    pose0 = np.array([0.0, 0.0, 0.0], dtype=float)

    phi_dot_l, phi_dot_r = 5.0, 8.0  # rad/s
    dt, N = 0.1, 50
    traj = simulate_constant_wheels(pose0, phi_dot_l, phi_dot_r, p, dt, N)

    v, w = body_twist_from_wheels(phi_dot_l, phi_dot_r, p)
    print(f"v = {v:.4f} m/s, w = {w:.4f} rad/s")
    print("Final pose [x, y, theta] =", traj[-1])

    pl, pr = wheels_from_body_twist(v, w, p)
    print("Recovered wheel rates [phi_dot_l, phi_dot_r] =", (pl, pr))

    dphi_l = phi_dot_l * dt
    dphi_r = phi_dot_r * dt
    pose = pose0.copy()
    for _ in range(N):
        pose = update_from_encoder_increments(pose, dphi_l, dphi_r, p)
    print("Final pose from encoder increments =", pose)
      

Chapter2_Lesson2_Ex1.py

"""Chapter 2 - Lesson 2 (Exercise 1): Estimate turning radius and curvature from wheel rates."""

from __future__ import annotations
from dataclasses import dataclass
import numpy as np


@dataclass(frozen=True)
class DiffDriveParams:
    r: float
    L: float


def twist(phi_dot_l: float, phi_dot_r: float, p: DiffDriveParams) -> tuple[float, float]:
    v_l = p.r * phi_dot_l
    v_r = p.r * phi_dot_r
    v = 0.5 * (v_r + v_l)
    w = (v_r - v_l) / p.L
    return float(v), float(w)


def icc_radius(v: float, w: float) -> float:
    if abs(w) < 1e-12:
        return float("inf")
    return v / w


def curvature(v: float, w: float) -> float:
    if abs(v) < 1e-12:
        return float("inf")
    return w / v


if __name__ == "__main__":
    p = DiffDriveParams(r=0.05, L=0.30)
    pairs = [(5, 5), (5, 8), (8, 5), (-5, 5), (0, 6)]

    print("phi_dot_l  phi_dot_r   v[m/s]    w[rad/s]   R[m]      kappa[1/m]")
    for pl, pr in pairs:
        v, w = twist(pl, pr, p)
        R = icc_radius(v, w)
        k = curvature(v, w)
        print(f"{pl:8.2f}  {pr:8.2f}  {v:8.4f}  {w:10.4f}  {R:8.4f}  {k:12.4f}")
      

Chapter2_Lesson2.cpp

// Chapter 2 - Lesson 2: Differential Drive Kinematics (C++)
//
// Build (example):
//   g++ -O2 -std=c++17 Chapter2_Lesson2.cpp -o diffdrive

#include <iostream>
#include <cmath>
#include <array>

struct DiffDriveParams {
    double r; // wheel radius [m]
    double L; // axle length [m]
};

struct Pose2D {
    double x;
    double y;
    double theta;
};

static std::array<double,2> body_twist_from_wheels(double phi_dot_l, double phi_dot_r, const DiffDriveParams& p) {
    const double v_l = p.r * phi_dot_l;
    const double v_r = p.r * phi_dot_r;
    const double v   = 0.5 * (v_r + v_l);
    const double w   = (v_r - v_l) / p.L;
    return {v, w};
}

static std::array<double,2> wheels_from_body_twist(double v, double w, const DiffDriveParams& p) {
    const double phi_dot_r = (v + 0.5 * p.L * w) / p.r;
    const double phi_dot_l = (v - 0.5 * p.L * w) / p.r;
    return {phi_dot_l, phi_dot_r};
}

static Pose2D integrate_pose_exact(const Pose2D& pose, double v, double w, double dt) {
    const double x = pose.x;
    const double y = pose.y;
    const double th = pose.theta;

    if (std::abs(w) < 1e-12) {
        return Pose2D{
            x + v * dt * std::cos(th),
            y + v * dt * std::sin(th),
            th
        };
    }

    const double th2 = th + w * dt;
    const double x2 = x + (v / w) * (std::sin(th2) - std::sin(th));
    const double y2 = y - (v / w) * (std::cos(th2) - std::cos(th));
    return Pose2D{x2, y2, th2};
}

static Pose2D step_from_wheels(const Pose2D& pose, double phi_dot_l, double phi_dot_r, const DiffDriveParams& p, double dt) {
    const auto vw = body_twist_from_wheels(phi_dot_l, phi_dot_r, p);
    return integrate_pose_exact(pose, vw[0], vw[1], dt);
}

int main() {
    DiffDriveParams p{0.05, 0.30};
    Pose2D pose{0.0, 0.0, 0.0};

    const double phi_dot_l = 5.0;
    const double phi_dot_r = 8.0;
    const double dt = 0.1;
    const int N = 50;

    auto vw = body_twist_from_wheels(phi_dot_l, phi_dot_r, p);
    std::cout << "v = " << vw[0] << " m/s, w = " << vw[1] << " rad/s\n";

    for (int k = 0; k < N; ++k) {
        pose = step_from_wheels(pose, phi_dot_l, phi_dot_r, p, dt);
    }

    std::cout << "Final pose [x, y, theta] = ["
              << pose.x << ", " << pose.y << ", " << pose.theta << "]\n";

    auto lr = wheels_from_body_twist(vw[0], vw[1], p);
    std::cout << "Inverse check [phi_dot_l, phi_dot_r] = ["
              << lr[0] << ", " << lr[1] << "]\n";
    return 0;
}
      

Chapter2_Lesson2.java

// Chapter 2 - Lesson 2: Differential Drive Kinematics (Java)
//
// Compile and run:
//   javac Chapter2_Lesson2.java
//   java Chapter2_Lesson2

public class Chapter2_Lesson2 {

    public static final class DiffDriveParams {
        public final double r; // wheel radius [m]
        public final double L; // axle length [m]
        public DiffDriveParams(double r, double L) {
            this.r = r;
            this.L = L;
        }
    }

    public static final class Pose2D {
        public final double x, y, theta;
        public Pose2D(double x, double y, double theta) {
            this.x = x; this.y = y; this.theta = theta;
        }
    }

    public static double[] bodyTwistFromWheels(double phiDotL, double phiDotR, DiffDriveParams p) {
        double vL = p.r * phiDotL;
        double vR = p.r * phiDotR;
        double v  = 0.5 * (vR + vL);
        double w  = (vR - vL) / p.L;
        return new double[]{v, w};
    }

    public static double[] wheelsFromBodyTwist(double v, double w, DiffDriveParams p) {
        double phiDotR = (v + 0.5 * p.L * w) / p.r;
        double phiDotL = (v - 0.5 * p.L * w) / p.r;
        return new double[]{phiDotL, phiDotR};
    }

    public static Pose2D integratePoseExact(Pose2D pose, double v, double w, double dt) {
        double x = pose.x, y = pose.y, th = pose.theta;

        if (Math.abs(w) < 1e-12) {
            return new Pose2D(
                    x + v * dt * Math.cos(th),
                    y + v * dt * Math.sin(th),
                    th
            );
        }

        double th2 = th + w * dt;
        double x2 = x + (v / w) * (Math.sin(th2) - Math.sin(th));
        double y2 = y - (v / w) * (Math.cos(th2) - Math.cos(th));
        return new Pose2D(x2, y2, th2);
    }

    public static Pose2D stepFromWheels(Pose2D pose, double phiDotL, double phiDotR, DiffDriveParams p, double dt) {
        double[] vw = bodyTwistFromWheels(phiDotL, phiDotR, p);
        return integratePoseExact(pose, vw[0], vw[1], dt);
    }

    public static void main(String[] args) {
        DiffDriveParams p = new DiffDriveParams(0.05, 0.30);
        Pose2D pose = new Pose2D(0.0, 0.0, 0.0);

        double phiDotL = 5.0;
        double phiDotR = 8.0;
        double dt = 0.1;
        int N = 50;

        double[] vw = bodyTwistFromWheels(phiDotL, phiDotR, p);
        System.out.printf("v = %.6f m/s, w = %.6f rad/s%n", vw[0], vw[1]);

        for (int k = 0; k < N; k++) {
            pose = stepFromWheels(pose, phiDotL, phiDotR, p, dt);
        }

        System.out.printf("Final pose [x, y, theta] = [%.6f, %.6f, %.6f]%n",
                pose.x, pose.y, pose.theta);

        double[] lr = wheelsFromBodyTwist(vw[0], vw[1], p);
        System.out.printf("Inverse check [phiDotL, phiDotR] = [%.6f, %.6f]%n",
                lr[0], lr[1]);
    }
}
      

Chapter2_Lesson2.m

% Chapter 2 - Lesson 2: Differential Drive Kinematics (MATLAB/Simulink)
%
% This script demonstrates:
%   - wheel rates -> (v,w)
%   - exact discrete-time integration
%   - optional use of robotics.DifferentialDriveKinematics (Robotics System Toolbox)

clear; clc;

r = 0.05;   % wheel radius [m]
L = 0.30;   % axle length [m]

phiDotL = 5.0;   % rad/s
phiDotR = 8.0;   % rad/s
dt = 0.1;        % s
N = 50;

pose = [0; 0; 0]; % [x; y; theta]

% Forward map: wheel rates -> (v,w)
vL = r * phiDotL;
vR = r * phiDotR;
v  = 0.5*(vR + vL);
w  = (vR - vL)/L;
fprintf('v = %.6f m/s, w = %.6f rad/s\n', v, w);

% Simulate with exact integration
traj = zeros(3, N+1);
traj(:,1) = pose;
for k = 1:N
    pose = integratePoseExact(pose, v, w, dt);
    traj(:,k+1) = pose;
end
fprintf('Final pose [x,y,theta] = [%.6f, %.6f, %.6f]\n', traj(1,end), traj(2,end), traj(3,end));

% Inverse map: (v,w) -> wheel rates
phiDotR2 = (v + 0.5*L*w)/r;
phiDotL2 = (v - 0.5*L*w)/r;
fprintf('Inverse check [phiDotL, phiDotR] = [%.6f, %.6f]\n', phiDotL2, phiDotR2);

% Optional: Robotics System Toolbox
if exist('robotics.DifferentialDriveKinematics','class') == 8
    kin = robotics.DifferentialDriveKinematics('TrackWidth', L, 'WheelRadius', r);
    poseObj = [0 0 0];
    for k = 1:N
        poseObj = poseObj + derivative(kin, poseObj, [v w])' * dt;
    end
    fprintf('Toolbox (Euler) pose [x,y,theta] = [%.6f, %.6f, %.6f]\n', poseObj(1), poseObj(2), poseObj(3));
end

function poseNext = integratePoseExact(pose, v, w, dt)
    x = pose(1); y = pose(2); th = pose(3);
    if abs(w) < 1e-12
        poseNext = [x + v*dt*cos(th);
                    y + v*dt*sin(th);
                    th];
        return;
    end
    th2 = th + w*dt;
    x2 = x + (v/w)*(sin(th2) - sin(th));
    y2 = y - (v/w)*(cos(th2) - cos(th));
    poseNext = [x2; y2; th2];
end
      

Chapter2_Lesson2.nb

(* ::Title:: *)
(* Chapter 2 - Lesson 2: Differential Drive Kinematics (Wolfram Mathematica) *)

ClearAll["Global`*"];

r = 0.05;   (* wheel radius [m] *)
L = 0.30;   (* axle length [m] *)

phiDotL = 5.0;
phiDotR = 8.0;

vL = r*phiDotL;
vR = r*phiDotR;

v = (vR + vL)/2;
w = (vR - vL)/L;

Print["v = ", N[v], " m/s, w = ", N[w], " rad/s"];

integratePoseExact[{x_, y_, th_}, v_?NumericQ, w_?NumericQ, dt_?NumericQ] :=
  Module[{th2},
    If[Abs[w] < 10^-12,
      {x + v*dt*Cos[th], y + v*dt*Sin[th], th},
      th2 = th + w*dt;
      {x + (v/w) (Sin[th2] - Sin[th]),
       y - (v/w) (Cos[th2] - Cos[th]),
       th2}
    ]
  ];

dt = 0.1; Nsteps = 50;
traj = NestList[integratePoseExact[#, v, w, dt] &, {0., 0., 0.}, Nsteps];
Print["Final pose = ", N[Last[traj]]];

phiDotR2 = (v + (L/2) w)/r;
phiDotL2 = (v - (L/2) w)/r;
Print["Inverse check (phiDotL, phiDotR) = ", N[{phiDotL2, phiDotR2}]];

sol = DSolveValue[{x'[t] == v Cos[th[t]], y'[t] == v Sin[th[t]], th'[t] == w,
                   x[0] == x0, y[0] == y0, th[0] == th0},
                  {x[t], y[t], th[t]}, t];
Print["Closed-form solution x(t), y(t), th(t):"];
Print[sol];
      

Problem 1 (Forward Kinematics from Wheel Rates): A differential-drive robot has \( r = 0.06 \) m and \( L = 0.32 \) m. If \( \dot{\phi}_L = 4 \) rad/s and \( \dot{\phi}_R = 7 \) rad/s, compute \( v \), \( \omega \), and the ICC radius \( R \).

Solution: \( v_L = r\dot{\phi}_L = 0.06\times4 = 0.24 \) m/s, \( v_R = r\dot{\phi}_R = 0.06\times7 = 0.42 \) m/s.

\[ v = \frac{v_R+v_L}{2} = \frac{0.42+0.24}{2} = 0.33\;\text{m/s},\qquad \omega = \frac{v_R-v_L}{L} = \frac{0.42-0.24}{0.32} = 0.5625\;\text{rad/s}. \]

Since \( \omega \neq 0 \), the ICC radius is \( R = v/\omega = 0.33/0.5625 \approx 0.5867 \) m.

Problem 2 (Inverse Kinematics with Limits): Suppose \( r = 0.05 \) m, \( L = 0.30 \) m, and maximum wheel rate is \( \dot{\phi}_{\max} = 10 \) rad/s. Determine whether the command \( v = 0.45 \) m/s and \( \omega = 2.0 \) rad/s is feasible.

Solution: Compute required wheel rates:

\[ \dot{\phi}_R = \frac{1}{r}\left(v + \frac{L}{2}\omega\right) = \frac{1}{0.05}\left(0.45 + 0.15\times2.0\right) = \frac{0.75}{0.05} = 15, \]

\[ \dot{\phi}_L = \frac{1}{r}\left(v - \frac{L}{2}\omega\right) = \frac{1}{0.05}\left(0.45 - 0.15\times2.0\right) = \frac{0.15}{0.05} = 3. \]

The right wheel requires \( \dot{\phi}_R = 15 \) rad/s which violates \( \dot{\phi}_{\max}=10 \). Therefore the command is not feasible under the wheel-rate limit.

Problem 3 (Nonholonomic Constraint Verification): Starting from \( \dot{x}=v\cos\theta \) and \( \dot{y}=v\sin\theta \), prove that the constraint \( -\sin\theta\,\dot{x} + \cos\theta\,\dot{y} = 0 \) always holds.

Solution: Substitute and simplify:

\[ -\sin\theta\,(v\cos\theta) + \cos\theta\,(v\sin\theta) = -v\sin\theta\cos\theta + v\cos\theta\sin\theta = 0. \]

Thus any motion generated by the differential-drive kinematics satisfies the no-lateral-velocity constraint at the axle midpoint.

Problem 4 (Exact Discrete Update Derivation): Assume \( v \) and \( \omega \) are constant over \( \Delta t \) and \( \omega \neq 0 \). Derive the closed-form update for \( x_{k+1} \) and \( y_{k+1} \).

Solution: With \( \theta(t)=\theta_k+\omega t \):

\[ x_{k+1}-x_k = \int_0^{\Delta t} v\cos(\theta_k+\omega t)\,dt = \frac{v}{\omega}\left[\sin(\theta_k+\omega t)\right]_0^{\Delta t} = \frac{v}{\omega}\left(\sin(\theta_k+\omega\Delta t)-\sin\theta_k\right), \]

\[ y_{k+1}-y_k = \int_0^{\Delta t} v\sin(\theta_k+\omega t)\,dt = -\frac{v}{\omega}\left[\cos(\theta_k+\omega t)\right]_0^{\Delta t} = -\frac{v}{\omega}\left(\cos(\theta_k+\omega\Delta t)-\cos\theta_k\right). \]

Together with \( \theta_{k+1}=\theta_k+\omega\Delta t \), this yields the update in Section 6.

Problem 5 (Wheel Rates for a Desired Circular Path): A robot must drive a circle of radius \( R = 1.2 \) m at constant forward speed \( v=0.30 \) m/s. Given \( r=0.05 \) m and \( L=0.30 \) m, compute \( \dot{\phi}_L,\dot{\phi}_R \).

Solution: The yaw rate is \( \omega = v/R = 0.30/1.2 = 0.25 \) rad/s. Then:

\[ \dot{\phi}_R = \frac{1}{r}\left(v + \frac{L}{2}\omega\right) = \frac{1}{0.05}\left(0.30 + 0.15\times0.25\right) = \frac{0.3375}{0.05} = 6.75, \]

\[ \dot{\phi}_L = \frac{1}{r}\left(v - \frac{L}{2}\omega\right) = \frac{1}{0.05}\left(0.30 - 0.15\times0.25\right) = \frac{0.2625}{0.05} = 5.25. \]

Therefore \( \dot{\phi}_L=5.25 \) rad/s and \( \dot{\phi}_R=6.75 \) rad/s.

We derived differential-drive kinematics from pure rolling constraints, established the forward map from wheel rates to body twist and pose rates, proved the induced nonholonomic constraint, and obtained an exact discrete-time pose update under piecewise-constant twist (and its encoder-increment form). These results are the implementation backbone for odometry (Chapter 5) and for controller design and motion feasibility (Chapter 3) in later lessons.