In ground AMR, the dominant feasibility limiter at low-to-moderate speeds is often wheel–ground contact kinematics: ideal wheels roll along their plane and do not slip sideways. This yields constraints of the form \( \mathbf{A}(\mathbf{q})\dot{\mathbf{q}} = \mathbf{0} \): linear constraints on instantaneous generalized velocities, where \( \mathbf{q} \) is the planar pose (and possibly steering states).

The applied workflow is: (i) state the physical assumptions (ideal rolling), (ii) write wheel contact velocities, (iii) project onto “no-slip” directions, and (iv) stack constraints into a matrix that can be checked numerically.

flowchart TD
  A["Assumptions: rigid wheel, no lateral slip, planar ground"] --> B["Compute contact point velocity"]
  B --> C["Project velocity onto forbidden directions"]
  C --> D["Stack constraints: A(q) * qdot = 0"]
  D --> E["Check feasibility: residual norm near 0?"]
  E --> F["If violated: slip model or revise command"]
        

Consider planar motion with robot pose \( \mathbf{q} = [x,\;y,\;\theta]^\top \in \mathbb{R}^3 \): position \( (x,y) \) and yaw \( \theta \). Let wheel \( i \) be located at a fixed offset in the body frame \( \mathbf{r}_i^b = [a_i,\;b_i]^\top \) and have rolling direction rotated by \( \alpha_i \) relative to the body \( x_b \)-axis.

The world-frame position of the wheel contact point is \( \mathbf{p}_i = [x,\;y]^\top + \mathbf{R}(\theta)\mathbf{r}_i^b \): where \( \mathbf{R}(\theta)=\begin{bmatrix}\cos\theta & -\sin\theta\\ \sin\theta & \cos\theta\end{bmatrix} \). Differentiating yields the contact point velocity \( \dot{\mathbf{p}}_i = [\dot{x},\;\dot{y}]^\top + \dot{\theta}\,\mathbf{k}\times(\mathbf{R}(\theta)\mathbf{r}_i^b) \): with the planar cross product giving \( \mathbf{k}\times [u,\;v]^\top = [-v,\;u]^\top \).

Define the wheel’s lateral (no-slip) unit direction in the world frame as \( \mathbf{n}_i(\theta) = [-\sin(\theta+\alpha_i),\;\cos(\theta+\alpha_i)]^\top \): i.e., perpendicular to the rolling direction. The ideal lateral no-slip condition is:

\[ \mathbf{n}_i(\theta)^\top \dot{\mathbf{p}}_i = 0. \]

Substituting \( \dot{\mathbf{p}}_i \) and simplifying (key identity shown below) yields a Pfaffian constraint:

\[ -\sin(\theta+\alpha_i)\,\dot{x} + \cos(\theta+\alpha_i)\,\dot{y} + \left(a_i\cos\alpha_i + b_i\sin\alpha_i\right)\dot{\theta} = 0. \]

Applied derivation (core simplification). Let \( \mathbf{r}_i^w=\mathbf{R}(\theta)\mathbf{r}_i^b = [r_x,\;r_y]^\top \). Then \( \mathbf{k}\times \mathbf{r}_i^w = [-r_y,\;r_x]^\top \), and \( \mathbf{n}_i^\top(\mathbf{k}\times \mathbf{r}_i^w) = \sin(\theta+\alpha_i)r_y + \cos(\theta+\alpha_i)r_x \). With \( r_x=a_i\cos\theta - b_i\sin\theta \), \( r_y=a_i\sin\theta + b_i\cos\theta \), one obtains the identity

\[ \sin(\theta+\alpha_i)r_y + \cos(\theta+\alpha_i)r_x = a_i\cos\alpha_i + b_i\sin\alpha_i, \]

which is independent of \( \theta \) and makes the constraint easy to evaluate online.

Stack \( m \) wheel lateral constraints into matrix form: \( \mathbf{A}(\mathbf{q})\dot{\mathbf{q}} = \mathbf{0} \):

\[ \mathbf{A}(\mathbf{q}) = \begin{bmatrix} -\sin(\theta+\alpha_1) & \cos(\theta+\alpha_1) & a_1\cos\alpha_1 + b_1\sin\alpha_1 \\ \vdots & \vdots & \vdots \\ -\sin(\theta+\alpha_m) & \cos(\theta+\alpha_m) & a_m\cos\alpha_m + b_m\sin\alpha_m \end{bmatrix}, \quad \dot{\mathbf{q}} = \begin{bmatrix}\dot{x}\\ \dot{y}\\ \dot{\theta}\end{bmatrix}. \]

