Theoretical Background: Velocity-Level Control and Mobile Robot Kinematics

Module 3 Theory: Jacobian of Mobile Robots and Velocity Control

3.2.1 Inverse Velocity Problem for 2-DOF

The forward velocity problem ($\dot{x}=J\dot{q}$) computes end-effector velocity from known joint velocities. The inverse velocity problem reverses this: given a desired end-effector velocity $\dot{x}$, determine the required joint velocities $\dot{q}$.

For the 2-DOF planar robot, the Jacobian is a $2\times 2$ square matrix. When the determinant is nonzero, a unique solution exists:

The inverse of a $2\times 2$ matrix has a closed-form expression. For $J$ with entries $J_{11}$, $J_{12}$, $J_{21}$, $J_{22}$:

This is the classical adjugate formula. It requires only the four entries of $J$ and the determinant, making it computationally efficient for real-time control.

Behavior Near Singularity: As ${\theta }_2$ approaches $0$ or $\pi$, $\det (J)=l_1{l}_2\sin {\theta }_2$ approaches zero, and each entry of $J^{-1}$ grows without bound. The resulting joint velocities become:

Even a modest desired end-effector velocity will demand physically impossible joint speeds. In practice, actuators saturate, and the robot fails to track the desired trajectory.

Damped Least-Squares (DLS) Solution: To prevent joint velocity explosion near singularities, a regularization term is added:

where $\lambda$ is a damping factor. When the robot is far from singularity, $J{J}^T$ dominates and the solution approaches $J^{-1}\dot{x}$. Near singularity, the ${\lambda }^{2}I$ term prevents the inverse from diverging, at the cost of tracking accuracy. The damping factor $\lambda$ is typically chosen based on the manipulability index: small when $w$ is large, increased as $w$ decreases toward zero.

Trade-Off: Damping sacrifices end-effector velocity tracking accuracy to maintain bounded joint velocities. The robot will not exactly follow the desired task-space trajectory near a singularity, but it will remain controllable.

3.2.2 The Jacobian of a Differential Drive Robot

The concept of a Jacobian — a matrix mapping one set of velocities to another — extends naturally from manipulators to mobile robots. A differential drive robot has two independently driven wheels that produce translational and rotational motion of the body. The velocity-level kinematics of this system can be expressed as a chain of Jacobian-like matrices.

Unicycle Model: The simplest kinematic model treats the robot as a point with heading $\theta$, linear velocity $v$, and angular velocity $\omega$. The world-frame velocities are:

This can be written compactly as a matrix-vector product — a Jacobian relating body-frame control inputs to world-frame velocities:

where $\dot{\xi }=[\dot{x},\dot{y},\dot{\theta }{]}^T$ and $S(\theta )$ is the $3\times 2$ configuration-dependent matrix:

Key Observation: $S(\theta )$ is the mobile robot’s analog of the manipulator Jacobian. It maps body-frame velocities $(v,\omega )$ to world-frame velocities $(\dot{x},\dot{y},\dot{\theta })$, just as the manipulator Jacobian maps joint velocities to end-effector velocities.

Wheel-to-Body Jacobian: The body-frame velocities $(v,\omega )$ are themselves produced by the two wheel angular velocities $({\omega }_R,{\omega }_L)$. This mapping is governed by the differential drive geometry:

where $r$ is the wheel radius and $L$ is the wheel separation (track width). This $2\times 2$ matrix is the wheel-to-body Jacobian. It is a constant matrix — unlike the manipulator Jacobian, it does not depend on configuration.

Full Velocity Chain: The complete mapping from wheel velocities to world-frame velocities is the composition of both Jacobians:

This is directly analogous to a serial chain of Jacobians in a multi-stage manipulator system: each stage maps one velocity space to the next.

Singularity Analysis of $S(\theta )$: The matrix $S(\theta )$ is $3\times 2$ and has rank 2 for all values of $\theta$. The two column vectors of $S(\theta )$ are $[\cos \theta ,\sin \theta ,0{]}^T$ and $[0,0,1{]}^T$, which are never parallel. Therefore, $S(\theta )$ has no singularities in the manipulator sense — the robot can always generate both forward motion and rotation, regardless of its heading.

Nonholonomic Constraints Replace Singularities: The mobile robot’s fundamental limitation is not a singularity but a nonholonomic constraint. The robot cannot move sideways (perpendicular to its heading). This constraint is:

This means the 3D velocity space $(\dot{x},\dot{y},\dot{\theta })$ is constrained to a 2D subspace. The robot can eventually reach any position and orientation (controllability is preserved), but it cannot do so by moving instantaneously in all directions. It must execute multi-step maneuvers, such as the parallel parking sequence familiar from everyday driving.

3.2.3 Velocity-Based Control for Mobile Robots

