For a planar rigid body, the instantaneous motion is determined by a twist (planar velocity + yaw rate). A wheeled robot, however, is additionally constrained by wheel–ground contact. The canonical assumption in mobile-robot kinematics is pure rolling: the wheel does not slip sideways, and the component of contact velocity along the wheel’s rolling direction matches the wheel’s spin rate.

We will express constraints on the robot’s instantaneous velocity in a compact matrix form. Throughout, the robot pose is \( \mathbf{q} := [x,\;y,\;\theta]^\top \in \mathbb{R}^2 \times S^1 \), and the body-frame planar twist is \( \boldsymbol{\xi}_B := [v_x,\;v_y,\;\omega]^\top \). The key outcome is a constraint matrix \( \mathbf{A}(\mathbf{q}) \) such that

\[ \mathbf{A}(\mathbf{q})\,\boldsymbol{\xi}_B = \mathbf{0}, \quad \text{(pure-rolling lateral constraints for standard wheels).} \]

The instantaneous center of rotation provides an equivalent geometric interpretation: when \( \omega \neq 0 \), the motion is a pure rotation about a point in the plane, and each standard wheel forces that point to lie on a line determined by its axle geometry.

Let \( \{I\} \) be an inertial frame and \( \{B\} \) the robot body frame at the robot reference point (often the chassis center). The planar rotation from body to inertial is \( \mathbf{R}(\theta) \in SO(2) \):

\[ \mathbf{R}(\theta) := \begin{bmatrix} \cos\theta & -\sin\theta\\ \sin\theta & \cos\theta \end{bmatrix}, \quad \begin{bmatrix}\dot{x}\\\dot{y}\end{bmatrix} = \mathbf{R}(\theta)\begin{bmatrix}v_x\\v_y\end{bmatrix}. \]

Consider wheel \( i \). In the body frame, the (contact-point or wheel-center) position is \( \mathbf{r}_i := [\ell_{xi},\;\ell_{yi}]^\top \). Define the wheel rolling direction (unit tangent along the wheel plane) by an angle \( \alpha_i \) measured in the body frame from \( +x_B \):

\[ \mathbf{t}_i := \begin{bmatrix}\cos\alpha_i\\ \sin\alpha_i\end{bmatrix}, \qquad \mathbf{n}_i := \begin{bmatrix}-\sin\alpha_i\\ \cos\alpha_i\end{bmatrix}, \qquad \mathbf{t}_i^\top \mathbf{n}_i = 0. \]

The planar rigid-body velocity at a body-frame point \( \mathbf{r}_i \) is \( \mathbf{v}_i = \mathbf{v} + \omega\,\mathbf{k}\times \mathbf{r}_i \), where \( \mathbf{v} := [v_x,\;v_y]^\top \) and \( \mathbf{k} \) is the out-of-plane unit axis. In 2D, \( \mathbf{k}\times [a,\;b]^\top = [-b,\;a]^\top \), so:

\[ \mathbf{v}_i = \begin{bmatrix}v_x\\ v_y\end{bmatrix} + \omega \begin{bmatrix} -\ell_{yi}\\ \ell_{xi} \end{bmatrix}. \]

This formula is the workhorse: every rolling constraint is a statement about projections of \( \mathbf{v}_i \) onto \( \mathbf{t}_i \) and \( \mathbf{n}_i \).

A standard wheel (not an omnidirectional wheel) admits rolling along \( \mathbf{t}_i \) but (ideally) forbids lateral slip along \( \mathbf{n}_i \).

3.1 Lateral no-slip (Pfaffian) constraint

The lateral no-slip assumption is: \( \mathbf{n}_i^\top \mathbf{v}_i = 0 \). Substituting the rigid-body velocity expression yields a linear constraint in the twist \( \boldsymbol{\xi}_B \).

\[ \mathbf{n}_i^\top \left( \begin{bmatrix}v_x\\ v_y\end{bmatrix} + \omega \begin{bmatrix}-\ell_{yi}\\ \ell_{xi}\end{bmatrix} \right) = 0. \]

Writing this explicitly as a row vector multiplying \( [v_x,\;v_y,\;\omega]^\top \) gives:

\[ \underbrace{ \begin{bmatrix} n_{ix} & n_{iy} & \left(-\ell_{yi}n_{ix} + \ell_{xi}n_{iy}\right) \end{bmatrix}}_{:=\;\mathbf{a}_i^\top} \begin{bmatrix}v_x\\ v_y\\ \omega\end{bmatrix} = 0. \]

Proof sketch (contact-velocity projection): Pure rolling means the relative velocity between the ground and the wheel at the contact point has no component along the lateral direction of the wheel. The contact point velocity in the body frame equals \( \mathbf{v}_i \) under rigid-body motion; projecting onto \( \mathbf{n}_i \) and enforcing zero yields \( \mathbf{n}_i^\top \mathbf{v}_i = 0 \), which becomes the Pfaffian linear form above.

3.2 Rolling (longitudinal) constraint and wheel spin rate

Let the wheel radius be \( R_i \) and wheel spin rate be \( \dot{\varphi}_i \). Pure rolling along the wheel plane enforces that the tangential component of contact velocity equals rim speed:

\[ \mathbf{t}_i^\top \mathbf{v}_i = R_i\,\dot{\varphi}_i. \]

Substituting \( \mathbf{v}_i \) gives an explicit mapping from body twist to wheel spin:

\[ \dot{\varphi}_i = \frac{1}{R_i}\, \mathbf{t}_i^\top \left( \begin{bmatrix}v_x\\ v_y\end{bmatrix} + \omega \begin{bmatrix}-\ell_{yi}\\ \ell_{xi}\end{bmatrix} \right). \]

In many robot bases, the lateral no-slip constraints determine the allowable instantaneous twist space, and the longitudinal equalities map a chosen twist to required wheel spin rates.

For \( m \) standard wheels, stack the lateral constraints: \( \mathbf{a}_i^\top \boldsymbol{\xi}_B = 0 \) for \( i = 1,\dots,m \). Define:

\[ \mathbf{A}(\mathbf{q}) := \begin{bmatrix} \mathbf{a}_1^\top\\ \mathbf{a}_2^\top\\ \vdots\\ \mathbf{a}_m^\top \end{bmatrix} \in \mathbb{R}^{m\times 3}, \qquad \mathbf{A}(\mathbf{q})\,\boldsymbol{\xi}_B = \mathbf{0}. \]

The feasible instantaneous twist space is the nullspace \( \mathcal{N}(\mathbf{A}) \). This immediately yields a degree-of-freedom count for planar motion.

Proposition (instantaneous mobility dimension): If the lateral constraints are exact and enforced at an instant, then

\[ \dim\big(\mathcal{N}(\mathbf{A})\big) = 3 - \operatorname{rank}(\mathbf{A}), \quad \text{with } \operatorname{rank}(\mathbf{A}) \le 3. \]

Proof: The nullspace dimension theorem gives \( \dim(\mathcal{N}(\mathbf{A})) = n - \operatorname{rank}(\mathbf{A}) \) for \( \mathbf{A}\in\mathbb{R}^{m\times n} \). Here \( n=3 \) because \( \boldsymbol{\xi}_B \in \mathbb{R}^3 \). Therefore \( \dim(\mathcal{N}(\mathbf{A})) = 3 - \operatorname{rank}(\mathbf{A}) \).

Practical reading: if \( \operatorname{rank}(\mathbf{A}) = 2 \), the robot has a 1D instantaneous motion subspace (e.g., “unicycle-like” instantaneous mobility). If \( \operatorname{rank}(\mathbf{A}) = 1 \), it has a 2D instantaneous motion subspace, and so on. The detailed path implications are deferred to Chapter 3; here we focus on the instantaneous (velocity-level) picture.

Any planar twist with \( \omega \neq 0 \) can be interpreted as a pure rotation about a point \( \mathbf{p}_{ICR} \) in the plane (expressed in the body frame). By definition, the velocity at that point is zero: \( \mathbf{v} + \omega\,\mathbf{k}\times \mathbf{p}_{ICR} = \mathbf{0} \). Solving for \( \mathbf{p}_{ICR} \) gives:

\[ \mathbf{p}_{ICR} = \begin{bmatrix} -v_y/\omega\\ \;\;v_x/\omega \end{bmatrix}, \quad \text{valid when } \omega \neq 0. \]

