Files
chenlin c32df70326
Checks / Layout, translation and Colab checks (push) Has been cancelled
Checks / Ruff (push) Has been cancelled
Checks / Framework tests (native backend, CPU) (push) Has been cancelled
Checks / Portable Rust controller (push) Has been cancelled
release: v0.1 generic robot training and application platform
2026-11-05 11:05:01 +08:00

53 lines
2.4 KiB
Python

"""Body-frame twist to named wheel angular velocities."""
from __future__ import annotations
from collections.abc import Mapping
import numpy as np
from .model import finite, vector
WHEEL_AXES = ("lin_vel_x", "lin_vel_y", "ang_vel_z")
def wheel_mixer(config: Mapping) -> tuple[list[str], np.ndarray, float]:
speed = finite(config.get("max_wheel_speed", 20.0), "max_wheel_speed", positive=True)
if config["kind"] == "differential":
width = finite(config.get("track_width"), "track_width", positive=True)
radius = finite(config.get("wheel_radius"), "wheel_radius", positive=True)
names = [config.get("left_joint"), config.get("right_joint")]
directions = vector(config.get("directions", [1, 1]), 2, "directions")
matrix = np.array([[1, 0, -width / 2], [1, 0, width / 2]]) / radius
else:
wheels = config.get("wheels")
if not isinstance(wheels, list) or len(wheels) < 3:
raise ValueError("Omnidirectional control needs at least three wheels")
names, rows, signs = [], [], []
for wheel in wheels:
if not isinstance(wheel, Mapping):
raise ValueError("Each omni wheel must name its joint and geometry")
names.append(wheel.get("joint"))
x, y = vector(wheel.get("position"), 2, "wheel.position")
angle = finite(wheel.get("angle"), "wheel.angle")
radius = finite(wheel.get("radius"), "wheel.radius", positive=True)
dx, dy = np.cos(angle), np.sin(angle)
rows.append(np.array([dx, dy, x * dy - y * dx]) / radius)
signs.append(finite(wheel.get("direction", 1), "wheel.direction"))
matrix = np.stack(rows)
directions = np.array(signs)
if np.linalg.matrix_rank(matrix) != 3:
raise ValueError("Omni wheel geometry cannot produce a full planar twist")
if np.any(np.abs(directions) != 1):
raise ValueError("Wheel directions must be +1 or -1")
return names, directions[:, None] * matrix, speed
def mix_velocity(matrix: np.ndarray, twist: np.ndarray, limit: float) -> np.ndarray:
angular = matrix @ twist
if not np.all(np.isfinite(angular)):
raise ValueError("Wheel mix produced a non-finite angular velocity")
# Desaturate all wheels together so the resulting twist keeps its direction.
peak = np.max(np.abs(angular))
return angular * (limit / max(limit, peak))