In Lessons 1–3, “standard” wheels (fixed or steered) imposed lateral no-slip constraints that reduced instantaneous mobility. In contrast, \( \)omnidirectional wheels introduce rollers that make motion “free” along one in-plane direction at the contact, leaving only one effective traction constraint per wheel. By arranging wheel traction directions to span the plane, the chassis can realize any planar twist \( \mathbf{v}_B = [v_x,\; v_y,\; \omega]^T \) (body-frame).

The central modeling pattern is linear-in-velocity: wheel rates \( \dot{\boldsymbol{\phi}} \) relate to body twist \( \mathbf{v}_B \) through a base Jacobian \( \mathbf{J} \):

\[ \dot{\boldsymbol{\phi}} = \mathbf{J}\,\mathbf{v}_B, \qquad \mathbf{v}_B \in \mathbb{R}^3,\; \dot{\boldsymbol{\phi}} \in \mathbb{R}^n \]

If \( \operatorname{rank}(\mathbf{J}) = 3 \), then for any desired planar chassis twist there exists at least one wheel-rate vector that achieves it. When \( n > 3 \) (e.g., 4-wheel mecanum), the system is redundant and admits infinitely many solutions.

flowchart TD
  A["Desired chassis twist v = [vx, vy, w]"] --> B["Inverse kinematics: wheel rates w = J v"]
  B --> C["Wheel speed commands to motor drivers"]
  C --> D["Encoders return wheel rates"]
  D --> E["Forward kinematics: v_hat = J_plus w"]
  E --> F["Integrate pose using v_hat"]
  F --> G["Controller updates next desired twist"]
        

Let the body frame \( \{B\} \) be attached to the chassis, with origin at a reference point (typically the geometric center), and axes \( x \) forward, \( y \) left. A wheel contact point \( i \) is located at \( \mathbf{r}_i = [x_i,\; y_i]^T \) in the body frame.

Under planar rigid-body kinematics, the velocity of the contact point (in the body frame) is:

\[ \mathbf{v}_{c,i} = \begin{bmatrix} v_x \\ v_y \end{bmatrix} + \omega \begin{bmatrix} -y_i \\ x_i \end{bmatrix} \]

Proof (planar rigid-body velocity decomposition). In 3D rigid-body kinematics, a point at displacement \( \mathbf{p}_i = [x_i, y_i, 0]^T \) has velocity \( \mathbf{v}_i = \mathbf{v}_0 + \boldsymbol{\omega} \times \mathbf{p}_i \). With planar angular velocity \( \boldsymbol{\omega} = [0,0,\omega]^T \),

\[ \boldsymbol{\omega} \times \mathbf{p}_i = \begin{bmatrix} 0 \\ 0 \\ \omega \end{bmatrix} \times \begin{bmatrix} x_i \\ y_i \\ 0 \end{bmatrix} = \begin{bmatrix} -\omega y_i \\ \omega x_i \\ 0 \end{bmatrix} \]

Dropping the zero \( z \)-component yields the stated planar expression.

A Swedish wheel (including omni wheels and mecanum wheels) uses passive rollers to eliminate resistance along one in-plane direction at the contact. Consequently, each wheel contributes a single scalar kinematic relation: the component of the contact-point velocity along the wheel’s traction direction equals the driven rim speed projected into that direction.

Define for wheel \( i \):

• A unit traction direction \( \mathbf{u}_i = [\cos\beta_i,\; \sin\beta_i]^T \) in the body frame.
• Wheel angular rate \( \dot{\phi}_i \), wheel radius \( r_i \).
• A skew angle \( \alpha_i \) between the wheel’s tangential drive direction and the traction direction (for classic mecanum, \( \alpha_i = 45^\circ \); for omni wheels, \( \alpha_i = 0^\circ \)).

The rim tangential speed magnitude is \( r_i \dot{\phi}_i \). Only its component along \( \mathbf{u}_i \) enforces traction; the orthogonal component is “absorbed” by roller rotation. Let the projection factor be \( \cos\alpha_i \), and define \( \kappa_i = r_i \cos\alpha_i \).

\[ \mathbf{u}_i^T \mathbf{v}_{c,i} = \kappa_i \dot{\phi}_i \]

Expanding \( \mathbf{v}_{c,i} \) from Section 2:

\[ \cos\beta_i\,v_x + \sin\beta_i\,v_y + \omega\left(-y_i\cos\beta_i + x_i\sin\beta_i\right) = \kappa_i \dot{\phi}_i \]

Stacked Jacobian form. Let \( \dot{\boldsymbol{\phi}} = [\dot{\phi}_1,\dots,\dot{\phi}_n]^T \). Define \( \mathbf{K} = \operatorname{diag}(\kappa_1,\dots,\kappa_n) \) and build a matrix \( \mathbf{A}\in\mathbb{R}^{n\times 3} \) whose \( i \)-th row is:

\[ \mathbf{a}_i^T = \begin{bmatrix} \cos\beta_i & \sin\beta_i & -y_i\cos\beta_i + x_i\sin\beta_i \end{bmatrix} \]

Then the wheel constraints become:

\[ \mathbf{A}\,\mathbf{v}_B = \mathbf{K}\,\dot{\boldsymbol{\phi}} \qquad \Longleftrightarrow \qquad \dot{\boldsymbol{\phi}} = \mathbf{J}\,\mathbf{v}_B,\;\; \mathbf{J} = \mathbf{K}^{-1}\mathbf{A} \]

