Omnidirectional Wheeled Robots

An omnidirectional robot can move its chassis in any direction while rotating. The wheeled robots overview introduces the wheel types, the chassis configuration

q = (\theta, x, y)

and the body twist

\mathcal{V}_b

used below. You will derive the wheel speeds that produce a given body twist,

u = H(0)\,\mathcal{V}_b

. You will then compare them with a physics simulation of a mecanum robot. This section follows chapter 13, Wheeled Mobile Robots, of Kevin Lynch and Frank Park, Modern Robotics: Mechanics, Planning, and Control (Cambridge University Press, 2017) (Kevin M. Lynch, 2017).

import math
from pathlib import Path

import matplotlib.pyplot as plt
import numpy as np
import rerun as rr
import rerun.blueprint as rrb
import warp as wp

wp.config.log_level = wp.LOG_WARNING
import newton
import newton.viewer
from newton.examples.robot.example_robot_omniwheel import add_mecanum_wheel

RECORDINGS = Path("recordings")
RECORDINGS.mkdir(exist_ok=True)

Omnidirectional robots

Place wheel

i

(x_i, y_i)

\{b\}

. Its driving direction is at angle

\beta_i

from

\hat{x}_b

. Its radius is

r_i

, and its free sliding direction is at angle

\gamma_i

from the driving direction. Chassis motion gives the wheel center a linear velocity. The motor must produce only the component along the driving direction. The rollers absorb the remaining component. From this relation, you can express the driving angular speed

u_i

of wheel

i

as a linear function of the body twist:

u_i = h_i(0)\, \mathcal{V}_b, \qquad h_i(0) = \frac{1}{r_i \cos\gamma_i} \begin{bmatrix} x_i \sin(\beta_i + \gamma_i) - y_i \cos(\beta_i + \gamma_i) & \cos(\beta_i + \gamma_i) & \sin(\beta_i + \gamma_i) \end{bmatrix}

Stacking one row for each wheel gives

u = H(0)\,\mathcal{V}_b

. When

H(0)

has rank 3, every body twist has a corresponding set of wheel speeds. This property makes the robot omnidirectional. With more than three wheels,

H(0)

has more rows than columns. Wheel speeds outside the form

H(0)\mathcal{V}_b

cause the wheels to fight each other and skid. The code below builds

H(0)

from the wheel geometry for two standard designs. The first uses three omniwheels spaced

120°

apart at distance

d

from the center. The second uses four mecanum wheels at

(\pm\ell, \pm w)

def wheel_row(x, y, beta, gamma, r):
    # h_i(0): maps the body twist (omega_bz, v_bx, v_by) to the driving speed of one wheel
    a = beta + gamma
    return np.array([x * np.sin(a) - y * np.cos(a), np.cos(a), np.sin(a)]) / (r * np.cos(gamma))


def H_three_omni(d, r):
    # wheels at 90, -30 and 210 degrees, each driving tangentially
    rows = []
    for angle in np.deg2rad([90.0, -30.0, 210.0]):
        rows.append(wheel_row(d * np.cos(angle), d * np.sin(angle), angle - np.pi / 2, 0.0, r))
    return np.array(rows)


def H_mecanum(l, w, r):
    # wheels numbered front-left, front-right, rear-right, rear-left; rollers alternate at -45 and +45 degrees
    specs = [(l, w, -np.pi / 4), (l, -w, np.pi / 4), (-l, -w, -np.pi / 4), (-l, w, np.pi / 4)]
    return np.array([wheel_row(x, y, 0.0, g, r) for x, y, g in specs])


d, r = 0.2, 0.05
H3 = H_three_omni(d, r)
print("three omniwheels, r * H(0):\n", np.round(r * H3, 3))
print("rank", np.linalg.matrix_rank(H3))

l, w = 0.32, 0.40
H4 = H_mecanum(l, w, r)
print("\nfour mecanum wheels, r * H(0):\n", np.round(r * H4, 3))
print("rank", np.linalg.matrix_rank(H4))

