> ## Documentation Index > Fetch the complete documentation index at: https://aegean.ai/llms.txt > Use this file to discover all available pages before exploring further. # Omnidirectional Wheeled Robots > Wheel speeds from the body twist, checked against a mecanum robot in simulation. [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/pantelis/eng-ai-agents/blob/main/notebooks/kinematics/omnidirectional-robots/index.ipynb) An omnidirectional robot can move its chassis in any direction while rotating. The [wheeled robots overview](https://aegean.ai/aiml-common/lectures/kinematics/wheeled-robots/index.md) 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). ```python theme={null} 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$ at $(x_i, y_i)$ in $\{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)$. ```python theme={null} 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)) ``` ```output theme={null} 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. ```python theme={null} 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. ```python theme={null} 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) ``` ```python theme={null} 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)}") ``` ```output theme={null} 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] ``` ![Output from cell 6](https://mintcdn.com/aegeanaiinc/qs6gXJUlKPZbLhmC/aiml-common/lectures/kinematics/omnidirectional-robots/images/cell_6_output_1.png?fit=max\&auto=format\&n=qs6gXJUlKPZbLhmC\&q=85\&s=f624ea718fec2fc327b4342aa446ca1a) 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. [https://artifacts.aegeanai.com/rerun/viewer/0.41.0/index.html?url=https%3A%2F%2Fartifacts.aegeanai.com%2Frerun%2Fwheeled-robots%2Fmecanum-0.41.rrd](https://artifacts.aegeanai.com/rerun/viewer/0.41.0/index.html?url=https%3A%2F%2Fartifacts.aegeanai.com%2Frerun%2Fwheeled-robots%2Fmecanum-0.41.rrd) *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](https://github.com/aegean-ai/eaia-feedback/issues/new?template=page-issue.yml\&page=/aiml-common/lectures/kinematics/omnidirectional-robots). This documentation is built and hosted on [Mintlify](https://mintlify.com), a developer documentation platform.