Interpretation. Each wheel provides one linear “measurement” of the chassis twist. An omnidirectional base is achieved by choosing wheel placements and traction directions so that the three components \( v_x, v_y, \omega \) are observable from wheel rates, i.e., \( \operatorname{rank}(\mathbf{J}) = 3 \).

Two computational directions are used in practice:

• Inverse kinematics (command generation): given \( \mathbf{v}_B \), compute wheel rates \( \dot{\boldsymbol{\phi}} \).
• Forward kinematics (odometry): given measured \( \dot{\boldsymbol{\phi}} \), estimate \( \mathbf{v}_B \).

From \( \dot{\boldsymbol{\phi}} = \mathbf{J}\mathbf{v}_B \):

• If \( n = 3 \) and \( \det(\mathbf{J}) \neq 0 \), then \( \mathbf{v}_B = \mathbf{J}^{-1}\dot{\boldsymbol{\phi}} \).
• If \( n > 3 \), forward kinematics is overdetermined and is typically solved by least squares: \( \mathbf{v}_B = \arg\min_{\mathbf{v}} \|\mathbf{J}\mathbf{v} - \dot{\boldsymbol{\phi}}\|_2^2 \).

Least-squares solution and proof. Consider \( f(\mathbf{v}) = \|\mathbf{J}\mathbf{v} - \dot{\boldsymbol{\phi}}\|_2^2 \). Expand:

\[ f(\mathbf{v}) = (\mathbf{J}\mathbf{v}-\dot{\boldsymbol{\phi}})^T(\mathbf{J}\mathbf{v}-\dot{\boldsymbol{\phi}}) \]

Differentiate and set to zero:

\[ \nabla f(\mathbf{v}) = 2\mathbf{J}^T(\mathbf{J}\mathbf{v}-\dot{\boldsymbol{\phi}}) = \mathbf{0} \quad \Longrightarrow \quad \mathbf{J}^T\mathbf{J}\,\mathbf{v} = \mathbf{J}^T\dot{\boldsymbol{\phi}} \]

If \( \operatorname{rank}(\mathbf{J}) = 3 \), then \( \mathbf{J}^T\mathbf{J} \) is symmetric positive definite and invertible. Therefore the unique minimizer is:

\[ \mathbf{v}_B = (\mathbf{J}^T\mathbf{J})^{-1}\mathbf{J}^T\dot{\boldsymbol{\phi}} \equiv \mathbf{J}^+ \dot{\boldsymbol{\phi}} \]

where \( \mathbf{J}^+ \) is the Moore–Penrose pseudoinverse (for full-column-rank \( \mathbf{J} \)).

Redundant inverse kinematics (nullspace structure). If we command wheel rates from a desired twist, one natural choice is the minimum-norm wheel-rate solution:

\[ \dot{\boldsymbol{\phi}}^\star = \arg\min_{\dot{\boldsymbol{\phi}}}\|\dot{\boldsymbol{\phi}}\|_2 \quad \text{s.t.}\quad \dot{\boldsymbol{\phi}} = \mathbf{J}\mathbf{v}_B \]

More generally, if \( \mathbf{J} \) maps twist to wheel rates and \( n > 3 \), the family of wheel-rate vectors that realize the same twist is:

\[ \dot{\boldsymbol{\phi}} = \mathbf{J}\mathbf{v}_B + \left(\mathbf{I} - \mathbf{J}\mathbf{J}^+\right)\mathbf{z}, \qquad \mathbf{z}\in\mathbb{R}^n \]

Proof sketch. Because \( \mathbf{J}\mathbf{J}^+ \) is the projector onto \( \operatorname{Range}(\mathbf{J}) \), the term \( \left(\mathbf{I}-\mathbf{J}\mathbf{J}^+\right)\mathbf{z} \) lies in \( \operatorname{Null}(\mathbf{J}^T) \), thus does not change the least-squares-consistent twist. For command-side inverse kinematics, this “internal motion” can be used to avoid saturation or balance traction.

flowchart TD
  S["Inputs: geometry (xi, yi, beta_i, kappa_i)"] --> J["Build J = K^-1 A"]
  J --> R["Check rank(J) = 3"]
  R -->|yes| IK["Inverse: w_cmd = J * v_cmd"]
  R -->|yes| FK["Forward: v_hat = pinv(J) * w_meas"]
  IK --> SAT["Optional: scale/weight to respect wheel limits"]
  SAT --> ACT["Actuate motors"]
  ACT --> ENC["Read encoders"]
  ENC --> FK
  FK --> ODOM["Integrate pose (x,y,theta)"]
        

Consider a rectangular chassis with wheel centers at \( (\pm l_x, \pm l_y) \). Define \( a = l_x + l_y \). For the standard mecanum roller arrangement, the traction directions in the body frame are at \( \pm 45^\circ \), yielding a sign pattern that couples \( v_x \) and \( v_y \). Let the skew angle (projection) be \( \alpha = 45^\circ \), hence \( \kappa = r\cos\alpha = r/\sqrt{2} \).

Using wheel order: 1 (Front-Left), 2 (Front-Right), 3 (Rear-Left), 4 (Rear-Right), the inverse kinematics \( \dot{\boldsymbol{\phi}} = \mathbf{J}\mathbf{v}_B \) can be written:

\[ \begin{bmatrix} \dot{\phi}_1\\ \dot{\phi}_2\\ \dot{\phi}_3\\ \dot{\phi}_4 \end{bmatrix} = \frac{1}{\kappa} \begin{bmatrix} 1 & -1 & -a\\ 1 & \;\;1 & \;\;a\\ 1 & \;\;1 & -a\\ 1 & -1 & \;\;a \end{bmatrix} \begin{bmatrix} v_x\\ v_y\\ \omega \end{bmatrix} \]

Rank condition (omnidirectional mobility). The above \( \mathbf{J} \in \mathbb{R}^{4\times 3} \) has full column rank (3) provided \( \kappa \neq 0 \) and \( a \neq 0 \). One direct argument: the first column is the all-ones vector, while the second column alternates signs, so they are linearly independent. The third column equals \( [-a,\; a,\; -a,\; a]^T \), which cannot be expressed as a linear combination of the first two unless \( a = 0 \). Hence the columns are independent and \( \operatorname{rank}(\mathbf{J}) = 3 \).

Forward kinematics (least squares). Given measured wheel rates \( \dot{\boldsymbol{\phi}} \), estimate twist:

\[ \hat{\mathbf{v}}_B = \mathbf{J}^+ \dot{\boldsymbol{\phi}} = (\mathbf{J}^T\mathbf{J})^{-1}\mathbf{J}^T \dot{\boldsymbol{\phi}} \]

In the ideal symmetric case, this reduces to the familiar closed-form average:

\[ \hat{v}_x = \frac{\kappa}{4}(\dot{\phi}_1+\dot{\phi}_2+\dot{\phi}_3+\dot{\phi}_4), \quad \hat{v}_y = \frac{\kappa}{4}(-\dot{\phi}_1+\dot{\phi}_2+\dot{\phi}_3-\dot{\phi}_4), \quad \hat{\omega} = \frac{\kappa}{4a}(-\dot{\phi}_1+\dot{\phi}_2-\dot{\phi}_3+\dot{\phi}_4) \]

These expressions follow from substituting the matrix above and simplifying \( (\mathbf{J}^T\mathbf{J})^{-1}\mathbf{J}^T \).

A common non-redundant omnidirectional platform uses 3 omni wheels equally spaced around a circle of radius \( L \). Place wheel centers at angles \( \psi_i \in \{0,\; 2\pi/3,\; 4\pi/3\} \) on the circle, so \( \mathbf{r}_i = [L\cos\psi_i,\; L\sin\psi_i]^T \). Choose traction directions tangential to the circle: \( \beta_i = \psi_i + \pi/2 \). For omni wheels, set \( \alpha_i = 0 \Rightarrow \kappa_i = r \).

The Jacobian row (Section 3) becomes:

\[ \dot{\phi}_i = \frac{1}{r}\left( \cos\beta_i\,v_x + \sin\beta_i\,v_y + \omega(-y_i\cos\beta_i + x_i\sin\beta_i) \right) \]

Stacking the three equations yields a square \( 3\times 3 \) Jacobian \( \mathbf{J}_3 \). Provided the wheels are not colocated (\( L \neq 0 \)) and directions are distinct (120° spacing), \( \det(\mathbf{J}_3) \neq 0 \), hence:

\[ \mathbf{v}_B = \mathbf{J}_3^{-1}\dot{\boldsymbol{\phi}} \]

Why invertibility holds (geometric proof idea). The first two columns of \( \mathbf{J}_3 \) are the 2D unit vectors \( [\cos\beta_i,\sin\beta_i] \) sampled at three distinct angles, which span the plane. The third column introduces moment arms \( -y_i\cos\beta_i + x_i\sin\beta_i \), which are nonzero for \( L\neq 0 \). Therefore no nonzero twist \( [v_x,v_y,\omega]^T \) can map to zero wheel rates unless all three independent directional projections vanish, implying the twist is zero.

Practical considerations (kinematic level).

• Roller-induced slip: mecanum bases often experience significant lateral slip; forward kinematics becomes biased. This motivates calibration (Lesson 5) and filtering (Chapter 5).
• Wheel speed saturation: if \( |\dot{\phi}_i| \) exceeds motor limits, scale the commanded twist: \( \mathbf{v}_{cmd} \leftarrow s\,\mathbf{v}_{cmd} \) with \( s = \min_i \frac{\dot{\phi}_{max}}{|\dot{\phi}_i|} \).
• Weighted least squares: to de-emphasize noisy wheels, use \( \hat{\mathbf{v}}_B = (\mathbf{J}^T\mathbf{W}\mathbf{J})^{-1}\mathbf{J}^T\mathbf{W}\dot{\boldsymbol{\phi}} \), with diagonal weights \( \mathbf{W} \).

Robotics-adjacent libraries used below.

• Python: NumPy (linear algebra), optional Matplotlib (trajectory plot), SciPy if extended to optimization.
• C++: Eigen for matrix computations (pseudoinverse via SVD).
• Java: EJML for pseudoinverse (SVD-based).
• MATLAB/Simulink: core matrix operations, programmatic Simulink model generation; optional Robotics System Toolbox for ROS message interfaces.
• Wolfram Mathematica: built-in SVD and symbolic/numeric linear algebra.

Chapter2_Lesson4.py