three omniwheels, r * H(0):
 [[-0.2    1.     0.   ]
 [-0.2   -0.5   -0.866]
 [-0.2   -0.5    0.866]]
rank 3

four mecanum wheels, r * H(0):
 [[-0.72  1.   -1.  ]
 [ 0.72  1.    1.  ]
 [ 0.72  1.   -1.  ]
 [-0.72  1.    1.  ]]
rank 3

When scaled by

r

, the second matrix is Lynch and Park’s equation (13.10). Its rows are

(-\ell - w, 1, -1)

(\ell + w, 1, 1)

(\ell + w, 1, -1)

and

(-\ell - w, 1, 1)

. You can read its columns as follows:

To move along $+\hat{x}_b$ , all four wheels drive forward at the same speed.
To move along $+\hat{y}_b$ , wheels 1 and 3 drive backward, while wheels 2 and 4 drive forward.
To rotate counterclockwise, wheels 1 and 4 drive backward, while wheels 2 and 3 drive forward.

The chassis can move sideways as fast as it moves forward because the second and third columns have the same magnitude.

Simulating the robot

The simulations use Newton, a robotics physics engine built on NVIDIA Warp (Data Curation for Machine Lear, n.d.). The robot consists of rigid links connected by joints. Its collision shapes have a friction coefficient

\mu

. Each wheel is a link on a revolute joint, and its motor tracks a commanded angular speed. The floor is a plane. The simulation has no built-in knowledge of

H(0)

or the no-slip condition. Friction at each contact pushes on the wheels and moves the chassis. The helpers below run a simulation at 60 frames per second. They command the wheel speeds at each frame and record the chassis pose

(\theta, x, y)

and the wheel angles. The body twist is measured from the recorded poses using the definition above. You can also record each run for the Rerun viewer. It displays the robot, logged paths and signals on a single timeline that you can scrub.

def planar_pose(body_q):
    # (theta, x, y) of a body from its Newton transform (px, py, pz, qx, qy, qz, qw)
    px, py, _, qx, qy, qz, qw = body_q
    theta = math.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz))
    return theta, px, py


def measured_body_twist(t, pose):
    # finite-difference the pose, then map q_dot to V_b = (omega_bz, v_bx, v_by)
    theta = np.unwrap(pose[:, 0])
    th_dot, x_dot, y_dot = (np.gradient(v, t) for v in (theta, pose[:, 1], pose[:, 2]))
    c, s = np.cos(theta), np.sin(theta)
    return np.stack([th_dot, c * x_dot + s * y_dot, -s * x_dot + c * y_dot], axis=1)


def simulate(model, chassis, drives, wheel_speeds, duration, recording=None, on_frame=None, blueprint=None,
             fps=60, substeps=6):
    solver = newton.solvers.SolverMuJoCo(
        model, use_mujoco_contacts=False, disable_sensors=True, solver="newton", integrator="implicitfast",
        cone="elliptic", iterations=20, ls_iterations=50, njmax=2048, nconmax=1024,
    )
    state, next_state, control = model.state(), model.state(), model.control()
    newton.eval_fk(model, model.joint_q, model.joint_qd, state)
    collision = newton.CollisionPipeline(model, rigid_contact_max=1024)
    contacts = collision.contacts()
    drive_dofs = [int(model.joint_qd_start.numpy()[j]) for j in drives]
    drive_coords = [int(model.joint_q_start.numpy()[j]) for j in drives]
    target = control.joint_target_qd.numpy()
    dt = 1.0 / fps / substeps

    def advance():
        nonlocal state, next_state
        for _ in range(substeps):
            state.clear_forces()
            collision.collide(state, contacts)
            solver.step(state, next_state, control, contacts, dt)
            state, next_state = next_state, state

    graph = None
    if wp.get_device().is_cuda:
        with wp.ScopedCapture() as capture:
            advance()
        graph = capture.graph

    viewer = None
    if recording is not None:
        viewer = newton.viewer.ViewerRerun(app_id="wheeled_robots_" + recording.stem.split("-0")[0].replace("-", "_"),
                                           record_to_rrd=str(recording), keep_historical_data=True)
        viewer.set_model(model)
        if blueprint is not None:
            rr.send_blueprint(blueprint)

    t, times, poses, angles = 0.0, [], [], []
    for _ in range(int(duration * fps)):
        target[drive_dofs] = wheel_speeds(t)
        control.joint_target_qd.assign(target)
        wp.capture_launch(graph) if graph else advance()
        t += 1.0 / fps
        times.append(t)
        poses.append(planar_pose(state.body_q.numpy()[chassis]))
        angles.append(state.joint_q.numpy()[drive_coords])
        if viewer is not None:
            viewer.begin_frame(t)
            viewer.log_state(state)
            if on_frame is not None:
                on_frame(np.array(times), np.array(poses), np.array(angles))
            viewer.end_frame()
    if viewer is not None:
        viewer.close()
    return np.array(times), np.array(poses), np.array(angles)


