cpp对齐官方

examples/cpp/example_official_compatible_position.cpp

@@ -0,0 +1,60 @@
+#include "unitree_legged_sdk/unitree_legged_sdk.h"
+
+#include <chrono>
+#include <cmath>
+#include <iostream>
+#include <thread>
+
+using namespace UNITREE_LEGGED_SDK;
+
+int main() {
+  constexpr float dt = 0.002f;
+  UDP udp(LOWLEVEL, 8090, UDP_SERVER_IP_BASIC, UDP_SERVER_PORT);
+  Safety safety(LeggedType::Go1);
+  LowCmd cmd{};
+  LowState state{};
+  udp.InitCmdData(cmd);
+
+  bool have_state = false;
+  for (int attempt = 0; attempt < 1000 && !have_state; ++attempt) {
+    udp.SetSend(cmd);
+    udp.Send();
+    if (udp.Recv() > 0) {
+      udp.GetRecv(state);
+      have_state = state.head[0] == 0xfe && state.head[1] == 0xef;
+    }
+    std::this_thread::sleep_for(std::chrono::duration<float>(dt));
+  }
+  if (!have_state) {
+    std::cerr << "No LowState received; active position command was not enabled.\n";
+    return 1;
+  }
+
+  const float initial = state.motorState[FR_1].q;
+  for (int step = 0; step < 2500; ++step) {
+    udp.Recv();
+    udp.GetRecv(state);
+    const float phase = static_cast<float>(step) * dt;
+    cmd.motorCmd[FR_1].q = initial + 0.15f * std::sin(phase * 2.0f);
+    cmd.motorCmd[FR_1].dq = 0.0f;
+    cmd.motorCmd[FR_1].Kp = 5.0f;
+    cmd.motorCmd[FR_1].Kd = 1.0f;
+    cmd.motorCmd[FR_1].tau = 0.0f;
+    safety.PositionLimit(cmd);
+    if (safety.PowerProtect(cmd, state, 1) < 0) {
+      udp.InitCmdData(cmd);
+      udp.SetSend(cmd);
+      udp.Send();
+      return 2;
+    }
+    safety.PositionProtect(cmd, state, 0.5);
+    udp.SetSend(cmd);
+    udp.Send();
+    std::this_thread::sleep_for(std::chrono::duration<float>(dt));
+  }
+
+  udp.InitCmdData(cmd);
+  udp.SetSend(cmd);
+  udp.Send();
+  return 0;
+}

examples/example_official_compatible_position.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+"""Low-level example that runs with either SDK's ``robot_interface`` module."""
+
+import time
+
+import robot_interface as sdk
+
+
+# Unitree's official Python wrapper does not export these C++ constants.
+LOWLEVEL = 0xff
+FR_1 = 1
+
+
+def control_step(udp, safe, cmd, state, target_q=1.2):
+    """Execute one control iteration using only the official Python API."""
+    udp.Recv()
+    udp.GetRecv(state)
+
+    motor = cmd.motorCmd[FR_1]
+    motor.q = target_q
+    motor.dq = 0.0
+    motor.Kp = 5.0
+    motor.Kd = 1.0
+    motor.tau = 0.0
+
+    safe.PositionLimit(cmd)
+    if safe.PowerProtect(cmd, state, 1) < 0:
+        raise RuntimeError("PowerProtect rejected the command")
+
+    udp.SetSend(cmd)
+    udp.Send()
+
+
+def initialize_transport(udp, cmd, state, frames=50, dt=0.01):
+    """Send only stop sentinels until the PRO MCU can return initial state."""
+    udp.SetSend(cmd)
+    received = False
+    for _ in range(frames):
+        udp.Send()
+        time.sleep(dt)
+        if udp.Recv() > 0:
+            received = True
+        udp.GetRecv(state)
+    if not received:
+        raise TimeoutError("No LowState received during initialization")
+
+
+def main():
+    udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007)
+    safe = sdk.Safety(sdk.LeggedType.Go1)
+    cmd = sdk.LowCmd()
+    state = sdk.LowState()
+    udp.InitCmdData(cmd)
+    initialize_transport(udp, cmd, state)
+
+    while True:
+        control_step(udp, safe, cmd, state)
+        time.sleep(0.002)
+
+
+if __name__ == "__main__":
+    main()