The instantaneous feasible velocity set is the null space \( \mathcal{N}(\mathbf{A}(\mathbf{q})) \): all \( \dot{\mathbf{q}} \) satisfying the constraints at that configuration. In planar kinematics, the unconstrained velocity space is 3D; constraints reduce it by \( \operatorname{rank}(\mathbf{A}) \).

\[ \dim\left(\mathcal{N}(\mathbf{A}(\mathbf{q}))\right) = 3 - \operatorname{rank}(\mathbf{A}(\mathbf{q})). \]

Applied interpretation. If \( \operatorname{rank}(\mathbf{A}) = 1 \), there is one forbidden instantaneous direction (typically sideways slip), leaving a 2D feasible velocity subspace (e.g., forward + yaw). If \( \operatorname{rank}(\mathbf{A}) = 2 \), only a 1D motion subspace remains (highly constrained; e.g., a “knife edge” wheel arrangement).

A standard differential-drive robot has two fixed wheels aligned with the body \( x_b \)-axis, placed at \( \mathbf{r}_L^b=[0,\; +b]^\top \) and \( \mathbf{r}_R^b=[0,\; -b]^\top \), with wheel rolling direction \( \alpha_L=\alpha_R=0 \). The lateral no-slip constraint from Section 2 becomes identical for both wheels, so the stacked matrix has rank 1:

\[ -\sin\theta\,\dot{x} + \cos\theta\,\dot{y} = 0. \]

This says the body-frame lateral velocity must be zero. Equivalently, if we define body-frame velocities \( v_x = \cos\theta\,\dot{x} + \sin\theta\,\dot{y} \) and \( v_y = -\sin\theta\,\dot{x} + \cos\theta\,\dot{y} \), then the constraint is simply \( v_y = 0 \).

To connect motion to actuators, add the longitudinal rolling constraints: each wheel’s forward contact speed equals wheel radius times wheel angular rate. Under the ideal differential-drive geometry, the standard mapping is:

\[ v = \frac{r}{2}\left(\omega_R + \omega_L\right),\qquad \dot{\theta} = \frac{r}{2b}\left(\omega_R - \omega_L\right), \]

and the world-frame kinematics follow:

\[ \dot{x} = v\cos\theta,\qquad \dot{y} = v\sin\theta,\qquad \dot{\theta} = \frac{r}{2b}\left(\omega_R - \omega_L\right). \]

flowchart TD
  W["Wheel rates (omega_L, omega_R)"] --> M["Map to (v, yaw_rate)"]
  M --> K["Integrate kinematics -> (x, y, theta)"]
  K --> C["Check: -sin(theta)*xdot + cos(theta)*ydot = 0"]
        

Applied note. In real AMR, a nonzero constraint residual often indicates slip (low friction, aggressive maneuvers, deformable terrain), wheel radius mismatch, or timing/synchronization issues between sensing and actuation. Here we model the ideal case to build a baseline feasibility check.

For a car-like robot on flat ground, a common kinematic state is \( \mathbf{q} = [x,\;y,\;\theta,\;\delta]^\top \): pose plus steering angle \( \delta \). The rear axle imposes the same lateral no-slip constraint as the differential-drive case (rear wheels aligned with body axis):

\[ -\sin\theta\,\dot{x} + \cos\theta\,\dot{y} = 0. \]

The steering geometry yields the familiar relationship between yaw rate and forward speed:

\[ \dot{x} = v\cos\theta,\qquad \dot{y} = v\sin\theta,\qquad \dot{\theta} = \frac{v}{L}\tan\delta,\qquad \dot{\delta} = u_\delta, \]

where \( L \) is wheelbase and \( u_\delta \) is the steering actuation rate. In an applied feasibility sense, the nonholonomic restriction is still “no instantaneous lateral velocity at the rear axle,” but the steering state controls how yaw couples to forward motion.

A single Pfaffian constraint in planar motion can be written as a 1-form \( \omega(\mathbf{q}) = \mathbf{a}(\mathbf{q})^\top d\mathbf{q} \): and the admissible velocities satisfy \( \omega(\mathbf{q})(\dot{\mathbf{q}})=0 \). For the common wheel-aligned constraint,

