安全策略移至cpp

fast_lowcmd_cpp/fast_lowcmd.py

@@ -6,6 +6,7 @@ the byte-equivalence and timing tests are stable on the robot board.
 from pathlib import Path
 
 from go1_fast_lowcmd import FastLowCmdBuilder as _NativeFastLowCmdBuilder
+from go1_pro_sdk import PowerProtectViolation
 
 
 def default_state_path() -> str:
@@ -33,6 +34,23 @@ class FastLowCmdBuilder:
     def build_encrypted_servo12(self, q, dq=None, tau=None, kp=None, kd=None):
         return self._native.build_encrypted_servo12(q, dq=dq, tau=tau, kp=kp, kd=kd)
 
+    def build_encrypted_servo12_checked(
+            self, q, kp, kd, actual_q=None, actual_tau=None, position_protect_limit=0.0):
+        try:
+            return self._native.build_encrypted_servo12_checked(
+                q,
+                kp,
+                kd,
+                actual_q=actual_q,
+                actual_tau=actual_tau,
+                position_protect_limit=position_protect_limit,
+            )
+        except RuntimeError as exc:
+            message = str(exc)
+            if message.startswith("power protect: "):
+                raise PowerProtectViolation(message[len("power protect: "):]) from exc
+            raise
+
     def build_plain_fields20(self, mode=None, q=None, dq=None, tau=None, kp=None, kd=None):
         return self._native.build_plain_fields20(mode=mode, q=q, dq=dq, tau=tau, kp=kp, kd=kd)
 

fast_lowcmd_cpp/go1_fast_lowcmd.cpp

@@ -2,6 +2,7 @@
 #include <Python.h>
 #include <array>
 #include <cmath>
+#include <cstdio>
 #include <cstdint>
 #include <cstring>
 #include <fstream>
@@ -16,6 +17,30 @@ constexpr Py_ssize_t MOTOR_SIZE = 27;
 constexpr int MOTOR_COUNT = 20;
 constexpr int SERVO_COUNT = 12;
 constexpr uint32_t CRC_POLY = 0x04c11db7u;
