安全策略移至cpp

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