From 8d8b171952736d50185b76c1a5e85e0af5acb0cc Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Mon, 27 Jul 2026 15:17:43 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=89=E5=85=A8=E7=AD=96=E7=95=A5=E7=A7=BB?= =?UTF-8?q?=E8=87=B3cpp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- fast_lowcmd_cpp/fast_lowcmd.py | 18 ++++++ fast_lowcmd_cpp/go1_fast_lowcmd.cpp | 99 +++++++++++++++++++++++++++++ fast_lowcmd_cpp/test_fast_lowcmd.py | 42 +++++++++++- 3 files changed, 158 insertions(+), 1 deletion(-) diff --git a/fast_lowcmd_cpp/fast_lowcmd.py b/fast_lowcmd_cpp/fast_lowcmd.py index 09974c1..3087c6b 100644 --- a/fast_lowcmd_cpp/fast_lowcmd.py +++ b/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) diff --git a/fast_lowcmd_cpp/go1_fast_lowcmd.cpp b/fast_lowcmd_cpp/go1_fast_lowcmd.cpp index 54f0299..8114ff1 100644 --- a/fast_lowcmd_cpp/go1_fast_lowcmd.cpp +++ b/fast_lowcmd_cpp/go1_fast_lowcmd.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -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 make_crc_table() { std::array 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(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(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(FastLowCmdBuilder_build_encrypted_servo12_checked), + METH_VARARGS | METH_KEYWORDS, + "Build an encrypted servo12 packet with native safety checks."}, {"build_plain_fields20", reinterpret_cast(FastLowCmdBuilder_build_plain_fields20), METH_VARARGS | METH_KEYWORDS, "Build a plain LowCmd packet from 20 motor field arrays."}, {"build_encrypted_fields20", reinterpret_cast(FastLowCmdBuilder_build_encrypted_fields20), diff --git a/fast_lowcmd_cpp/test_fast_lowcmd.py b/fast_lowcmd_cpp/test_fast_lowcmd.py index c939b0d..e4524fc 100644 --- a/fast_lowcmd_cpp/test_fast_lowcmd.py +++ b/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)]