+constexpr double TAU_MAX_SERVO[SERVO_COUNT] = {
+    23.7, 23.7, 35.55,
+    23.7, 23.7, 35.55,
+    23.7, 23.7, 35.55,
+    23.7, 23.7, 35.55,
+};
+constexpr double JOINT_LIMIT_LO[SERVO_COUNT] = {
+    -0.78, -0.60, -2.70,
+    -0.78, -0.60, -2.70,
+    -0.78, -0.60, -2.70,
+    -0.78, -0.60, -2.70,
+};
+constexpr double JOINT_LIMIT_HI[SERVO_COUNT] = {
+    0.78, 3.50, -0.95,
+    0.78, 3.50, -0.95,
+    0.78, 3.50, -0.95,
+    0.78, 3.50, -0.95,
+};
+constexpr const char* JOINT_NAMES_SERVO[SERVO_COUNT] = {
+    "FR_0", "FR_1", "FR_2",
+    "FL_0", "FL_1", "FL_2",
+    "RR_0", "RR_1", "RR_2",
+    "RL_0", "RL_1", "RL_2",
+};
 
 std::array<uint32_t, 256> make_crc_table() {
     std::array<uint32_t, 256> table{};
@@ -569,6 +594,76 @@ PyObject* FastLowCmdBuilder_build_encrypted_servo12(FastLowCmdBuilder* self, PyO
     return result;
 }
 
+PyObject* FastLowCmdBuilder_build_encrypted_servo12_checked(
+        FastLowCmdBuilder* self, PyObject* args, PyObject* kwargs) {
+    PyObject* q_obj = nullptr;
+    PyObject* actual_q_obj = nullptr;
+    PyObject* actual_tau_obj = nullptr;
+    double kp_scalar = 0.0;
+    double kd_scalar = 0.0;
+    double position_protect_limit = 0.0;
+    static const char* kwlist[] = {
+        "q", "kp", "kd", "actual_q", "actual_tau", "position_protect_limit", nullptr};
+    if (!PyArg_ParseTupleAndKeywords(
+            args, kwargs, "Odd|OOd", const_cast<char**>(kwlist),
+            &q_obj, &kp_scalar, &kd_scalar,
+            &actual_q_obj, &actual_tau_obj, &position_protect_limit)) {
+        return nullptr;
+    }
+
+    double q[SERVO_COUNT], actual_q[SERVO_COUNT], actual_tau[SERVO_COUNT];
+    if (!read_double_sequence(q_obj, q, SERVO_COUNT, 0.0, "q")) return nullptr;
+
+    bool have_actual_q = actual_q_obj != nullptr && actual_q_obj != Py_None;
+    bool have_actual_tau = actual_tau_obj != nullptr && actual_tau_obj != Py_None;
+    if (have_actual_q && !read_double_sequence(actual_q_obj, actual_q, SERVO_COUNT, 0.0, "actual_q")) {
+        return nullptr;
+    }
+    if (have_actual_tau && !read_double_sequence(actual_tau_obj, actual_tau, SERVO_COUNT, 0.0, "actual_tau")) {
+        return nullptr;
+    }
+
+    if (have_actual_tau) {
+        for (int i = 0; i < SERVO_COUNT; ++i) {
+            if (std::fabs(actual_tau[i]) > TAU_MAX_SERVO[i]) {
+                char buf[192];
+                std::snprintf(
+                    buf,
+                    sizeof(buf),
+                    "power protect: 电机 %s 实测 tauEst=%.2f 已超 max %.2f, 立即停机!",
+                    JOINT_NAMES_SERVO[i], actual_tau[i], TAU_MAX_SERVO[i]);
+                PyObject* message = PyUnicode_FromString(buf);
+                if (message) {
+                    PyErr_SetObject(PyExc_RuntimeError, message);
+                    Py_DECREF(message);
+                }
+                return nullptr;
+            }
+        }
+    }
+
+    uint8_t cmd[LOWCMD_SIZE];
+    init_lowcmd_header(cmd);
+    for (int i = 0; i < SERVO_COUNT; ++i) {
+        double target = q[i];
+        if (target < JOINT_LIMIT_LO[i]) {
+            target = JOINT_LIMIT_LO[i];
+        } else if (target > JOINT_LIMIT_HI[i]) {
+            target = JOINT_LIMIT_HI[i];
+        }
+        double kp = kp_scalar;
+        double kd = kd_scalar;
+        if (have_actual_q && position_protect_limit > 0.0
+                && std::fabs(target - actual_q[i]) > position_protect_limit) {
+            kp = 0.0;
+            kd = 0.0;
+        }
+        encode_motor(cmd + MOTOR_OFFSET + i * MOTOR_SIZE, 0x0a, target, 0.0, 0.0, kp, kd);
+    }
+    finish_crc(cmd);
+    return encrypt_bytes(self, cmd, LOWCMD_SIZE);
+}
+
 PyObject* FastLowCmdBuilder_build_plain_fields20(FastLowCmdBuilder*, PyObject* args, PyObject* kwargs) {
     PyObject* mode_obj = nullptr;
     PyObject* q_obj = nullptr;
@@ -662,6 +757,10 @@ PyMethodDef FastLowCmdBuilder_methods[] = {
      METH_VARARGS | METH_KEYWORDS, "Build a plain LowCmd packet with first 12 motors in servo mode."},
     {"build_encrypted_servo12", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_encrypted_servo12),
      METH_VARARGS | METH_KEYWORDS, "Build an encrypted LowCmd packet with first 12 motors in servo mode."},
+    {"build_encrypted_servo12_checked",
+     reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_encrypted_servo12_checked),
+     METH_VARARGS | METH_KEYWORDS,
+     "Build an encrypted servo12 packet with native safety checks."},
     {"build_plain_fields20", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_plain_fields20),
      METH_VARARGS | METH_KEYWORDS, "Build a plain LowCmd packet from 20 motor field arrays."},
     {"build_encrypted_fields20", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_encrypted_fields20),

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)]