When \( \omega = 0 \), the motion is a pure translation and the ICR is “at infinity” (direction of translation).

The ICR provides an immediate geometric reading of wheel constraints. Under pure rolling, the velocity at wheel contact is aligned with the rolling direction \( \mathbf{t}_i \), hence perpendicular to the vector from the ICR to that contact point.

Lemma (axle-line condition for a standard wheel): If wheel \( i \) satisfies the lateral no-slip constraint and \( \omega \neq 0 \), then the ICR must lie on the line

\[ \mathbf{p}_{ICR} = \mathbf{r}_i + s\,\mathbf{n}_i \quad \text{for some } s\in\mathbb{R}. \]

Proof: For planar rotation about \( \mathbf{p}_{ICR} \), the velocity at point \( \mathbf{r}_i \) is \( \mathbf{v}_i = \omega\,\mathbf{k}\times(\mathbf{r}_i - \mathbf{p}_{ICR}) \), which is perpendicular to \( \mathbf{r}_i - \mathbf{p}_{ICR} \). Pure rolling requires \( \mathbf{v}_i \parallel \mathbf{t}_i \), so \( (\mathbf{r}_i - \mathbf{p}_{ICR}) \perp \mathbf{t}_i \). Since \( \mathbf{n}_i \perp \mathbf{t}_i \) and is unit-length, the perpendicular direction is exactly \( \mathbf{n}_i \), hence \( \mathbf{p}_{ICR} = \mathbf{r}_i + s\mathbf{n}_i \).

flowchart TD
  A["Given twist (vx, vy, omega) in body frame"] --> B["If omega != 0, \ncompute pICR = (-vy/omega, vx/omega)"]
  A --> C["If omega == 0, translation case \n(ICR at infinity)"]
  B --> D["For each wheel i compute \naxle direction n_i and point r_i"]
  D --> E["Check collinearity: \npICR lies on line r_i + s*n_i"]
  E -->|all wheels pass| F["Pure rolling consistent"]
  E -->|any wheel fails| G["Slip required or constraints violated"]
        

Thus, for multiple standard wheels, pure rolling with \( \omega \neq 0 \) is possible only if their axle-lines admit a common intersection (or are consistent in the parallel/translation limit).

In software, you will typically do one of the following instantaneous computations:

• Forward (twist → wheel rates): choose a feasible twist \( \boldsymbol{\xi}_B \in \mathcal{N}(\mathbf{A}) \), then compute each \( \dot{\varphi}_i \) via the rolling equalities.
• Projection (desired twist → feasible twist): given a desired \( \boldsymbol{\xi}_{des} \), compute the closest feasible \( \boldsymbol{\xi}^\star \) satisfying \( \mathbf{A}\boldsymbol{\xi}^\star = \mathbf{0} \).

A common least-squares projection solves: \( \min_{\boldsymbol{\xi}} \|\boldsymbol{\xi}-\boldsymbol{\xi}_{des}\|_2 \) subject to \( \mathbf{A}\boldsymbol{\xi}=\mathbf{0} \). If \( \mathbf{A} \) has full row rank, one closed-form projector is:

\[ \boldsymbol{\xi}^\star = \left(\mathbf{I}_3 - \mathbf{A}^\top(\mathbf{A}\mathbf{A}^\top)^{-1}\mathbf{A}\right)\boldsymbol{\xi}_{des}. \]

flowchart TD
  S["Inputs: wheel geometry \n(r_i, alpha_i, R_i), \ndesired twist xi_des"] --> A1["Build A from lateral rows a_i^T"]
  A1 --> B1["Compute feasible twist xi* \nby projection onto Null(A)"]
  B1 --> C1["Compute ICR from xi* \n(if omega != 0) for diagnostics"]
  B1 --> D1["Compute wheel spin rates: \nphidot_i = (1/R_i)*t_i^T*(v + omega*kxr_i)"]
  D1 --> E1["Outputs: feasible twist xi*, \nwheel rates phidot, diagnostic residuals"]
        

These computations are exactly what the provided code files implement.