# Chapter2_Lesson4.py
"""Autonomous Mobile Robots — Chapter 2 Lesson 4
Omnidirectional Bases (mecanum, Swedish wheels)

This script implements:
  1) 4-wheel mecanum inverse/forward kinematics (redundant; least-squares forward map)
  2) 3-wheel Swedish/omni base inverse/forward kinematics (square; exact inverse)
  3) A simple pose integrator for planar motion

Dependencies:
  - numpy (required)
  - matplotlib (optional, for plotting the trajectory)

Notes:
  - Kinematic model assumes ideal rolling in the constrained traction direction and free motion along rollers.
  - For mecanum wheels, the roller skew reduces the effective drive component; modeled via kappa = r*cos(alpha).
"""

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


def pinv_svd(A: np.ndarray, tol: float = 1e-10) -> np.ndarray:
    """Moore–Penrose pseudoinverse via SVD.

    Returns A^+ such that:
      - A A^+ A = A
      - A^+ A A^+ = A^+
      - (A A^+)^T = A A^+
      - (A^+ A)^T = A^+ A
    """
    U, s, Vt = np.linalg.svd(A, full_matrices=False)
    s_inv = np.zeros_like(s)
    for i, si in enumerate(s):
        if si > tol:
            s_inv[i] = 1.0 / si
    return Vt.T @ np.diag(s_inv) @ U.T


def body_to_world(vx: float, vy: float, theta: float) -> tuple[float, float]:
    """Rotate body-frame planar velocity (vx,vy) into world frame."""
    c = float(np.cos(theta))
    s = float(np.sin(theta))
    xdot = c * vx - s * vy
    ydot = s * vx + c * vy
    return xdot, ydot


@dataclass(frozen=True)
class Mecanum4:
    """4-wheel mecanum kinematics.

    Wheel order:
      1: Front-Left  (FL)
      2: Front-Right (FR)
      3: Rear-Left   (RL)
      4: Rear-Right  (RR)

    Geometry:
      - wheel radius r
      - wheel centers at (±lx, ±ly) in body frame
      - alpha: skew angle between traction direction and wheel tangential drive direction
              (alpha=45deg for classic mecanum)
    """
    r: float
    lx: float
    ly: float
    alpha: float = float(np.deg2rad(45.0))

    def jacobian(self) -> np.ndarray:
        a = self.lx + self.ly
        kappa = self.r * float(np.cos(self.alpha))  # effective drive coefficient
        if abs(kappa) < 1e-12:
            raise ValueError("kappa is ~0; check alpha and r.")
        # Mapping: w = (1/kappa) * A * [vx, vy, omega]^T
        A = np.array(
            [
                [1.0, -1.0, -a],  # FL
                [1.0,  1.0,  a],  # FR
                [1.0,  1.0, -a],  # RL
                [1.0, -1.0,  a],  # RR
            ],
            dtype=float,
        )
        return (1.0 / kappa) * A

    def inverse(self, vx: float, vy: float, omega: float) -> np.ndarray:
        v = np.array([vx, vy, omega], dtype=float)
        return self.jacobian() @ v

    def forward_ls(self, w: np.ndarray) -> np.ndarray:
        """Least-squares forward kinematics: v = argmin ||J v - w||_2."""
        J = self.jacobian()
        w = np.asarray(w, dtype=float).reshape(-1)
        if w.size != 4:
            raise ValueError("Expected 4 wheel speeds.")
        return pinv_svd(J) @ w

    def nullspace_vector(self) -> np.ndarray:
        """One convenient nullspace vector for classic mecanum configuration.

        If v = 0, wheel speeds proportional to this vector ideally yield zero chassis twist.
        """
        return np.array([1.0, -1.0, 1.0, -1.0], dtype=float)


@dataclass(frozen=True)
class Omni3:
    """3-wheel Swedish/omni base kinematics (non-redundant).

    We place 3 omni wheels on a circle of radius L at angles psi_i:
      psi = [0, 2pi/3, 4pi/3]
    Wheel traction direction is tangent to the circle:
      beta_i = psi_i + pi/2

    With ideal omni wheels, kappa = r (no skew reduction).
    """
    r: float
    L: float

    def jacobian(self) -> np.ndarray:
        psi = np.array([0.0, 2.0*np.pi/3.0, 4.0*np.pi/3.0], dtype=float)
        beta = psi + np.pi/2.0  # tangential traction direction
        x = self.L * np.cos(psi)
        y = self.L * np.sin(psi)

        kappa = self.r
        J = np.zeros((3, 3), dtype=float)
        for i in range(3):
            cb = float(np.cos(beta[i]))
            sb = float(np.sin(beta[i]))
            J[i, 0] = cb / kappa
            J[i, 1] = sb / kappa
            J[i, 2] = (-y[i]*cb + x[i]*sb) / kappa
        return J

    def inverse(self, vx: float, vy: float, omega: float) -> np.ndarray:
        v = np.array([vx, vy, omega], dtype=float)
        return self.jacobian() @ v

    def forward(self, w: np.ndarray) -> np.ndarray:
        J = self.jacobian()
        w = np.asarray(w, dtype=float).reshape(-1)
        if w.size != 3:
            raise ValueError("Expected 3 wheel speeds.")
        return np.linalg.solve(J, w)


