add r1 robot project

2026-08-28 10:10:18 +02:00
parent 27aea1146d
commit 8d88495f6f
82 changed files with 22935 additions and 0 deletions
@@ -0,0 +1,233 @@
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <unistd.h>
+
+#include <unitree/robot/channel/channel_factory.hpp>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+
+namespace r1_vision {
+
+using unitree_hg::msg::dds_::LowCmd_;
+using unitree_hg::msg::dds_::LowState_;
+using namespace unitree::robot;
+
+inline constexpr int kNumMotors = 12;
+
+inline constexpr std::array<int, kNumMotors> kJointId = {
+    15, 16, 17, 18, 19,
+    22, 23, 24, 25, 26,
+    29, 30
+};
+
+inline constexpr std::array<const char*, kNumMotors> kJointName = {
+    "left_shoulder_pitch",
+    "left_shoulder_roll",
+    "left_shoulder_yaw",
+    "left_elbow",
+    "left_wrist_roll",
+    "right_shoulder_pitch",
+    "right_shoulder_roll",
+    "right_shoulder_yaw",
+    "right_elbow",
+    "right_wrist_roll",
+    "head_pitch",
+    "head_yaw"
+};
+
+inline constexpr std::array<float, kNumMotors> kKp = {
+    100.f, 100.f, 100.f, 100.f, 50.f,
+    100.f, 100.f, 100.f, 100.f, 50.f,
+    50.f, 10.f
+};
+
+inline constexpr std::array<float, kNumMotors> kKd = {
+    2.f, 2.f, 2.f, 2.f, 2.f,
+    2.f, 2.f, 2.f, 2.f, 2.f,
+    2.f, 0.1f
+};
+
+inline uint32_t Crc32Core(uint32_t* ptr, uint32_t len) {
+    uint32_t xbit = 0;
+    uint32_t data = 0;
+    uint32_t crc = 0xFFFFFFFFu;
+    constexpr uint32_t polynomial = 0x04c11db7u;
+
+    for (uint32_t i = 0; i < len; ++i) {
+        xbit = 1u << 31;
+        data = ptr[i];
+        for (uint32_t bits = 0; bits < 32; ++bits) {
+            if (crc & 0x80000000u) {
+                crc <<= 1;
+                crc ^= polynomial;
+            } else {
+                crc <<= 1;
+            }
+            if (data & xbit) crc ^= polynomial;
+            xbit >>= 1;
+        }
+    }
+    return crc;
+}
+
+class RobotController {
+public:
+    explicit RobotController(const std::string& network_interface) {
+        ChannelFactory::Instance()->Init(0, network_interface);
+
+        motion_switcher_ = std::make_shared<unitree::robot::b2::MotionSwitcherClient>();
+        motion_switcher_->SetTimeout(5.0f);
+        motion_switcher_->Init();
+
+        std::string form, name;
+        while (true) {
+            motion_switcher_->CheckMode(form, name);
+            if (name.empty()) break;
+            std::cout << "[SYS] Releasing motion mode: " << name << std::endl;
+            if (motion_switcher_->ReleaseMode()) {
+                std::cerr << "[WARN] ReleaseMode failed" << std::endl;
+            }
+            ::sleep(2);
+        }
+
+        lowcmd_ = std::make_shared<ChannelPublisher<LowCmd_>>("rt/lowcmd");
+        lowcmd_->InitChannel();
+
+        lowstate_ = std::make_shared<ChannelSubscriber<LowState_>>("rt/lowstate");
+        lowstate_->InitChannel(
+            std::bind(&RobotController::StateHandler, this, std::placeholders::_1), 1);
+
+        std::cout << "[SYS] Waiting for LowState..." << std::endl;
+        while (!state_received_.load()) ::usleep(10000);
+
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            target_q_ = current_q_;
+        }
+
+        running_.store(true);
+        writer_thread_ = std::thread(&RobotController::CommandLoop, this);
+        std::cout << "[SYS] Low-level controller running at 500 Hz." << std::endl;
+    }
+
+    ~RobotController() {
+        HoldCurrent();
+        running_.store(false);
+        if (writer_thread_.joinable()) writer_thread_.join();
+    }
+
+    void StateHandler(const void* message) {
+        const auto& state = *static_cast<const LowState_*>(message);
+        const uint32_t crc = Crc32Core(
+            reinterpret_cast<uint32_t*>(const_cast<LowState_*>(&state)),
+            (sizeof(LowState_) >> 2) - 1);
+        if (state.crc() != crc) return;
+
+        std::lock_guard<std::mutex> lock(mutex_);
+        for (int i = 0; i < kNumMotors; ++i) {
+            current_q_[i] = state.motor_state().at(kJointId[i]).q();
+        }
+        mode_machine_ = state.mode_machine();
+        state_received_.store(true);
+    }
+
+    void CommandLoop() {
+        auto next = std::chrono::steady_clock::now();
+        while (running_.load()) {
+            next += std::chrono::milliseconds(2);
+
+            LowCmd_ cmd;
+            {
+                std::lock_guard<std::mutex> lock(mutex_);
+                cmd.mode_pr() = 0;
+                cmd.mode_machine() = mode_machine_;
+                for (int i = 0; i < kNumMotors; ++i) {
+                    auto& motor = cmd.motor_cmd().at(kJointId[i]);
+                    motor.mode() = 1;
+                    motor.q() = target_q_[i];
+                    motor.dq() = 0.0f;
+                    motor.kp() = kKp[i];
+                    motor.kd() = kKd[i];
+                    motor.tau() = 0.0f;
+                }
+            }
+
+            cmd.crc() = Crc32Core(
+                reinterpret_cast<uint32_t*>(&cmd),
+                (sizeof(cmd) >> 2) - 1);
+            lowcmd_->Write(cmd);
+            std::this_thread::sleep_until(next);
+        }
+    }
+
+    std::array<float, kNumMotors> CurrentQ() const {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return current_q_;
+    }
+
+    float CurrentJoint(int joint) const {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return current_q_.at(joint);
+    }
+
+    void SetTargets(const std::array<float, kNumMotors>& q) {
+        std::lock_guard<std::mutex> lock(mutex_);
+        target_q_ = q;
+    }
+
+    void SetJointTarget(int joint, float q) {
+        std::lock_guard<std::mutex> lock(mutex_);
+        target_q_.at(joint) = q;
+    }
+
+    void MoveJointRelative(int joint, float delta_rad, double seconds = 0.6) {
+        const float start = CurrentJoint(joint);
+        const float target = start + delta_rad;
+        const auto t0 = std::chrono::steady_clock::now();
+
+        while (true) {
+            const double elapsed = std::chrono::duration<double>(
+                std::chrono::steady_clock::now() - t0).count();
+            const double a = std::clamp(elapsed / seconds, 0.0, 1.0);
+            const double s = a * a * (3.0 - 2.0 * a);
+            SetJointTarget(joint, static_cast<float>(start + delta_rad * s));
+            if (a >= 1.0) break;
+            std::this_thread::sleep_for(std::chrono::milliseconds(5));
+        }
+
+        SetJointTarget(joint, target);
+        std::this_thread::sleep_for(std::chrono::milliseconds(300));
+    }
+
+    void HoldCurrent() {
+        SetTargets(CurrentQ());
+        std::this_thread::sleep_for(std::chrono::milliseconds(250));
+    }
+
+private:
+    std::shared_ptr<unitree::robot::b2::MotionSwitcherClient> motion_switcher_;
+    ChannelPublisherPtr<LowCmd_> lowcmd_;
+    ChannelSubscriberPtr<LowState_> lowstate_;
+    std::thread writer_thread_;
+    std::atomic<bool> running_{false};
+    std::atomic<bool> state_received_{false};
+    mutable std::mutex mutex_;
+    std::array<float, kNumMotors> current_q_{};
+    std::array<float, kNumMotors> target_q_{};
+    uint8_t mode_machine_{0};
+};
+
+} // namespace r1_vision