def displace(q, twist, dt):
    # move the chassis at a constant body twist for time dt (exponential of the planar twist)
    w, vx, vy = np.asarray(twist) * dt
    if abs(w) < 1e-12:
        db = np.array([0.0, vx, vy])
    else:
        db = np.array([w, (vx * np.sin(w) + vy * (np.cos(w) - 1)) / w, (vy * np.sin(w) + vx * (1 - np.cos(w))) / w])
    c, s = np.cos(q[0]), np.sin(q[0])
    return q + np.array([db[0], c * db[1] - s * db[2], s * db[1] + c * db[2]])


def integrate_twists(q0, twists, dt):
    path = [np.asarray(q0, float)]
    for tw in twists[1:]:
        path.append(displace(path[-1], tw, dt))
    return np.array(path)


VIEWER = "https://artifacts.aegeanai.com/rerun/viewer/0.41.0/index.html?url="
RECORDING_URL = "https://artifacts.aegeanai.com/rerun/wheeled-robots/{}-0.41.rrd"


def show_recording(name, height=560):
    # the published recording in the self-hosted Rerun web viewer
    from urllib.parse import quote
    from IPython.display import IFrame
    src = VIEWER + quote(RECORDING_URL.format(name), safe="")
    return IFrame(src, width="100%", height=height, extras=['allow="fullscreen"', 'loading="lazy"'])


def path_strip(poses, z=0.01):
    return np.column_stack([poses[:, 1], poses[:, 2], np.full(len(poses), z)])


def layout(*plots):
    # the robot in 3D on the left, one time-series panel per signal on the right
    return rrb.Blueprint(
        rrb.Horizontal(rrb.Spatial3DView(origin="/"), rrb.Vertical(*[rrb.TimeSeriesView(origin=f"/plots/{p}", name=p) for p in plots])),
        rrb.BlueprintPanel(state="collapsed"),
        rrb.SelectionPanel(state="collapsed"),
        rrb.TimePanel(state="expanded"),
    )

The mecanum robot in simulation

The simulated robot has four mecanum wheels, each with twelve passive rollers, at

\ell = 0.32

m and

w = 0.40

m. You command the chassis through four phases of three seconds each: forward, sideways, turning in place, and a combination of all three. At every frame, the wheel speeds come from

u = H(0)\,\mathcal{V}_b

with the commanded twist.

MEC_L, MEC_W = 0.32, 0.40


def build_mecanum():
    builder = newton.ModelBuilder()
    builder.default_joint_cfg.damping = 0.01
    chassis = builder.add_link(xform=wp.transform(p=wp.vec3(0.0, 0.0, 0.247), q=wp.quat_identity()), label="chassis")
    builder.add_shape_box(chassis, hx=0.42, hy=0.30, hz=0.06,
                          cfg=newton.ModelBuilder.ShapeConfig(density=80.0, mu=0.8, gap=0.01))
    joints, drives = [builder.add_joint_free(chassis)], []
    # Lynch and Park numbering: front-left, front-right, rear-right, rear-left
    for label, x, y, handedness in [("fl", MEC_L, MEC_W, -1.0), ("fr", MEC_L, -MEC_W, 1.0),
                                    ("rr", -MEC_L, -MEC_W, -1.0), ("rl", -MEC_L, MEC_W, 1.0)]:
        drive, rollers, radius = add_mecanum_wheel(builder, chassis, wp.vec3(x, y, -0.04), handedness, label)
        drives.append(drive)
        joints += [drive, *rollers]
    builder.add_articulation(joints)
    builder.add_ground_plane(cfg=newton.ModelBuilder.ShapeConfig(mu=1.5, gap=0.01))
    return builder.finalize(), chassis, drives, radius