def simulate_open_loop(base: Mecanum4 | Omni3, T: float = 8.0, dt: float = 0.01) -> np.ndarray:
    """Simple open-loop simulation: command a constant body twist and integrate pose.

    Pose state: [x, y, theta] in world frame.
    """
    # Example command (body frame): translate and rotate
    vx_cmd, vy_cmd, omega_cmd = 0.35, 0.20, 0.45

    n = int(np.ceil(T / dt))
    pose = np.zeros((n + 1, 3), dtype=float)

    # Compute wheel speeds, then reconstruct twist using forward map (mimics encoder-based odometry)
    w = base.inverse(vx_cmd, vy_cmd, omega_cmd)

    for k in range(n):
        if isinstance(base, Mecanum4):
            v_hat = base.forward_ls(w)
        else:
            v_hat = base.forward(w)

        vx, vy, omega = float(v_hat[0]), float(v_hat[1]), float(v_hat[2])
        x, y, th = pose[k, :]
        xdot, ydot = body_to_world(vx, vy, th)
        pose[k + 1, 0] = x + dt * xdot
        pose[k + 1, 1] = y + dt * ydot
        pose[k + 1, 2] = th + dt * omega

    return pose


def main() -> None:
    np.set_printoptions(precision=6, suppress=True)

    print("=== 4-wheel mecanum demo ===")
    mec = Mecanum4(r=0.05, lx=0.20, ly=0.15, alpha=float(np.deg2rad(45.0)))
    v_cmd = np.array([0.40, -0.10, 0.60], dtype=float)
    w = mec.inverse(*v_cmd)
    v_hat = mec.forward_ls(w)

    print("Commanded twist [vx, vy, omega] =", v_cmd)
    print("Wheel speeds [w1..w4] rad/s      =", w)
    print("Reconstructed twist (LS)        =", v_hat)
    print("Nullspace example vector        =", mec.nullspace_vector())

    pose = simulate_open_loop(mec, T=6.0, dt=0.01)
    print("Final pose [x, y, theta] =", pose[-1, :])

    # Optional plotting
    try:
        import matplotlib.pyplot as plt
        plt.figure()
        plt.plot(pose[:, 0], pose[:, 1])
        plt.xlabel("x [m]")
        plt.ylabel("y [m]")
        plt.title("Open-loop trajectory (mecanum)")
        plt.axis("equal")
        plt.show()
    except Exception as e:
        print("Plot skipped:", e)

    print("\n=== 3-wheel omni demo ===")
    omni = Omni3(r=0.05, L=0.18)
    v_cmd2 = np.array([0.20, 0.25, -0.40], dtype=float)
    w2 = omni.inverse(*v_cmd2)
    v_hat2 = omni.forward(w2)
    print("Commanded twist [vx, vy, omega] =", v_cmd2)
    print("Wheel speeds [w1..w3] rad/s      =", w2)
    print("Reconstructed twist (solve)      =", v_hat2)


if __name__ == "__main__":
    main()

Chapter2_Lesson4.cpp

// Chapter2_Lesson4.cpp
// Autonomous Mobile Robots — Chapter 2 Lesson 4
// Omnidirectional Bases (mecanum, Swedish wheels)
//
// Build (example):
//   g++ -O2 -std=c++17 Chapter2_Lesson4.cpp -I /path/to/eigen -o Chapter2_Lesson4
//
// Dependencies:
//   - Eigen (header-only) for linear algebra

#include <iostream>
#include <cmath>
#include <Eigen/Dense>

static Eigen::MatrixXd pinv_svd(const Eigen::MatrixXd& A, double tol = 1e-10) {
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    const Eigen::VectorXd s = svd.singularValues();
    Eigen::VectorXd s_inv(s.size());
    for (int i = 0; i < s.size(); ++i) {
        s_inv(i) = (s(i) > tol) ? (1.0 / s(i)) : 0.0;
    }
    return svd.matrixV() * s_inv.asDiagonal() * svd.matrixU().transpose();
}

struct Mecanum4 {
    double r;
    double lx;
    double ly;
    double alpha; // rad

    Eigen::Matrix<double, 4, 3> J() const {
        const double a = lx + ly;
        const double kappa = r * std::cos(alpha);
        Eigen::Matrix<double, 4, 3> A;
        A <<  1.0, -1.0, -a,
              1.0,  1.0,  a,
              1.0,  1.0, -a,
              1.0, -1.0,  a;
        return (1.0 / kappa) * A;
    }

    Eigen::Vector4d inverse(double vx, double vy, double omega) const {
        Eigen::Vector3d v(vx, vy, omega);
        return J() * v;
    }

    Eigen::Vector3d forward_ls(const Eigen::Vector4d& w) const {
        Eigen::MatrixXd Jd = J();
        Eigen::MatrixXd Jp = pinv_svd(Jd);
        return Jp * w;
    }
};

int main() {
    Mecanum4 mec;
    mec.r = 0.05;
    mec.lx = 0.20;
    mec.ly = 0.15;
    mec.alpha = M_PI / 4.0;

    Eigen::Vector3d v_cmd(0.40, -0.10, 0.60);
    Eigen::Vector4d w = mec.inverse(v_cmd(0), v_cmd(1), v_cmd(2));
    Eigen::Vector3d v_hat = mec.forward_ls(w);

    std::cout << "Commanded twist [vx vy omega]^T =\n" << v_cmd << "\n\n";
    std::cout << "Wheel speeds [w1..w4]^T (rad/s)=\n" << w << "\n\n";
    std::cout << "Reconstructed twist (LS)=\n" << v_hat << "\n";

    return 0;
}