\[ \omega = -\sin\theta\,dx + \cos\theta\,dy. \]

Claim. The constraint is nonholonomic (nonintegrable): there does not exist a scalar function \( h(x,y,\theta) \) such that \( \omega = dh \) (even locally, generically).

Proof (Frobenius in 3D, applied). Compute the exterior derivative:

\[ d\omega = -\cos\theta\,d\theta\wedge dx - \sin\theta\,d\theta\wedge dy. \]

Frobenius’ integrability condition for a single 1-form in a 3D manifold is: the distribution \( \omega=0 \) is integrable iff \( \omega\wedge d\omega = 0 \). Here,

\[ \begin{aligned} \omega\wedge d\omega &= \left(-\sin\theta\,dx + \cos\theta\,dy\right)\wedge \left(-\cos\theta\,d\theta\wedge dx - \sin\theta\,d\theta\wedge dy\right) \\ &= \left(\sin^2\theta + \cos^2\theta\right)\,dx\wedge dy\wedge d\theta \\ &= dx\wedge dy\wedge d\theta \neq 0. \end{aligned} \]

Since \( \omega\wedge d\omega \neq 0 \), the constraint is nonintegrable and therefore nonholonomic. Applied takeaway: there is no scalar “side-slip forbidden surface” in \( (x,y,\theta) \)-space whose level sets encode the constraint; the restriction is intrinsically velocity-level.

Given any candidate trajectory \( \mathbf{q}(t) \), define the constraint residual \( \mathbf{r}(t) = \mathbf{A}(\mathbf{q}(t))\dot{\mathbf{q}}(t) \):

\[ \mathbf{r}(t) = \mathbf{A}(\mathbf{q}(t))\dot{\mathbf{q}}(t),\qquad \text{feasible (ideal)} \;\;\Longleftrightarrow\;\; \|\mathbf{r}(t)\| \approx 0\;\;\forall t. \]

In experiments, you interpret \( \|\mathbf{r}(t)\| \) as “how much lateral slip (or model mismatch) is present,” after accounting for differentiation noise and time alignment. The multi-language labs below simulate an ideal differential-drive model and report the maximum residual.

Each implementation simulates differential-drive kinematics using wheel rates and evaluates the Pfaffian residual \( -\sin\theta\,\dot{x} + \cos\theta\,\dot{y} \): ideally near zero.

8.1 Python (NumPy/Matplotlib; optional SymPy)

Code: Chapter3_Lesson1.py

"""
Chapter 3 — Nonholonomic Motion and Feasibility for AMR
Lesson 1 — Nonholonomic Constraints in Wheeled Robots (applied view)

This script:
  1) simulates a differential-drive robot using wheel angular rates
  2) checks the lateral no-slip Pfaffian constraint residual
  3) visualizes the resulting planar trajectory and constraint violation

Dependencies:
  - numpy, matplotlib (standard scientific Python stack)
Optional (symbolic addendum):
  - sympy
"""

from __future__ import annotations

import numpy as np
import matplotlib.pyplot as plt


def diff_drive_twist(omega_l: float, omega_r: float, r: float, b: float) -> tuple[float, float]:
    """
    Map wheel angular rates (rad/s) to body-frame forward speed v (m/s)
    and yaw rate w (rad/s).

        v = (r/2) (omega_r + omega_l)
        w = (r/(2b)) (omega_r - omega_l)

    where b is half the wheel track (distance from centerline to each wheel).
    """
    v = 0.5 * r * (omega_r + omega_l)
    w = 0.5 * r * (omega_r - omega_l) / b
    return v, w


def unicycle_step(x: float, y: float, theta: float, v: float, w: float, dt: float) -> tuple[float, float, float]:
    """
    Euler step for the unicycle kinematics:
        x_dot = v cos(theta)
        y_dot = v sin(theta)
        theta_dot = w
    """
    x = x + dt * v * np.cos(theta)
    y = y + dt * v * np.sin(theta)
    theta = theta + dt * w
    # wrap angle for nicer plots
    theta = (theta + np.pi) % (2 * np.pi) - np.pi
    return x, y, theta


def lateral_constraint_residual(x_dot: float, y_dot: float, theta: float) -> float:
    """
    Pfaffian lateral no-slip constraint for a wheel aligned with body x-axis:
        -sin(theta) x_dot + cos(theta) y_dot = 0

    Returns the scalar residual (should be ~0 for an ideal no-slip model).
    """
    return -np.sin(theta) * x_dot + np.cos(theta) * y_dot