Just as resolved-rate control uses the Jacobian to convert desired end-effector velocities into joint commands, mobile robot controllers convert desired world-frame motion into wheel commands. The go-to-goal controller is the simplest and most widely used example.

Go-to-Goal Controller: Given a goal position $(x_{\mathrm{goal}},y_{\mathrm{goal}})$ and the robot’s current state $(x,y,\theta )$:

Step 1 – Compute desired heading:

Step 2 – Compute angular velocity to align with goal:

where $K_p$ is a proportional gain. The heading error $({\theta }_d-\theta )$ must be wrapped to the interval $[-\pi ,\pi ]$ to avoid discontinuities.

Step 3 – Compute linear velocity scaled by alignment:

where $d=\sqrt{(x_{\mathrm{goal}}-x{)}^2+(y_{\mathrm{goal}}-y{)}^2}$ is the distance to goal, $e_{\theta }={\theta }_d-\theta$ is the heading error, and $K_v$ is a proportional gain. The $\cos (e_{\theta })$ factor reduces forward speed when the robot is not well aligned, preventing it from driving away from the goal while turning.

Analogy to Resolved-Rate Control: This controller follows the same pattern as manipulator resolved-rate control. The desired task-space velocity (move toward the goal) is mapped through the inverse of the velocity kinematics to produce actuator commands $(v,\omega )$. These are then converted to wheel velocities using the inverse of Equation (8):

The full chain is: task-space goal $\to$ desired body velocity $\to$ wheel commands, mirroring the manipulator chain: task-space trajectory $\to$ desired end-effector velocity $\to$ joint velocity commands.

3.2.4 Connecting Manipulator and Mobile Robot Concepts

Both manipulators and mobile robots are governed by velocity-level mappings of the form $\dot{x}=J(q)\dot{q}$. The unifying concept is the Jacobian as a configuration-dependent linear map between velocity spaces.

Manipulator Jacobian:

• Configuration dependence: $J$ changes with joint angles
• Singularities: Determinant drops to zero at specific configurations, causing loss of DOF in task space
• Matrix shape: Square ($n=m$) or rectangular ($n\ne m$), depending on number of joints vs. task dimensions
• Inverse: Direct inverse for square nonsingular $J$, pseudo-inverse for rectangular $J$
• Constraints: None at velocity level (all joint velocities are independent)

Mobile Robot Jacobian ($S$ matrix):

• Configuration dependence: $S$ changes with heading angle $\theta$
• Singularities: None — $S$ always has full column rank
• Matrix shape: $3\times 2$ (rectangular — three task-space dimensions, two control inputs)
• Inverse: Not directly invertible; uses control law (Equations 9–11) to select from the constrained velocity space
• Constraints: Nonholonomic constraint eliminates one velocity direction (no lateral motion)

Mobile Manipulators: In industrial applications, a manipulator arm is often mounted on a mobile base, creating a mobile manipulator. The full Jacobian of such a system combines the base Jacobian ($S$ matrix and wheel kinematics) with the arm Jacobian into a single matrix that maps all actuator velocities (wheels + joints) to end-effector velocities. This combined system is typically redundant (more actuators than end-effector DOFs), which introduces both opportunities (null-space optimization) and challenges (pseudo-inverse computation, redundancy resolution).

3.2.5 Brief Awareness: Redundancy and Pseudo-Inverse

When a robot has more degrees of freedom than the task requires ($n>m$), the Jacobian is rectangular with more columns than rows, and the system $\dot{x}=J\dot{q}$ has infinitely many solutions. This occurs in 7-DOF manipulator arms, in mobile manipulators, and even in simple cases like a 3-joint planar arm performing a 2D task.

Minimum-Norm Solution: Among all joint velocities that produce the desired end-effector velocity, the one with smallest norm is:

where $J^{+}=J^T(J{J}^T{)}^{-1}$ is the Moore-Penrose pseudo-inverse. This selects the least-effort solution in joint space.

Null-Space Motion: The general solution is $\dot{q}=J^{+}\dot{x}+(I-J^{+}J)\mathbf{z}$, where $\mathbf{z}$ is an arbitrary vector and $(I-J^{+}J)$ projects $\mathbf{z}$ into the null space of $J$. This allows the robot to move its joints without affecting the end-effector, which can be used to avoid joint limits, avoid obstacles, or optimize manipulability — all while tracking the desired task-space trajectory.

Scope Note: Redundancy resolution is an active area of research and is used extensively in 7-DOF collaborative robots (e.g., KUKA iiwa, Franka Emika) and in mobile manipulation platforms. The mathematics builds directly on the 2-DOF Jacobian concepts developed in this module, but the implementation details are beyond the scope of this course. The key takeaway is that the pseudo-inverse generalizes the direct inverse, and the null space provides extra freedom that can be exploited for secondary objectives.

