安全策略移至cpp

fast_lowcmd_cpp/test_fast_lowcmd.py

@@ -3,7 +3,8 @@ import os
 import pytest
 
 from fast_lowcmd import FastLowCmdBuilder, default_state_path
-from go1_pro_sdk import Blowfish, LowCmd, MotorCmd, MotorMode
+from go1_pro_sdk import Blowfish, LowCmd, MotorCmd, MotorMode, PowerProtectViolation
+from go1_pro_sdk import apply_safety
 from go1_pro_sdk.codec.lowcmd_builder import build_low_cmd_encrypted, build_low_cmd_plain
 
 
@@ -56,6 +57,45 @@ def test_servo12_encrypted_matches_python(builder, bf):
     assert builder.build_encrypted_servo12(q, kp=28.0, kd=0.7) == build_low_cmd_encrypted(cmd, bf)
 
 
+def test_servo12_checked_matches_python_safety(builder, bf):
+    q = [-1.0, 0.8, -1.5, 0.1, 0.8, -1.5, -0.1, 1.0, -1.5, 0.1, 1.0, -1.5]
+    actual_q = [0.0] * 12
+    actual_tau = [0.0] * 12
+
+    class Motor:
+        def __init__(self, q_value, tau_value):
+            self.q = q_value
+            self.tauEst = tau_value
+
+    class State:
+        pass
+
+    state = State()
+    state.motorState = [Motor(actual_q[i], actual_tau[i]) for i in range(12)]
+
+    cmd = LowCmd()
+    for i in range(12):
+        cmd.set_motor(i, MotorCmd(mode=MotorMode.Servo, q=q[i], Kp=36.0, Kd=1.0))
+    apply_safety(cmd, state, power_factor=9, position_limit_on=True, position_protect_limit=0.5)
+
+    assert builder.build_encrypted_servo12_checked(
+        q,
+        kp=36.0,
+        kd=1.0,
+        actual_q=actual_q,
+        actual_tau=actual_tau,
+        position_protect_limit=0.5,
+    ) == build_low_cmd_encrypted(cmd, bf)
+
+
+def test_servo12_checked_power_protect(builder):
+    q = [-0.1, 0.8, -1.5, 0.1, 0.8, -1.5, -0.1, 1.0, -1.5, 0.1, 1.0, -1.5]
+    actual_tau = [0.0] * 12
+    actual_tau[4] = -24.02
+    with pytest.raises(PowerProtectViolation):
+        builder.build_encrypted_servo12_checked(q, kp=36.0, kd=1.0, actual_tau=actual_tau)
+
+
 def test_fields20_plain_matches_python(builder):
     mode = [int(MotorMode.Servo) if i < 12 else int(MotorMode.Damping) for i in range(20)]
     q = [0.05 * i for i in range(20)]