def wheel_profile(t: float) -> tuple[float, float]:
    """
    Simple piecewise wheel rate profile to excite both translation and rotation.
    """
    if t < 4.0:
        return 6.0, 6.0          # straight
    elif t < 8.0:
        return 3.0, 7.0          # turn left
    elif t < 12.0:
        return 7.0, 3.0          # turn right
    else:
        return 5.0, 5.0          # straight


def run_sim(T: float = 16.0, dt: float = 0.01, r: float = 0.10, b: float = 0.22) -> dict[str, np.ndarray]:
    """
    Simulate the differential-drive kinematics for duration T.
    """
    N = int(np.floor(T / dt)) + 1
    t = np.linspace(0.0, T, N)

    x = np.zeros(N)
    y = np.zeros(N)
    th = np.zeros(N)

    v = np.zeros(N)
    w = np.zeros(N)
    res = np.zeros(N)

    for k in range(N - 1):
        om_l, om_r = wheel_profile(t[k])
        v[k], w[k] = diff_drive_twist(om_l, om_r, r=r, b=b)

        # implied world-frame velocities
        x_dot = v[k] * np.cos(th[k])
        y_dot = v[k] * np.sin(th[k])
        res[k] = lateral_constraint_residual(x_dot, y_dot, th[k])

        x[k + 1], y[k + 1], th[k + 1] = unicycle_step(x[k], y[k], th[k], v[k], w[k], dt)

    # last sample
    v[-1], w[-1] = v[-2], w[-2]
    x_dot = v[-1] * np.cos(th[-1])
    y_dot = v[-1] * np.sin(th[-1])
    res[-1] = lateral_constraint_residual(x_dot, y_dot, th[-1])

    return {"t": t, "x": x, "y": y, "theta": th, "v": v, "w": w, "res": res}


def plot_results(sim: dict[str, np.ndarray]) -> None:
    t, x, y, res = sim["t"], sim["x"], sim["y"], sim["res"]

    plt.figure()
    plt.plot(x, y)
    plt.axis("equal")
    plt.xlabel("x [m]")
    plt.ylabel("y [m]")
    plt.title("Differential-drive trajectory (ideal no-slip)")

    plt.figure()
    plt.plot(t, res)
    plt.xlabel("t [s]")
    plt.ylabel("constraint residual")
    plt.title("Lateral no-slip Pfaffian constraint residual (should be ~0)")
    plt.show()


def symbolic_nonintegrability_demo() -> None:
    """
    Optional: symbolic check that the 1-form omega = -sin(theta) dx + cos(theta) dy
    is non-integrable via omega ^ d(omega) != 0.

    Requires: sympy
    """
    try:
        import sympy as sp
    except Exception as e:
        print("sympy not available:", e)
        return

    x, y, th = sp.symbols("x y th", real=True)
    dx, dy, dth = sp.symbols("dx dy dth")
    # We emulate wedge products with formal basis and antisymmetry by hand (minimal demo).
    # Known result: omega ^ d omega = dx ^ dy ^ dth (nonzero).
    print("Known Frobenius test result: omega ^ d(omega) = dx ^ dy ^ dth != 0 -> nonholonomic.")


if __name__ == "__main__":
    sim = run_sim()
    print("max |constraint residual| =", np.max(np.abs(sim["res"])))
    plot_results(sim)

    # Uncomment if you want the symbolic addendum (requires sympy)
    # symbolic_nonintegrability_demo()
      

8.2 C++ (portable simulator; typical robotics stack: Eigen/ROS2 optional)

Code: Chapter3_Lesson1.cpp

/*
Chapter 3 — Nonholonomic Motion and Feasibility for AMR
Lesson 1 — Nonholonomic Constraints in Wheeled Robots (applied view)

Differential-drive kinematic simulation and constraint check.

Build (example):
  g++ -O2 -std=c++17 Chapter3_Lesson1.cpp -o Chapter3_Lesson1

Optional robotics-adjacent dependency:
  - Eigen (linear algebra) can be used, but this example stays standard-library only.
*/

#include <cmath>
#include <iostream>
#include <vector>

struct SimResult {
    std::vector<double> t, x, y, th, v, w, res;
};