The implementations below follow the same definitions: \( \mathbf{r}_i = [\ell_{xi},\ell_{yi}]^\top \), \( \mathbf{t}_i = [\cos\alpha_i,\sin\alpha_i]^\top \), \( \mathbf{n}_i = [-\sin\alpha_i,\cos\alpha_i]^\top \), \( \mathbf{v}_i = [v_x,v_y]^\top + \omega[-\ell_{yi},\ell_{xi}]^\top \).

7.1 Python (NumPy; optional SymPy)

Libraries: numpy for numeric linear algebra; (optional) sympy for symbolic checks. This code is also compatible with robotics workflows where twists are exchanged (e.g., ROS geometry twists), but we keep this lesson self-contained.

File: Chapter2_Lesson1.py

#!/usr/bin/env python3
# Chapter2_Lesson1.py
"""
Autonomous Mobile Robots (Control Engineering)
Chapter 2, Lesson 1: Rolling Constraints and Instantaneous Motion

This script:
1) Defines a planar rigid-body twist in the BODY frame: xi = [v_x, v_y, omega]
2) Encodes standard-wheel rolling constraints:
   - no lateral slip: n_i^T (v + omega * k x r_i) = 0
   - rolling rate:    phi_dot_i = (1/R_i) t_i^T (v + omega * k x r_i)
3) Computes the ICR in the BODY frame: p_ICR = [ -v_y/omega, v_x/omega ]  (omega != 0)
4) Demonstrates consistency of ICR with axle-line intersection.

Dependencies: numpy (required), sympy (optional, for symbolic check).
"""

from __future__ import annotations
import math
from dataclasses import dataclass
from typing import List, Optional

import numpy as np


@dataclass(frozen=True)
class Wheel:
    lx: float
    ly: float
    alpha: float
    R: float


def t_dir(alpha: float) -> np.ndarray:
    return np.array([math.cos(alpha), math.sin(alpha)], dtype=float)


def n_dir(alpha: float) -> np.ndarray:
    return np.array([-math.sin(alpha), math.cos(alpha)], dtype=float)


def v_point_body(vx: float, vy: float, omega: float, r: np.ndarray) -> np.ndarray:
    return np.array([vx, vy], dtype=float) + omega * np.array([-r[1], r[0]], dtype=float)


def lateral_constraint_row(w: Wheel) -> np.ndarray:
    r = np.array([w.lx, w.ly], dtype=float)
    n = n_dir(w.alpha)
    return np.array([n[0], n[1], (-r[1] * n[0] + r[0] * n[1])], dtype=float)


def build_A(wheels: List[Wheel]) -> np.ndarray:
    return np.vstack([lateral_constraint_row(w) for w in wheels])


def wheel_spin_rates(wheels: List[Wheel], vx: float, vy: float, omega: float) -> np.ndarray:
    rates = []
    for w in wheels:
        r = np.array([w.lx, w.ly], dtype=float)
        t = t_dir(w.alpha)
        vpt = v_point_body(vx, vy, omega, r)
        rates.append((t @ vpt) / w.R)
    return np.array(rates, dtype=float)


def icr_body(vx: float, vy: float, omega: float, eps: float = 1e-12) -> Optional[np.ndarray]:
    if abs(omega) < eps:
        return None
    return np.array([-vy / omega, vx / omega], dtype=float)


def axle_line_residual(w: Wheel, p: np.ndarray) -> float:
    r = np.array([w.lx, w.ly], dtype=float)
    n = n_dir(w.alpha)
    d = p - r
    return float(d[0] * n[1] - d[1] * n[0])


def solve_feasible_twist_least_norm(wheels: List[Wheel], v_des: np.ndarray) -> np.ndarray:
    A = build_A(wheels)
    I = np.eye(3)
    if A.size == 0:
        return v_des.copy()
    AA = A @ A.T
    AA_reg = AA + 1e-12 * np.eye(AA.shape[0])
    P = I - A.T @ np.linalg.solve(AA_reg, A)
    return P @ v_des


