83 lines
2.2 KiB
Python
83 lines
2.2 KiB
Python
"""Safety 保护层测试."""
|
||
import pytest
|
||
from go1_pro_sdk import (
|
||
LowCmd, MotorCmd, MotorMode,
|
||
apply_safety, position_limit, power_protect,
|
||
PowerProtectViolation,
|
||
TAU_MAX,
|
||
)
|
||
|
||
|
||
class _FakeMotor:
|
||
tauEst = 0.0
|
||
q = 0.0
|
||
|
||
|
||
class _FakeState:
|
||
def __init__(self):
|
||
self.motorState = [_FakeMotor() for _ in range(20)]
|
||
|
||
|
||
def test_position_limit_clamps_hip_over():
|
||
cmd = LowCmd()
|
||
cmd.motorCmd[0].q = 5.0 # FR_0 hip 超限
|
||
position_limit(cmd)
|
||
assert cmd.motorCmd[0].q == 0.78
|
||
|
||
|
||
def test_position_limit_clamps_thigh_under():
|
||
cmd = LowCmd()
|
||
cmd.motorCmd[1].q = -3.0
|
||
position_limit(cmd)
|
||
assert cmd.motorCmd[1].q == -0.60
|
||
|
||
|
||
def test_power_protect_clamps():
|
||
cmd = LowCmd()
|
||
for i in range(12):
|
||
cmd.motorCmd[i].tau = 10.0
|
||
state = _FakeState()
|
||
n = power_protect(cmd, state, factor=1)
|
||
# factor=1 → 限制到 TAU_MAX * 0.1
|
||
assert cmd.motorCmd[0].tau == TAU_MAX['hip'] * 0.1
|
||
assert cmd.motorCmd[2].tau == TAU_MAX['knee'] * 0.1
|
||
|
||
|
||
def test_power_protect_critical_command_raises():
|
||
cmd = LowCmd()
|
||
cmd.motorCmd[2].tau = 200.0 # > 5 × 35.55
|
||
with pytest.raises(PowerProtectViolation):
|
||
power_protect(cmd, _FakeState(), factor=5)
|
||
|
||
|
||
def test_power_protect_overload_actual_raises():
|
||
"""实测 tauEst 已超 max, 也应 raise."""
|
||
cmd = LowCmd()
|
||
cmd.motorCmd[2].tau = 1.0
|
||
state = _FakeState()
|
||
state.motorState[2].tauEst = 40.0 # > 35.55
|
||
with pytest.raises(PowerProtectViolation):
|
||
power_protect(cmd, state, factor=1)
|
||
|
||
|
||
def test_apply_safety_degraded_mode():
|
||
"""raise_on_critical=False 应当降级到全 damping."""
|
||
cmd = LowCmd()
|
||
cmd.motorCmd[2].tau = 200.0
|
||
cmd.motorCmd[2].Kp = 10
|
||
apply_safety(cmd, _FakeState(), power_factor=5,
|
||
position_limit_on=False, raise_on_critical=False)
|
||
assert cmd.motorCmd[0].tau == 0
|
||
assert cmd.motorCmd[0].Kp == 0
|
||
assert cmd.motorCmd[0].mode == 0
|
||
|
||
|
||
def test_apply_safety_normal_flow():
|
||
"""正常命令应能通过. 返回 LowCmd."""
|
||
cmd = LowCmd()
|
||
cmd.motorCmd[0].q = 0.5
|
||
cmd.motorCmd[0].tau = 1.0
|
||
result = apply_safety(cmd, _FakeState(), power_factor=5)
|
||
assert result is cmd
|
||
assert cmd.motorCmd[0].q == 0.5 # 在限位内, 不变
|