phases = [(0.0, 0.5, 0.0), (0.0, 0.0, 0.5), (0.6, 0.0, 0.0), (0.3, 0.4, 0.2)]  # (omega_bz, v_bx, v_by)


def commanded_twist(t):
    phase = min(int(t // 3.0), len(phases) - 1)
    ramp = min((t - 3.0 * phase) / 0.5, 1.0)  # ease into each phase
    return ramp * np.array(phases[phase])


mec_model, mec_chassis, mec_drives, mec_radius = build_mecanum()
H_sim = H_mecanum(MEC_L, MEC_W, mec_radius)

def mecanum_frame(t, pose, angles):
    # log the commanded and measured twist and the chassis path so far
    for i, name in enumerate(["omega_bz", "v_bx", "v_by"]):
        rr.log(f"plots/{name}/commanded", rr.Scalars(commanded_twist(t[-1])[i]))
        if len(t) > 2:
            rr.log(f"plots/{name}/measured", rr.Scalars(measured_body_twist(t[-3:], pose[-3:])[-1, i]))
    rr.log("paths/chassis", rr.LineStrips3D([path_strip(pose, z=0.02)], colors=[[255, 170, 0]], radii=0.01))


t_mec, pose_mec, _ = simulate(mec_model, mec_chassis, mec_drives, lambda t: H_sim @ commanded_twist(t), duration=12.0,
                              recording=RECORDINGS / "mecanum-0.41.rrd", on_frame=mecanum_frame,
                              blueprint=layout("omega_bz", "v_bx", "v_by"))

V_meas = measured_body_twist(t_mec, pose_mec)
V_cmd = np.array([commanded_twist(t) for t in t_mec])
for i, (name, _) in enumerate(zip(["forward", "sideways", "rotate", "combined"], phases)):
    window = (t_mec > 3.0 * i + 1.5) & (t_mec < 3.0 * (i + 1) - 0.1)
    print(f"{name:9s} commanded {np.round(V_cmd[window].mean(0), 3)}  measured {np.round(V_meas[window].mean(0), 3)}")

forward   commanded [0.  0.5 0. ]  measured [-0.     0.498 -0.   ]
sideways  commanded [0.  0.  0.5]  measured [ 0.    -0.     0.494]
rotate    commanded [0.6 0.  0. ]  measured [ 0.595 -0.    -0.   ]
combined  commanded [0.3 0.4 0.2]  measured [0.297 0.396 0.193]

The measured twist follows the command in all four phases. This includes pure sideways motion, which a robot with conventional wheels cannot produce. Small differences arise from the rollers. They are separate rigid bodies that touch the floor one or two at a time. As each wheel turns, its effective rolling radius varies slightly.

The mecanum robot driven by $u = H(0)\mathcal{V}_b$ . Scrub the timeline to follow the four phases; the right-hand panels show the commanded and measured components of the body twist.

References

Kevin M. Lynch, F. (2017). Modern Robotics.
(n.d.). Data Curation for Machine Learning.

Found an error on this page? Report an issue.

Robotics Systems

Motion Algebras

Kinematics

Dynamics

Systems Integration

Omnidirectional Wheeled Robots

Omnidirectional robots

Simulating the robot

The mecanum robot in simulation

References

​Omnidirectional robots

​Simulating the robot

​The mecanum robot in simulation

​References

Omnidirectional robots

Simulating the robot

The mecanum robot in simulation

References