def main() -> None:
    b = 0.6
    wheels = [
        Wheel(lx=0.0, ly=+b/2, alpha=0.0, R=0.1),
        Wheel(lx=0.0, ly=-b/2, alpha=0.0, R=0.1),
    ]

    vx, vy, omega = 0.5, 0.0, 0.8
    A = build_A(wheels)
    xi = np.array([vx, vy, omega])
    print("A@xi =", A @ xi)

    p = icr_body(vx, vy, omega)
    print("ICR (body) =", p)
    for i, w in enumerate(wheels):
        print("residual wheel", i+1, axle_line_residual(w, p))

    print("wheel spin rates =", wheel_spin_rates(wheels, vx, vy, omega))

    v_des = np.array([0.5, 0.2, 0.6])
    xi_star = solve_feasible_twist_least_norm(wheels, v_des)
    print("xi* =", xi_star, "constraint residual =", build_A(wheels) @ xi_star)


if __name__ == "__main__":
    main()

7.2 C++ (Eigen)

Libraries: Eigen for linear algebra (ubiquitous in robotics C++ stacks). The code builds \( \mathbf{A} \), checks \( \mathbf{A}\boldsymbol{\xi}=\mathbf{0} \), computes ICR, and computes wheel spin rates.

File: Chapter2_Lesson1.cpp

// Chapter2_Lesson1.cpp
#include <iostream>
#include <vector>
#include <cmath>

#include <Eigen/Dense>

struct Wheel {
  double lx;
  double ly;
  double alpha;
  double R;
};

static Eigen::Vector2d t_dir(double a) { return {std::cos(a), std::sin(a)}; }
static Eigen::Vector2d n_dir(double a) { return {-std::sin(a), std::cos(a)}; }

static Eigen::Vector2d v_point(double vx, double vy, double omega, const Eigen::Vector2d& r) {
  return Eigen::Vector2d(vx, vy) + omega * Eigen::Vector2d(-r.y(), r.x());
}

static Eigen::RowVector3d lateral_constraint_row(const Wheel& w) {
  Eigen::Vector2d r(w.lx, w.ly);
  Eigen::Vector2d n = n_dir(w.alpha);
  double c = (-r.y() * n.x() + r.x() * n.y());
  return Eigen::RowVector3d(n.x(), n.y(), c);
}

static Eigen::MatrixXd buildA(const std::vector<Wheel>& wheels) {
  Eigen::MatrixXd A(wheels.size(), 3);
  for (size_t i = 0; i < wheels.size(); ++i) A.row((int)i) = lateral_constraint_row(wheels[i]);
  return A;
}

static std::vector<double> wheelSpinRates(const std::vector<Wheel>& wheels,
                                         double vx, double vy, double omega) {
  std::vector<double> rates;
  for (const auto& w : wheels) {
    Eigen::Vector2d r(w.lx, w.ly);
    Eigen::Vector2d t = t_dir(w.alpha);
    Eigen::Vector2d vpt = v_point(vx, vy, omega, r);
    rates.push_back(t.dot(vpt) / w.R);
  }
  return rates;
}

static bool icrBody(double vx, double vy, double omega, Eigen::Vector2d& p) {
  if (std::abs(omega) < 1e-12) return false;
  p = Eigen::Vector2d(-vy / omega, vx / omega);
  return true;
}

int main() {
  double b = 0.6;
  std::vector<Wheel> wheels = { {0.0, +b/2, 0.0, 0.1}, {0.0, -b/2, 0.0, 0.1} };

  double vx = 0.5, vy = 0.0, omega = 0.8;
  Eigen::Vector3d xi(vx, vy, omega);

  Eigen::MatrixXd A = buildA(wheels);
  std::cout << "A*xi = " << (A * xi).transpose() << "\n";

  Eigen::Vector2d p;
  if (icrBody(vx, vy, omega, p)) std::cout << "ICR = [" << p.x() << ", " << p.y() << "]\n";

  auto rates = wheelSpinRates(wheels, vx, vy, omega);
  std::cout << "rates: ";
  for (double r : rates) std::cout << r << " ";
  std::cout << "\n";
  return 0;
}

7.3 Java (EJML)

Libraries: EJML is a standard numeric linear algebra choice in Java robotics projects. The code mirrors the same definitions of \( \mathbf{A} \), ICR, and wheel spin rates.

File: Chapter2_Lesson1.java

// Chapter2_Lesson1.java
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;