Chapter2_Lesson4.java

// Chapter2_Lesson4.java
// Autonomous Mobile Robots — Chapter 2 Lesson 4
// Omnidirectional Bases (mecanum, Swedish wheels)
//
// Dependencies (recommended):
//   - EJML (Efficient Java Matrix Library) for SVD/pseudoinverse
//
// If EJML is not available, you can implement a small 3x4 pseudoinverse via
// SVD or use a closed-form mapping for the standard mecanum geometry.

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;

public class Chapter2_Lesson4 {

    // Build the 4x3 mecanum Jacobian J such that w = J * v, v=[vx vy omega]^T
    static DMatrixRMaj mecanumJacobian(double r, double lx, double ly, double alphaRad) {
        double a = lx + ly;
        double kappa = r * Math.cos(alphaRad);
        // Order: FL, FR, RL, RR
        double[] data = new double[] {
            1.0/kappa, -1.0/kappa, -a/kappa,
            1.0/kappa,  1.0/kappa,  a/kappa,
            1.0/kappa,  1.0/kappa, -a/kappa,
            1.0/kappa, -1.0/kappa,  a/kappa
        };
        return new DMatrixRMaj(4, 3, true, data);
    }

    static DMatrixRMaj inverseKinematics(DMatrixRMaj J, double vx, double vy, double omega) {
        DMatrixRMaj v = new DMatrixRMaj(3, 1, true, new double[]{vx, vy, omega});
        DMatrixRMaj w = new DMatrixRMaj(4, 1);
        CommonOps_DDRM.mult(J, v, w);
        return w;
    }

    static DMatrixRMaj forwardKinematicsLS(DMatrixRMaj J, DMatrixRMaj w) {
        // v = pinv(J) * w (least squares)
        DMatrixRMaj Jpinv = new DMatrixRMaj(3, 4);
        CommonOps_DDRM.pinv(J, Jpinv);
        DMatrixRMaj v = new DMatrixRMaj(3, 1);
        CommonOps_DDRM.mult(Jpinv, w, v);
        return v;
    }

    public static void main(String[] args) {
        double r = 0.05, lx = 0.20, ly = 0.15, alpha = Math.PI / 4.0;

        DMatrixRMaj J = mecanumJacobian(r, lx, ly, alpha);

        double vx = 0.40, vy = -0.10, omega = 0.60;
        DMatrixRMaj w = inverseKinematics(J, vx, vy, omega);
        DMatrixRMaj vHat = forwardKinematicsLS(J, w);

        System.out.println("Wheel speeds w (rad/s):");
        w.print();

        System.out.println("Reconstructed twist vHat:");
        vHat.print();
    }
}

Chapter2_Lesson4.m

% Chapter2_Lesson4.m
% Autonomous Mobile Robots — Chapter 2 Lesson 4
% Omnidirectional Bases (mecanum, Swedish wheels)
%
% This script demonstrates:
%   1) 4-wheel mecanum inverse/forward kinematics
%   2) A programmatically-generated Simulink model that implements w = J*v
%
% Toolbox notes:
%   - Basic MATLAB: matrix ops, pinv
%   - Simulink: model creation (new_system, add_block, etc.)
%   - Robotics System Toolbox (optional): for ROS Twist message interfaces

function Chapter2_Lesson4()
    % Geometry
    r = 0.05;     % wheel radius [m]
    lx = 0.20;    % half-length in x [m]
    ly = 0.15;    % half-width  in y [m]
    alpha = deg2rad(45); % skew angle [rad]
    a = lx + ly;
    kappa = r*cos(alpha);

    % Jacobian: w = (1/kappa) * A * v, v = [vx; vy; omega]
    J = (1/kappa) * [ 1  -1  -a;
                      1   1   a;
                      1   1  -a;
                      1  -1   a ];

    % Example command
    v = [0.40; -0.10; 0.60];
    w = J*v;

    % Forward (least squares)
    v_hat = pinv(J)*w;

    disp('Commanded v = [vx; vy; omega]'); disp(v);
    disp('Wheel speeds w = [w1; w2; w3; w4]'); disp(w);
    disp('Reconstructed v_hat (LS)'); disp(v_hat);

    % Build a simple Simulink model implementing w = J*v
    build_simulink_model(J);
    fprintf('Simulink model generated: Chapter2_Lesson4_Simulink.slx (in current folder)\n');
end

function build_simulink_model(J)
    mdl = 'Chapter2_Lesson4_Simulink';
    if bdIsLoaded(mdl)
        close_system(mdl, 0);
    end
    if exist([mdl '.slx'], 'file')
        delete([mdl '.slx']);
    end

    new_system(mdl);
    open_system(mdl);

    % Blocks: v (3x1) constant -> Gain (J) -> w (4x1) -> Scope
    add_block('simulink/Sources/Constant', [mdl '/v_cmd'], 'Value', 'v_cmd');
    add_block('simulink/Math Operations/Gain', [mdl '/J_gain'], 'Gain', 'J');
    add_block('simulink/Sinks/Scope', [mdl '/w_scope']);

    % Set positions
    set_param([mdl '/v_cmd'], 'Position', [80 70 140 110]);
    set_param([mdl '/J_gain'], 'Position', [220 60 320 120]);
    set_param([mdl '/w_scope'], 'Position', [420 65 450 115]);

    % Wire
    add_line(mdl, 'v_cmd/1', 'J_gain/1');
    add_line(mdl, 'J_gain/1', 'w_scope/1');

    % Workspace variables used by the model
    assignin('base', 'J', J);
    assignin('base', 'v_cmd', [0.40; -0.10; 0.60]);

    save_system(mdl);