Script Implementation

        # Velocity-based go-to-goal controller using Jacobian concepts
        # Maps desired world velocity to wheel commands via the mobile robot Jacobian
        import numpy as np
        
        
        def unicycle_jacobian(theta):
        # S(theta) matrix -- Eq. (7)
        # Maps [v, omega] to [x_dot, y_dot, theta_dot]
        S = np.array([[np.cos(theta), 0],
        [np.sin(theta), 0],
        [0,             1]])
        return S
        
        
        def wheel_to_body_jacobian(r, L):
        # Constant wheel-to-body Jacobian -- Eq. (8)
        # Maps [omega_R, omega_L] to [v, omega]
        J_wheel = np.array([[r/2,  r/2],
        [r/L, -r/L]])
        return J_wheel
        
        
        def go_to_goal(state, goal, Kp, Kv):
        # Implements Equations (9), (10), (11)
        x, y, theta = state
        x_goal, y_goal = goal
        
        # Distance to goal
        d = np.sqrt((x_goal - x)**2 + (y_goal - y)**2)
        
        # Desired heading -- Eq. (9)
        theta_d = np.arctan2(y_goal - y, x_goal - x)
        
        # Heading error wrapped to [-pi, pi]
        e_theta = theta_d - theta
        e_theta = np.arctan2(np.sin(e_theta), np.cos(e_theta))
        
        # Angular velocity -- Eq. (10)
        omega = Kp * e_theta
        
        # Linear velocity -- Eq. (11)
        v = Kv * d * np.cos(e_theta)
        
        return v, omega
        
        
        def body_to_wheel(v, omega, r, L):
        # Inverse of Eq. (8): maps [v, omega] to [omega_R, omega_L]
        omega_R = (v + omega * L / 2) / r
        omega_L = (v - omega * L / 2) / r
        return omega_R, omega_L
        
        
        # Robot parameters
        r = 0.033    # wheel radius (m)
        L = 0.16     # wheel separation (m)
        Kp = 3.0     # angular gain
        Kv = 0.5     # linear gain
        
        # Current state and goal
        state = np.array([0.0, 0.0, 0.0])   # at origin, facing right
        goal  = np.array([1.0, 1.0])         # goal at (1, 1)
        
        # Compute control commands
        v, omega = go_to_goal(state, goal, Kp, Kv)
        print("Body velocities: v = {:.3f} m/s, omega = {:.3f} rad/s".format(v, omega))
        
        # Map to wheel commands using inverse wheel Jacobian
        omega_R, omega_L = body_to_wheel(v, omega, r, L)
        print("Wheel velocities: omega_R = {:.3f} rad/s, omega_L = {:.3f} rad/s".format(
        omega_R, omega_L))
        
        # Verify using the full Jacobian chain
        S       = unicycle_jacobian(state[2])
        J_wheel = wheel_to_body_jacobian(r, L)
        xi_dot  = S @ np.array([v, omega])
        print("World-frame velocity: x_dot = {:.3f}, y_dot = {:.3f}, theta_dot = {:.3f}".format(
        *xi_dot))
        
        # Show that S(theta) always has rank 2
        print("Rank of S(theta):", np.linalg.matrix_rank(S))

Integration: Theory to Practice

The $S(\theta )$ matrix is implicitly used every time a /cmd_vel message is published in ROS 2. When a controller node publishes a Twist message containing $(v,\omega )$, the robot’s firmware converts these body-frame velocities to individual wheel commands using Equation (8). The web-browser simulation platform’s virtual_robot.py implements this exact mapping in its update loop: it reads the commanded $(v,\omega )$, applies the unicycle kinematic model (Equations 4–6), and integrates forward to update the robot’s pose. The go-to-goal controller developed here maps directly to a ROS 2 node that subscribes to /odom for state estimation and publishes to /cmd_vel for actuation. The complete velocity chain — from goal position to desired body velocity to wheel commands — mirrors the resolved-rate control architecture used in manipulator systems, with the $S(\theta )$ matrix playing the role of the manipulator Jacobian.

Theoretical Design Choices

The mobile robot Jacobian ($S$ matrix) avoids the singularity problems that complicate manipulator control because the two column vectors of $S(\theta )$ are always linearly independent for any heading angle. This is a fundamental geometric property: forward motion and rotation about the vertical axis are always independent directions of motion for a wheeled robot on a plane. The nonholonomic constraint (no lateral motion) replaces manipulator singularities as the primary kinematic limitation, but unlike a singularity, this constraint does not depend on configuration — it is always present and always restricts the same direction relative to the robot body. Velocity-level control is preferred over position-level control for mobile robots because the nonholonomic constraint is expressed at the velocity level, and direct position control would require solving the nonlinear kinematic equations globally rather than linearizing them locally. The Jacobian framework provides this local linearization naturally, making it the appropriate tool for both manipulator and mobile robot control design.