import java.util.ArrayList;
import java.util.List;

public class Chapter2_Lesson1 {

    static class Wheel {
        public double lx, ly, alpha, R;
        public Wheel(double lx, double ly, double alpha, double R) {
            this.lx = lx; this.ly = ly; this.alpha = alpha; this.R = R;
        }
    }

    static double[] tDir(double a) { return new double[]{Math.cos(a), Math.sin(a)}; }
    static double[] nDir(double a) { return new double[]{-Math.sin(a), Math.cos(a)}; }

    static double[] vPoint(double vx, double vy, double omega, double[] r) {
        return new double[]{vx + omega * (-r[1]), vy + omega * (r[0])};
    }

    static double[] lateralConstraintRow(Wheel w) {
        double[] r = new double[]{w.lx, w.ly};
        double[] n = nDir(w.alpha);
        double c = (-r[1] * n[0] + r[0] * n[1]);
        return new double[]{n[0], n[1], c};
    }

    static DMatrixRMaj buildA(List<Wheel> wheels) {
        DMatrixRMaj A = new DMatrixRMaj(wheels.size(), 3);
        for (int i = 0; i < wheels.size(); i++) {
            double[] row = lateralConstraintRow(wheels.get(i));
            A.set(i, 0, row[0]);
            A.set(i, 1, row[1]);
            A.set(i, 2, row[2]);
        }
        return A;
    }

    static boolean icrBody(double vx, double vy, double omega, double[] outP) {
        if (Math.abs(omega) < 1e-12) return false;
        outP[0] = -vy / omega;
        outP[1] =  vx / omega;
        return true;
    }

    static double[] wheelSpinRates(List<Wheel> wheels, double vx, double vy, double omega) {
        double[] rates = new double[wheels.size()];
        for (int i = 0; i < wheels.size(); i++) {
            Wheel w = wheels.get(i);
            double[] r = new double[]{w.lx, w.ly};
            double[] t = tDir(w.alpha);
            double[] vpt = vPoint(vx, vy, omega, r);
            rates[i] = (t[0]*vpt[0] + t[1]*vpt[1]) / w.R;
        }
        return rates;
    }

    public static void main(String[] args) {
        double b = 0.6;
        List<Wheel> wheels = new ArrayList<>();
        wheels.add(new Wheel(0.0, +b/2.0, 0.0, 0.1));
        wheels.add(new Wheel(0.0, -b/2.0, 0.0, 0.1));

        double vx = 0.5, vy = 0.0, omega = 0.8;

        DMatrixRMaj A = buildA(wheels);
        DMatrixRMaj xi = new DMatrixRMaj(3, 1, true, new double[]{vx, vy, omega});
        DMatrixRMaj res = new DMatrixRMaj(A.numRows, 1);
        CommonOps_DDRM.mult(A, xi, res);

        System.out.println("A*xi = ");
        res.print();

        double[] p = new double[2];
        if (icrBody(vx, vy, omega, p)) System.out.println("ICR = [" + p[0] + ", " + p[1] + "]");

        double[] rates = wheelSpinRates(wheels, vx, vy, omega);
        System.out.print("rates: ");
        for (double r : rates) System.out.print(r + " ");
        System.out.println();
    }
}

7.4 MATLAB / Simulink

Libraries: MATLAB base linear algebra is sufficient; Robotics System Toolbox is recommended for later chapters (frames, transformations, and integration with navigation stacks).

File: Chapter2_Lesson1.m

% Chapter2_Lesson1.m
clear; clc;

% Each wheel: [lx, ly, alpha, R]
b = 0.6;
wheels = [
    0.0, +b/2, 0.0, 0.1;
    0.0, -b/2, 0.0, 0.1
];

vx = 0.5; vy = 0.0; omega = 0.8;
xi = [vx; vy; omega];

A = zeros(size(wheels,1), 3);
for i=1:size(wheels,1)
    lx = wheels(i,1); ly = wheels(i,2); alpha = wheels(i,3);
    n = [-sin(alpha); cos(alpha)];
    c = (-ly*n(1) + lx*n(2));
    A(i,:) = [n(1), n(2), c];
end

disp('A*xi ='); disp(A*xi);