end

Chapter2_Lesson4.nb

(* Chapter2_Lesson4.nb
   Autonomous Mobile Robots — Chapter 2 Lesson 4
   Omnidirectional Bases (mecanum, Swedish wheels)

   This notebook-style file (plain-text Notebook expression) contains:
     - construction of the mecanum Jacobian
     - pseudoinverse via SVD
     - nullspace check and numerical example
*)

Notebook[{
  Cell["Autonomous Mobile Robots — Chapter 2 Lesson 4: Omnidirectional Bases", "Title"],
  Cell["Mecanum 4-wheel Jacobian and pseudoinverse (SVD)", "Section"],

  Cell[BoxData[
    RowBox[{
      RowBox[{"r","=","0.05"}],";"," ",
      RowBox[{"lx","=","0.20"}],";"," ",
      RowBox[{"ly","=","0.15"}],";"," ",
      RowBox[{"alpha","=","Pi","/","4"}],";"," ",
      RowBox[{"a","=","lx","+","ly"}],";"," ",
      RowBox[{"kappa","=","r","*","Cos","[","alpha","]"}],";"
    }]
  ], "Input"],

  Cell[BoxData[
    RowBox[{
      RowBox[{"J","=",
        RowBox[{
          RowBox[{"1","/","kappa"}],"*",
          RowBox[{"{",
            RowBox[{
              RowBox[{"{","1",",","-1",",",RowBox[{"-a"}],"}"}],",",
              RowBox[{"{","1",",","1",",","a","}"}],",",
              RowBox[{"{","1",",","1",",",RowBox[{"-a"}],"}"}],",",
              RowBox[{"{","1",",","-1",",","a","}" }]
            }],"}"
          }]
        }]
      }],";"
    }]
  ], "Input"],

  Cell[BoxData["MatrixForm[J]"], "Input"],

  Cell["SVD-based pseudoinverse", "Subsection"],
  Cell[BoxData[
    RowBox[{
      RowBox[{"pinvSVD","[","A_","]",":=",
        RowBox[{"Module","[",
          RowBox[{
            RowBox[{"{","u"," ,","s",",","v"," ,","sinv","}"}],",",
            RowBox[{
              RowBox[{
                RowBox[{"{","u",",","s",",","v","}"}],"=",
                RowBox[{"SingularValueDecomposition","[","A","]"}
              }],";",
              RowBox[{"sinv","=",
                RowBox[{"DiagonalMatrix","[",
                  RowBox[{
                    RowBox[{"Map","[",
                      RowBox[{
                        RowBox[{"If","[",
                          RowBox[{RowBox[{"#>10^-10"}],",",RowBox[{"1/#"}],",","0"}],"]"}], "&"
                      }],",","s"
                    }],"]"}],"]"
              }],";",
              RowBox[{"v",".","sinv",".",RowBox[{"Transpose","[","u","]"}]}
            }]
          }],"]"}
      }],";"
    }]
  ], "Input"],

  Cell[BoxData[
    RowBox[{"Jp","=","pinvSVD","[","J","]",";"}]
  ], "Input"],

  Cell[BoxData["Dimensions[Jp]"], "Input"],
  Cell[BoxData["MatrixForm[Jp]"], "Input"],

  Cell["Numerical example", "Subsection"],
  Cell[BoxData[
    RowBox[{
      RowBox[{"v","=",
        RowBox[{"{","0.40",",","-0.10",",","0.60","}"}]
      }],";"," ",
      RowBox[{"w","=","J",".","v"}],";"," ",
      RowBox[{"vhat","=","Jp",".","w"}],";"
    }]
  ], "Input"],

  Cell[BoxData["w"], "Input"],
  Cell[BoxData["vhat"], "Input"],

  Cell["Nullspace sanity check (one internal mode)", "Subsection"],
  Cell[BoxData[
    RowBox[{
      RowBox[{"z","=",
        RowBox[{"{","1",",","-1",",","1",",","-1","}"}
      }],";"," ",
      RowBox[{"Jp",".","z"}]
    }]
  ], "Input"]
}]

Problem 1 (Derive a Jacobian row): For a Swedish wheel at body-frame location \( (x_i,y_i) \) with traction direction angle \( \beta_i \) and effective drive factor \( \kappa_i \), derive the scalar constraint relating \( (v_x,v_y,\omega) \) to \( \dot{\phi}_i \).

Solution: From Section 2, \( \mathbf{v}_{c,i} = [v_x,\; v_y]^T + \omega[-y_i,\; x_i]^T \). The traction component is \( \mathbf{u}_i^T \mathbf{v}_{c,i} \) with \( \mathbf{u}_i = [\cos\beta_i,\; \sin\beta_i]^T \). Enforcing \( \mathbf{u}_i^T \mathbf{v}_{c,i} = \kappa_i \dot{\phi}_i \) gives:

\[ \cos\beta_i\,v_x + \sin\beta_i\,v_y + \omega\left(-y_i\cos\beta_i + x_i\sin\beta_i\right) = \kappa_i \dot{\phi}_i \]

Thus the Jacobian row is \( \frac{1}{\kappa_i}[\cos\beta_i,\; \sin\beta_i,\; -y_i\cos\beta_i + x_i\sin\beta_i] \).

Problem 2 (Mecanum inverse kinematics numeric): A mecanum base has \( r=0.05 \) m, \( l_x=0.20 \) m, \( l_y=0.15 \) m, \( \alpha=45^\circ \). Compute wheel rates for \( (v_x,v_y,\omega) = (0.40,\; -0.10,\; 0.60) \).

Solution: Here \( a=l_x+l_y=0.35 \) and \( \kappa=r\cos\alpha=0.05/\sqrt{2} \). Using Section 5:

\[ \dot{\boldsymbol{\phi}} = \frac{1}{\kappa} \begin{bmatrix} 1 & -1 & -a\\ 1 & 1 & a\\ 1 & 1 & -a\\ 1 & -1 & a \end{bmatrix} \begin{bmatrix} 0.40\\ -0.10\\ 0.60 \end{bmatrix} \]

Compute the inner products row-wise: \( s_1 = 0.40 -(-0.10) -0.35(0.60) = 0.40+0.10-0.21=0.29 \), \( s_2 = 0.40 +(-0.10) +0.35(0.60)=0.30+0.21=0.51 \), \( s_3 = 0.40 +(-0.10) -0.35(0.60)=0.30-0.21=0.09 \), \( s_4 = 0.40 -(-0.10) +0.35(0.60)=0.50+0.21=0.71 \). Hence \( \dot{\phi}_i = s_i/\kappa \).

Problem 3 (Least-squares forward kinematics): For a 4-wheel mecanum base, show that the least-squares twist estimate satisfies \( \mathbf{J}^T\mathbf{J}\hat{\mathbf{v}}=\mathbf{J}^T\dot{\boldsymbol{\phi}} \) and write the closed-form solution when \( \operatorname{rank}(\mathbf{J})=3 \).

Solution: Minimize \( \|\mathbf{J}\mathbf{v}-\dot{\boldsymbol{\phi}}\|_2^2 \). Setting the gradient to zero yields the normal equations:

\[ 2\mathbf{J}^T(\mathbf{J}\hat{\mathbf{v}}-\dot{\boldsymbol{\phi}})=\mathbf{0} \;\Longrightarrow\; \mathbf{J}^T\mathbf{J}\hat{\mathbf{v}}=\mathbf{J}^T\dot{\boldsymbol{\phi}} \]

If \( \operatorname{rank}(\mathbf{J})=3 \), then \( \mathbf{J}^T\mathbf{J} \) is invertible, so:

\[ \hat{\mathbf{v}}=(\mathbf{J}^T\mathbf{J})^{-1}\mathbf{J}^T\dot{\boldsymbol{\phi}}=\mathbf{J}^+\dot{\boldsymbol{\phi}} \]

Problem 4 (Nullspace / internal motion): For the classic mecanum Jacobian in Section 5, verify that \( \mathbf{z}=[1,-1,1,-1]^T \) corresponds to an internal wheel-speed mode that does not change the estimated chassis twist under \( \hat{\mathbf{v}}=\mathbf{J}^+\dot{\boldsymbol{\phi}} \).

Solution: Because \( \mathbf{J}^+ \) is a left-inverse on \( \operatorname{Range}(\mathbf{J}) \), any component in \( \operatorname{Null}(\mathbf{J}^+) \) (equivalently orthogonal to the range in the least-squares sense) does not affect \( \hat{\mathbf{v}} \). In the provided implementations, compute \( \mathbf{J}^+\mathbf{z} \); it evaluates (numerically) near zero, confirming it is an internal mode.

Problem 5 (Weighted least squares): Suppose wheel-rate measurement noise has covariance \( \mathbf{\Sigma}=\operatorname{diag}(\sigma_1^2,\dots,\sigma_n^2) \). Derive the maximum-likelihood (Gauss–Markov) twist estimate.

Solution: With Gaussian noise, the negative log-likelihood is proportional to \( (\mathbf{J}\mathbf{v}-\dot{\boldsymbol{\phi}})^T\mathbf{\Sigma}^{-1}(\mathbf{J}\mathbf{v}-\dot{\boldsymbol{\phi}}) \). Minimizing yields the weighted normal equations:

\[ \mathbf{J}^T\mathbf{\Sigma}^{-1}\mathbf{J}\,\hat{\mathbf{v}} = \mathbf{J}^T\mathbf{\Sigma}^{-1}\dot{\boldsymbol{\phi}} \quad\Longrightarrow\quad \hat{\mathbf{v}}= (\mathbf{J}^T\mathbf{W}\mathbf{J})^{-1}\mathbf{J}^T\mathbf{W}\dot{\boldsymbol{\phi}}, \;\mathbf{W}=\mathbf{\Sigma}^{-1} \]

We modeled omnidirectional bases by (i) computing contact-point velocities from the planar chassis twist, (ii) projecting those velocities onto each wheel’s traction direction to obtain a scalar wheel constraint, and (iii) stacking constraints into a Jacobian \( \mathbf{J} \) relating wheel rates to chassis twist. We proved least-squares forward kinematics via the normal equations, introduced pseudoinverses and nullspace motion for redundancy, and specialized the framework to classic 4-wheel mecanum and 3-wheel omni (Swedish) configurations. These kinematic foundations will be used in Lesson 5 for calibration of wheel parameters.