53 lines
2.4 KiB
Python
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))
|