static inline void diff_drive_twist(double omega_l, double omega_r, double r, double b,
                                    double& v_out, double& w_out) {
    v_out = 0.5 * r * (omega_r + omega_l);
    w_out = 0.5 * r * (omega_r - omega_l) / b;
}

static inline double lateral_constraint_residual(double x_dot, double y_dot, double theta) {
    // -sin(theta) x_dot + cos(theta) y_dot = 0
    return -std::sin(theta) * x_dot + std::cos(theta) * y_dot;
}

static inline void unicycle_step(double& x, double& y, double& theta, double v, double w, double dt) {
    x += dt * v * std::cos(theta);
    y += dt * v * std::sin(theta);
    theta += dt * w;

    // wrap to [-pi, pi]
    theta = std::fmod(theta + M_PI, 2.0 * M_PI);
    if (theta < 0) theta += 2.0 * M_PI;
    theta -= M_PI;
}

static inline void wheel_profile(double t, double& omega_l, double& omega_r) {
    if (t < 4.0) {
        omega_l = 6.0; omega_r = 6.0;
    } else if (t < 8.0) {
        omega_l = 3.0; omega_r = 7.0;
    } else if (t < 12.0) {
        omega_l = 7.0; omega_r = 3.0;
    } else {
        omega_l = 5.0; omega_r = 5.0;
    }
}

SimResult run_sim(double T = 16.0, double dt = 0.01, double r = 0.10, double b = 0.22) {
    const int N = static_cast<int>(std::floor(T / dt)) + 1;

    SimResult s;
    s.t.resize(N); s.x.resize(N); s.y.resize(N); s.th.resize(N);
    s.v.resize(N); s.w.resize(N); s.res.resize(N);

    double x = 0.0, y = 0.0, th = 0.0;

    for (int k = 0; k < N; ++k) {
        const double tk = k * dt;
        s.t[k] = tk;

        double om_l = 0.0, om_r = 0.0;
        wheel_profile(tk, om_l, om_r);

        double v = 0.0, w = 0.0;
        diff_drive_twist(om_l, om_r, r, b, v, w);

        const double x_dot = v * std::cos(th);
        const double y_dot = v * std::sin(th);

        s.x[k] = x; s.y[k] = y; s.th[k] = th;
        s.v[k] = v; s.w[k] = w;
        s.res[k] = lateral_constraint_residual(x_dot, y_dot, th);

        if (k < N - 1) {
            unicycle_step(x, y, th, v, w, dt);
        }
    }
    return s;
}

int main() {
    auto sim = run_sim();

    double max_abs = 0.0;
    for (double e : sim.res) max_abs = std::max(max_abs, std::abs(e));

    std::cout << "max |constraint residual| = " << max_abs << "\n";
    std::cout << "final pose: x=" << sim.x.back() << " y=" << sim.y.back()
              << " theta=" << sim.th.back() << "\n";

    // For plotting, export CSV if needed (left as exercise)
    return 0;
}
      

8.3 Java (portable; typical robotics stack: EJML/JOML/ROSJava optional)

Code: Chapter3_Lesson1.java

/*
Chapter 3 — Nonholonomic Motion and Feasibility for AMR
Lesson 1 — Nonholonomic Constraints in Wheeled Robots (applied view)

Differential-drive kinematic simulation and lateral constraint residual check.

Build:
  javac Chapter3_Lesson1.java
Run:
  java Chapter3_Lesson1

Robotics-adjacent libraries you may integrate in larger projects:
  - EJML (linear algebra), JOML (geometry), ROSJava (middleware bindings)
  - This example uses only core Java for portability.
*/

public class Chapter3_Lesson1 {

    static class SimResult {
        double[] t, x, y, th, v, w, res;
    }

    static void diffDriveTwist(double omegaL, double omegaR, double r, double b, double[] outVW) {
        double v = 0.5 * r * (omegaR + omegaL);
        double w = 0.5 * r * (omegaR - omegaL) / b;
        outVW[0] = v;
        outVW[1] = w;
    }

    static double lateralConstraintResidual(double xDot, double yDot, double theta) {
        // -sin(theta) x_dot + cos(theta) y_dot = 0
        return -Math.sin(theta) * xDot + Math.cos(theta) * yDot;
    }

    static double wrapToPi(double a) {
        double twoPi = 2.0 * Math.PI;
        a = (a + Math.PI) % twoPi;
        if (a < 0) a += twoPi;
        return a - Math.PI;
    }

