Files
UniversalTrain-ai/tests/test_actuator_terms.py
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

171 lines
6.6 KiB
Python

"""`actuator_power` and `actuator_headroom`: the two ways they can be wrong quietly.
Both terms multiply a torque by something that belongs to a joint, and both
produce a perfectly plausible number when the something belongs to the **wrong**
joint. A power computed from a mismatched pair is still watts; a headroom ratio
computed against another joint's speed is still a fraction. Nothing downstream
can tell, and the policy is trained to minimise a quantity nobody named.
So these tests run the real functions against a stub entity, which is what makes
the pairing testable at all: in a real environment the robot's actuator and joint
lists are identical and in the same order, so index-pairing and name-pairing agree
and neither test would fail against the broken version.
"""
from __future__ import annotations
import math
import pytest
torch = pytest.importorskip("torch", reason="the terms are torch code")
from mjlab.managers.scene_entity_config import SceneEntityCfg # noqa: E402
from mjrl.tasks.actuator import ServoCurve
from mjrl.tasks.mdp import (
actuator_headroom,
actuator_power,
)
CURVE = ServoCurve(2.0, 1.5, 2.0, 12.0, 1.4, 0.3)
class _Data:
def __init__(self, force, vel):
self.actuator_force = force
self.joint_vel = vel
class _Entity:
"""Just enough entity for the two terms: names, forces and joint speeds."""
def __init__(self, joint_names, actuator_names, force, vel):
self.joint_names = tuple(joint_names)
self.actuator_names = tuple(actuator_names)
self.data = _Data(force, vel)
class _Env:
def __init__(self, entity):
self.scene = {"robot": entity}
def _cfg(entity, actuator_names=None) -> SceneEntityCfg:
"""A resolved `SceneEntityCfg` without a scene to resolve against.
`resolve` needs a real entity, so the ids are filled in by hand -- which is
also what keeps each test's config object distinct, since the terms cache
their name lookup by `id(asset_cfg)`.
"""
cfg = SceneEntityCfg("robot", actuator_names=actuator_names)
if actuator_names is None:
cfg.actuator_ids = slice(None)
else:
order = {n: i for i, n in enumerate(entity.actuator_names)}
cfg.actuator_ids = [order[n] for n in actuator_names]
return cfg
def test_torque_is_paired_with_its_own_joints_speed() -> None:
"""The pairing is by name, and pairing by index would be silent.
The stub's two lists are **deliberately in different orders**, which is the
arrangement that can tell name pairing and index pairing apart.
"""
entity = _Entity(
joint_names=("a", "b"),
actuator_names=("b", "a"), # reversed
force=torch.tensor([[2.0, 0.0]]), # actuator "b" pushes 2 N*m, "a" none
vel=torch.tensor([[10.0, 1.0]]), # joint "a" spins at 10, "b" at 1
)
env = _Env(entity)
# Correct: 2 N*m on actuator "b" x joint "b"'s 1 rad/s = 2 W, over 2 actuators.
got = actuator_power(env, _cfg(entity))
assert got.item() == pytest.approx(1.0)
# The control: index pairing would have used joint "a"'s 10 rad/s and read
# 20 W, i.e. ten times the truth.
naive = (entity.data.actuator_force * entity.data.joint_vel).abs().mean()
assert naive.item() == pytest.approx(10.0)
def test_power_is_a_magnitude_so_braking_is_not_paid_for() -> None:
"""Signed power would pay a policy for falling into its joint limits."""
entity = _Entity(("a",), ("a",), torch.tensor([[-1.5]]), torch.tensor([[2.0]]))
assert actuator_power(_Env(entity), _cfg(entity)).item() == pytest.approx(3.0)
def test_an_actuator_with_no_joint_of_its_name_raises() -> None:
"""Rather than pairing it with whatever is at that index."""
entity = _Entity(("a",), ("gearbox",), torch.zeros(1, 1), torch.zeros(1, 1))
with pytest.raises(KeyError, match="drive no joint"):
actuator_power(_Env(entity), _cfg(entity))
@pytest.mark.parametrize(
"fraction, expected",
[(0.0, 0.0), (0.5, 0.0), (0.687, 0.0), (1.0, 1.0)],
)
def test_headroom_is_zero_below_the_deadband_and_one_at_the_limit(
fraction: float, expected: float
) -> None:
"""The term's whole shape, at a speed where the curve is at its plateau.
`fraction` is of what the servo can produce at that speed, so 1.0 is an
actuator pinned against the clamp -- which is the state the term is named
for.
"""
deadband = CURVE.continuous_torque / CURVE.plateau_torque
slow = 0.5 * CURVE.corner_speed # on the plateau, so the limit is the plateau
entity = _Entity(
("a",), ("a",),
torch.tensor([[fraction * CURVE.plateau_torque]]),
torch.tensor([[slow]]),
)
got = actuator_headroom(_Env(entity), deadband, _cfg(entity), CURVE)
assert got.item() == pytest.approx(expected, abs=2e-3)
def test_headroom_measures_against_the_speed_the_joint_is_actually_at() -> None:
"""The same torque is more of the capability when the joint is moving fast.
This is the property that makes the term worth its complexity: dividing by the
flat `EFFORT_LIMIT` would score these two identically, and they are not the
same situation -- the fast one has no torque left to reject a disturbance.
"""
tau = 0.9 * CURVE.continuous_torque
fast = CURVE.corner_speed + 2.0 * CURVE.decay_speed # well past the corner
slow = 0.0
deadband = CURVE.continuous_torque / CURVE.plateau_torque
costs = []
for speed in (slow, fast):
entity = _Entity(("a",), ("a",), torch.tensor([[tau]]), torch.tensor([[speed]]))
costs.append(actuator_headroom(_Env(entity), deadband, _cfg(entity), CURVE).item())
assert costs[0] == pytest.approx(0.0), "below the deadband while slow"
assert costs[1] > 0.5, (
"the same torque past the corner speed is most of what the servo can "
f"still produce, and the term did not see it: {costs[1]:.3f}"
)
def test_past_the_cutoff_speed_the_ratio_stays_finite() -> None:
"""The curve is exactly zero up there, and a division by it is not a big
number but a meaningless one. The clamp keeps the gradient finite."""
entity = _Entity(
("a",), ("a",),
torch.tensor([[0.1]]),
torch.tensor([[CURVE.cutoff_speed * 1.5]]),
)
got = actuator_headroom(_Env(entity), 0.687, _cfg(entity), CURVE).item()
# Bounded by 1 by construction (the ratio is clamped there), and measured at
# 1.0000002 in float32 -- the division and the square each round up. Worth a
# tolerance rather than a tighter clamp: two parts in ten million on a reward
# term is not a number anything can notice, and chasing it would add an
# operation to every step of every environment.
assert math.isfinite(got) and 0.0 <= got <= 1.0 + 1e-5