if abs(omega) < 1e-12
    disp('omega ~ 0: ICR at infinity');
else
    pICR = [-vy/omega; vx/omega];
    disp('ICR (body) ='); disp(pICR);
end

phi_dot = zeros(size(wheels,1),1);
for i=1:size(wheels,1)
    lx = wheels(i,1); ly = wheels(i,2); alpha = wheels(i,3); R = wheels(i,4);
    t = [cos(alpha); sin(alpha)];
    vpt = [vx; vy] + omega * [-ly; lx];
    phi_dot(i) = (t' * vpt) / R;
end
disp('wheel spin rates ='); disp(phi_dot);

7.5 Wolfram Mathematica

Mathematica is well-suited for symbolic verification of the Pfaffian constraint rows and ICR identities.

File: Chapter2_Lesson1.nb

(* Chapter2_Lesson1.nb (excerpt) *)
Clear[vx, vy, w, lx, ly, a];
t = {Cos[a], Sin[a]};
n = {-Sin[a], Cos[a]};
r = {lx, ly};
v = {vx, vy};
kCrossR = {-ly, lx};
vpt = v + w*kCrossR;
constraint = Simplify[n.vpt];
constraint // TraditionalForm

pICR = {-vy/w, vx/w};
vFromICR = w*{-pICR[[2]], pICR[[1]]};
Simplify[vFromICR - {vx, vy}]

Problem 1 (Derive the lateral Pfaffian row): A standard wheel is located at \( \mathbf{r} = [\ell_x,\;\ell_y]^\top \) in the body frame, with rolling direction angle \( \alpha \). Starting from \( \mathbf{n}^\top(\mathbf{v} + \omega\,\mathbf{k}\times\mathbf{r}) = 0 \), derive the row \( \mathbf{a}^\top \) such that \( \mathbf{a}^\top [v_x,\;v_y,\;\omega]^\top = 0 \).

Solution: With

\[ \mathbf{n} = \begin{bmatrix}-\sin\alpha\\ \cos\alpha\end{bmatrix}, \qquad \mathbf{k}\times\mathbf{r} = \begin{bmatrix}-\ell_y\\ \ell_x\end{bmatrix}, \qquad \mathbf{v}=\begin{bmatrix}v_x\\ v_y\end{bmatrix}, \]

the constraint becomes

\[ \mathbf{n}^\top \mathbf{v} + \omega\,\mathbf{n}^\top(\mathbf{k}\times\mathbf{r}) = 0. \]

Compute each term:

\[ \mathbf{n}^\top \mathbf{v} = (-\sin\alpha)\,v_x + (\cos\alpha)\,v_y, \\ \qquad \mathbf{n}^\top(\mathbf{k}\times\mathbf{r}) = (-\sin\alpha)(-\ell_y) + (\cos\alpha)\ell_x = \ell_y\sin\alpha + \ell_x\cos\alpha. \]

Therefore

\[ \big[-\sin\alpha,\;\cos\alpha,\;(\ell_y\sin\alpha + \ell_x\cos\alpha)\big] \begin{bmatrix}v_x\\ v_y\\ \omega\end{bmatrix} = 0. \]

Problem 2 (Two parallel wheels → restriction on \( v_y \)): Two standard wheels have rolling direction aligned with \( +x_B \) (so \( \alpha=0 \) for both), and are positioned at \( \ell_x=0 \), \( \ell_y = \pm b/2 \). Show that the lateral constraints imply \( v_y = 0 \). For \( \omega \neq 0 \), compute \( \mathbf{p}_{ICR} \).

Solution: For \( \alpha = 0 \),

\[ \mathbf{n} = \begin{bmatrix}-\sin 0\\ \cos 0\end{bmatrix} = \begin{bmatrix}0\\ 1\end{bmatrix}. \]

The lateral constraint at wheel \( i \) is \( \mathbf{n}^\top \mathbf{v}_i = 0 \). Since \( \ell_x=0 \), we have

\[ \mathbf{v}_i = \begin{bmatrix}v_x\\ v_y\end{bmatrix} + \omega \begin{bmatrix}-\ell_{yi}\\ 0\end{bmatrix}, \quad \mathbf{n}^\top \mathbf{v}_i = \begin{bmatrix}0 & 1\end{bmatrix} \left( \begin{bmatrix}v_x\\ v_y\end{bmatrix} + \omega \begin{bmatrix}-\ell_{yi}\\ 0\end{bmatrix} \right) = v_y. \]

Hence \( v_y = 0 \). If \( \omega \neq 0 \), then

\[ \mathbf{p}_{ICR} = \begin{bmatrix}-v_y/\omega\\ v_x/\omega\end{bmatrix} = \begin{bmatrix}0\\ v_x/\omega\end{bmatrix}. \]

Problem 3 (Consistency test via rank): Consider three standard wheels producing the constraint matrix \( \mathbf{A}\in\mathbb{R}^{3\times 3} \). Show that if \( \operatorname{rank}(\mathbf{A}) = 3 \), then the only feasible instantaneous twist is \( \boldsymbol{\xi}_B=\mathbf{0} \). Interpret physically.

Solution: If \( \operatorname{rank}(\mathbf{A}) = 3 \), then the nullspace dimension is

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

Therefore \( \mathcal{N}(\mathbf{A}) = \{\mathbf{0}\} \) and the only twist satisfying \( \mathbf{A}\boldsymbol{\xi}_B=\mathbf{0} \) is \( \boldsymbol{\xi}_B=\mathbf{0} \). Physically, the three wheels overconstrain planar motion: pure rolling would require simultaneous satisfaction of three independent lateral no-slip constraints, which is generically impossible unless the robot does not move (or slip occurs).

Problem 4 (Least-squares feasible twist projection): Solve \( \min_{\boldsymbol{\xi}} \|\boldsymbol{\xi}-\boldsymbol{\xi}_{des}\|_2^2 \) subject to \( \mathbf{A}\boldsymbol{\xi}=\mathbf{0} \), assuming \( \mathbf{A} \) has full row rank. Derive the closed form for \( \boldsymbol{\xi}^\star \).

Solution: Form the Lagrangian

\[ \mathcal{L}(\boldsymbol{\xi},\boldsymbol{\lambda}) = (\boldsymbol{\xi}-\boldsymbol{\xi}_{des})^\top(\boldsymbol{\xi}-\boldsymbol{\xi}_{des}) + 2\boldsymbol{\lambda}^\top(\mathbf{A}\boldsymbol{\xi}). \]

Stationarity in \( \boldsymbol{\xi} \) gives

\[ \frac{\partial \mathcal{L}}{\partial \boldsymbol{\xi}} = 2(\boldsymbol{\xi}-\boldsymbol{\xi}_{des}) + 2\mathbf{A}^\top\boldsymbol{\lambda} = \mathbf{0} \;\;\Rightarrow\;\; \boldsymbol{\xi} = \boldsymbol{\xi}_{des} - \mathbf{A}^\top\boldsymbol{\lambda}. \]

Enforce the constraint:

\[ \mathbf{A}\boldsymbol{\xi} = \mathbf{A}\boldsymbol{\xi}_{des} - \mathbf{A}\mathbf{A}^\top\boldsymbol{\lambda} = \mathbf{0} \;\;\Rightarrow\;\; \boldsymbol{\lambda} = (\mathbf{A}\mathbf{A}^\top)^{-1}\mathbf{A}\boldsymbol{\xi}_{des}. \]

Substitute back:

\[ \boldsymbol{\xi}^\star = \left(\mathbf{I}_3 - \mathbf{A}^\top(\mathbf{A}\mathbf{A}^\top)^{-1}\mathbf{A}\right)\boldsymbol{\xi}_{des}. \]

We derived the pure-rolling constraints for standard wheels by projecting rigid-body point velocity onto wheel-aligned tangent/normal directions. The lateral no-slip constraints stack into a Pfaffian matrix equation \( \mathbf{A}(\mathbf{q})\boldsymbol{\xi}_B=\mathbf{0} \), whose nullspace determines the feasible instantaneous motion subspace. The ICR provides a geometric diagnostic: for \( \omega \neq 0 \), the ICR must lie on each wheel’s axle line for pure rolling to be consistent. The provided implementations compute \( \mathbf{A} \), project twists, compute ICR, and compute wheel spin rates.