    static void unicycleStep(double[] pose, double v, double w, double dt) {
        double x = pose[0], y = pose[1], th = pose[2];
        x += dt * v * Math.cos(th);
        y += dt * v * Math.sin(th);
        th = wrapToPi(th + dt * w);
        pose[0] = x; pose[1] = y; pose[2] = th;
    }

    static void wheelProfile(double t, double[] outOmegaLR) {
        if (t < 4.0) {
            outOmegaLR[0] = 6.0; outOmegaLR[1] = 6.0;
        } else if (t < 8.0) {
            outOmegaLR[0] = 3.0; outOmegaLR[1] = 7.0;
        } else if (t < 12.0) {
            outOmegaLR[0] = 7.0; outOmegaLR[1] = 3.0;
        } else {
            outOmegaLR[0] = 5.0; outOmegaLR[1] = 5.0;
        }
    }

    static SimResult runSim(double T, double dt, double r, double b) {
        int N = (int)Math.floor(T / dt) + 1;

        SimResult s = new SimResult();
        s.t = new double[N];
        s.x = new double[N];
        s.y = new double[N];
        s.th = new double[N];
        s.v = new double[N];
        s.w = new double[N];
        s.res = new double[N];

        double[] pose = new double[] {0.0, 0.0, 0.0};
        double[] omegas = new double[2];
        double[] vw = new double[2];

        for (int k = 0; k < N; ++k) {
            double tk = k * dt;
            s.t[k] = tk;

            wheelProfile(tk, omegas);
            diffDriveTwist(omegas[0], omegas[1], r, b, vw);

            double v = vw[0], w = vw[1];
            double xDot = v * Math.cos(pose[2]);
            double yDot = v * Math.sin(pose[2]);

            s.x[k] = pose[0];
            s.y[k] = pose[1];
            s.th[k] = pose[2];
            s.v[k] = v;
            s.w[k] = w;
            s.res[k] = lateralConstraintResidual(xDot, yDot, pose[2]);

            if (k < N - 1) {
                unicycleStep(pose, v, w, dt);
            }
        }
        return s;
    }

    public static void main(String[] args) {
        SimResult sim = runSim(16.0, 0.01, 0.10, 0.22);

        double maxAbs = 0.0;
        for (double e : sim.res) maxAbs = Math.max(maxAbs, Math.abs(e));

        System.out.println("max |constraint residual| = " + maxAbs);
        System.out.println("final pose: x=" + sim.x[sim.x.length - 1]
                + " y=" + sim.y[sim.y.length - 1]
                + " theta=" + sim.th[sim.th.length - 1]);

        // For plotting, export to CSV and use your preferred plotting tool (exercise).
    }
}
      

8.4 MATLAB/Simulink (Robotics System Toolbox compatible workflow)

Code: Chapter3_Lesson1.m

% Chapter 3 — Nonholonomic Motion and Feasibility for AMR
% Lesson 1 — Nonholonomic Constraints in Wheeled Robots (applied view)
%
% This script simulates differential-drive kinematics and checks the
% lateral no-slip Pfaffian constraint residual.
%
% Tooling notes (MATLAB/Simulink):
%  - Robotics System Toolbox provides differentialDriveKinematics,
%    controllerPurePursuit, navPath, etc.
%  - In Simulink, a comparable model can be built with Integrator blocks
%    and a MATLAB Function block implementing the unicycle dynamics.

clear; clc; close all;

T  = 16.0;
dt = 0.01;
r  = 0.10;   % wheel radius [m]
b  = 0.22;   % half track width [m]

N = floor(T/dt) + 1;
t = linspace(0, T, N);

x  = zeros(1,N);
y  = zeros(1,N);
th = zeros(1,N);
v  = zeros(1,N);
w  = zeros(1,N);
res = zeros(1,N);

for k = 1:N-1
    [omL, omR] = wheel_profile(t(k));
    [v(k), w(k)] = diff_drive_twist(omL, omR, r, b);

    x_dot = v(k)*cos(th(k));
    y_dot = v(k)*sin(th(k));
    res(k) = -sin(th(k))*x_dot + cos(th(k))*y_dot;

    % Euler integration
    x(k+1)  = x(k)  + dt*x_dot;
    y(k+1)  = y(k)  + dt*y_dot;
    th(k+1) = wrapToPi(th(k) + dt*w(k));
end
v(end) = v(end-1);
w(end) = w(end-1);
x_dot = v(end)*cos(th(end));
y_dot = v(end)*sin(th(end));
res(end) = -sin(th(end))*x_dot + cos(th(end))*y_dot;

fprintf('max |constraint residual| = %.3e\n', max(abs(res)));
fprintf('final pose: x=%.3f y=%.3f theta=%.3f rad\n', x(end), y(end), th(end));

figure;
plot(x, y, 'LineWidth', 1.5);
axis equal; grid on;
xlabel('x [m]'); ylabel('y [m]');
title('Differential-drive trajectory (ideal no-slip)');

figure;
plot(t, res, 'LineWidth', 1.2);
grid on;
xlabel('t [s]'); ylabel('constraint residual');
title('Lateral no-slip Pfaffian residual (should be ~0)');

% ---------------- Local functions ----------------

function [v, w] = diff_drive_twist(omL, omR, r, b)
    v = 0.5*r*(omR + omL);
    w = 0.5*r*(omR - omL)/b;
end

function [omL, omR] = wheel_profile(t)
    if t < 4.0
        omL = 6.0; omR = 6.0;
    elseif t < 8.0
        omL = 3.0; omR = 7.0;
    elseif t < 12.0
        omL = 7.0; omR = 3.0;
    else
        omL = 5.0; omR = 5.0;
    end
end

function a = wrapToPi(a)
    a = mod(a + pi, 2*pi);
    if a < 0
        a = a + 2*pi;
    end
    a = a - pi;
end
      

8.5 Wolfram Mathematica (symbolic check + general wheel constraint)

Code: Chapter3_Lesson1.nb

Notebook[{
  Cell["Chapter 3 — Nonholonomic Motion and Feasibility for AMR", "Title"],
  Cell["Lesson 1 — Nonholonomic Constraints in Wheeled Robots (applied view)", "Subtitle"],
  Cell["Symbolic nonholonomic check for the unicycle Pfaffian 1-form.", "Text"],
  Cell[BoxData@ToBoxes@HoldForm[
    (* Coordinates *)
    Clear[x, y, th];
    (* Pfaffian 1-form: omega = -Sin[th] dx + Cos[th] dy *)
    omega = {-Sin[th], Cos[th], 0};  (* coefficients for {dx, dy, dth} *)
  ], "Input"],
  Cell["In differential-form notation, omega = -Sin(th) dx + Cos(th) dy.", "Text"],
  Cell[BoxData@ToBoxes@HoldForm[
    (* Frobenius test result (known): omega ^ d omega = dx ^ dy ^ dth != 0 *)
    Print["Known result: omega \[Wedge] d(omega) = dx \[Wedge] dy \[Wedge] dth  (nonzero)"];
  ], "Input"],
  Cell["Therefore the constraint is nonintegrable (nonholonomic).", "Text"],
  Cell["General wheel lateral constraint (for reference):", "Text"],
  Cell[BoxData@ToBoxes@HoldForm[
    (* -Sin[th+alpha] xdot + Cos[th+alpha] ydot + (a Cos[alpha] + b Sin[alpha]) thdot == 0 *)
    Clear[a, b, alpha, xdot, ydot, thdot];
    constraint = -Sin[th + alpha]*xdot + Cos[th + alpha]*ydot + (a*Cos[alpha] + b*Sin[alpha])*thdot == 0;
    constraint
  ], "Input"]
},
WindowSize -> {1000, 700},
StyleDefinitions -> "Default.nb"
]
      

Problem 1 (Derive the general lateral wheel constraint): Using planar pose \( \mathbf{q}=[x,y,\theta]^\top \), a wheel at body-frame offset \( [a,b]^\top \) and rolling direction offset \( \alpha \), derive the Pfaffian constraint for “no lateral slip” in the world frame.

Solution: Contact point velocity is \( \dot{\mathbf{p}} = [\dot{x},\dot{y}]^\top + \dot{\theta}\,\mathbf{k}\times(\mathbf{R}(\theta)[a,b]^\top) \): with lateral unit direction \( \mathbf{n}=[-\sin(\theta+\alpha),\cos(\theta+\alpha)]^\top \). No slip gives \( \mathbf{n}^\top\dot{\mathbf{p}}=0 \). Expanding and using the identity \( \mathbf{n}^\top(\mathbf{k}\times(\mathbf{R}(\theta)[a,b]^\top)) = a\cos\alpha + b\sin\alpha \), we obtain:

\[ -\sin(\theta+\alpha)\,\dot{x} + \cos(\theta+\alpha)\,\dot{y} + \left(a\cos\alpha + b\sin\alpha\right)\dot{\theta} = 0. \]

Problem 2 (Differential drive rank reduction): For a differential-drive robot with wheel offsets \( [0,+b]^\top \) and \( [0,-b]^\top \), and \( \alpha=0 \) for both wheels, show that the two lateral constraints collapse into one independent constraint.

Solution: For \( a=0 \) and \( \alpha=0 \), the coefficient of \( \dot{\theta} \) is \( a\cos\alpha + b\sin\alpha = 0 \) (for both wheels). Both wheels yield the same scalar constraint:

\[ -\sin\theta\,\dot{x} + \cos\theta\,\dot{y} = 0. \]

Thus the stacked \( \mathbf{A}(\mathbf{q}) \) has two identical rows, so \( \operatorname{rank}(\mathbf{A}) = 1 \).

Problem 3 (Applied Frobenius test): For \( \omega = -\sin\theta\,dx + \cos\theta\,dy \), compute \( \omega\wedge d\omega \) and conclude integrability/nonintegrability.

Solution: As in Section 6, \( d\omega = -\cos\theta\,d\theta\wedge dx - \sin\theta\,d\theta\wedge dy \). Then

\[ \omega\wedge d\omega = \left(\sin^2\theta + \cos^2\theta\right)\,dx\wedge dy\wedge d\theta = dx\wedge dy\wedge d\theta \neq 0, \]

so the distribution is nonintegrable and the constraint is nonholonomic.

Problem 4 (Projection-based “closest feasible velocity”): Suppose you have a desired velocity \( \dot{\mathbf{q}}_d \) that may violate constraints. Define the feasible velocity \( \dot{\mathbf{q}}^\star \) as the solution to \( \min_{\dot{\mathbf{q}}}\|\dot{\mathbf{q}}-\dot{\mathbf{q}}_d\|^2 \) subject to \( \mathbf{A}(\mathbf{q})\dot{\mathbf{q}}=\mathbf{0} \). Derive a closed-form expression for \( \dot{\mathbf{q}}^\star \) assuming \( \mathbf{A} \) has full row rank.

Solution: Using Lagrange multipliers \( \boldsymbol{\lambda} \), minimize \( J=\|\dot{\mathbf{q}}-\dot{\mathbf{q}}_d\|^2 + 2\boldsymbol{\lambda}^\top(\mathbf{A}\dot{\mathbf{q}}) \). Stationarity gives \( 2(\dot{\mathbf{q}}-\dot{\mathbf{q}}_d) + 2\mathbf{A}^\top\boldsymbol{\lambda}=0 \), hence \( \dot{\mathbf{q}}=\dot{\mathbf{q}}_d-\mathbf{A}^\top\boldsymbol{\lambda} \). Enforcing the constraint:

\[ \mathbf{A}\dot{\mathbf{q}}_d - \mathbf{A}\mathbf{A}^\top\boldsymbol{\lambda} = 0 \;\;\Longrightarrow\;\; \boldsymbol{\lambda} = (\mathbf{A}\mathbf{A}^\top)^{-1}\mathbf{A}\dot{\mathbf{q}}_d. \]

Therefore the projection is:

\[ \dot{\mathbf{q}}^\star = \left(\mathbf{I} - \mathbf{A}^\top(\mathbf{A}\mathbf{A}^\top)^{-1}\mathbf{A}\right)\dot{\mathbf{q}}_d, \]

which is the orthogonal projector onto \( \mathcal{N}(\mathbf{A}) \).

We derived the wheel-level lateral no-slip constraint and stacked it into the robot-level Pfaffian form \( \mathbf{A}(\mathbf{q})\dot{\mathbf{q}}=\mathbf{0} \): the practical feasibility test used throughout mobile robotics. For differential drive, we saw the constraint reduces to a single “no sideways velocity” restriction plus a clean mapping from wheel rates to \( (v,\dot{\theta}) \). Finally, we proved (applied Frobenius test) that the canonical wheeled constraint is nonintegrable, hence nonholonomic.