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,91 @@
+#pragma once
+
+#include <array>
+#include <string>
+
+#include <Eigen/Dense>
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/jacobian.hpp>
+#include <pinocchio/algorithm/kinematics.hpp>
+#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/multibody/model.hpp>
+
+namespace r1_vision {
+
+inline Eigen::VectorXd MakePinocchioQ(
+    const pinocchio::Model& model,
+    const std::array<float, 12>& robot_q)
+{
+    Eigen::VectorXd q = pinocchio::neutral(model);
+
+    static const std::array<const char*, 12> names = {
+        "left_shoulder_pitch_joint",
+        "left_shoulder_roll_joint",
+        "left_shoulder_yaw_joint",
+        "left_elbow_joint",
+        "left_wrist_roll_joint",
+        "right_shoulder_pitch_joint",
+        "right_shoulder_roll_joint",
+        "right_shoulder_yaw_joint",
+        "right_elbow_joint",
+        "right_wrist_roll_joint",
+        "head_pitch_joint",
+        "head_yaw_joint"
+    };
+
+    for (int i = 0; i < 12; ++i) {
+        const int jid = model.getJointId(names[i]);
+        if (jid == 0) {
+            continue;
+        }
+        q[model.joints[jid].idx_q()] = robot_q[i];
+    }
+
+    return q;
+}
+
+inline Eigen::Vector3d FramePosition(
+    pinocchio::Model& model,
+    pinocchio::Data& data,
+    const std::array<float, 12>& robot_q,
+    const std::string& frame_name)
+{
+    const Eigen::VectorXd q = MakePinocchioQ(model, robot_q);
+    const int frame_id = model.getFrameId(frame_name);
+    if (frame_id >= static_cast<int>(model.frames.size())) {
+        throw std::runtime_error("Pinocchio frame not found: " + frame_name);
+    }
+
+    pinocchio::forwardKinematics(model, data, q);
+    pinocchio::updateFramePlacements(model, data);
+    return data.oMf[frame_id].translation();
+}
+
+inline Eigen::MatrixXd FrameTranslationJacobian(
+    pinocchio::Model& model,
+    pinocchio::Data& data,
+    const std::array<float, 12>& robot_q,
+    const std::string& frame_name)
+{
+    const Eigen::VectorXd q = MakePinocchioQ(model, robot_q);
+    const int frame_id = model.getFrameId(frame_name);
+    if (frame_id >= static_cast<int>(model.frames.size())) {
+        throw std::runtime_error("Pinocchio frame not found: " + frame_name);
+    }
+
+    pinocchio::forwardKinematics(model, data, q);
+    pinocchio::updateFramePlacements(model, data);
+
+    pinocchio::Data::Matrix6x J6(6, model.nv);
+    pinocchio::computeFrameJacobian(
+        model,
+        data,
+        q,
+        frame_id,
+        pinocchio::LOCAL_WORLD_ALIGNED,
+        J6);
+
+    return J6.topRows(3);
+}
+
+} // namespace r1_vision
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <string>
+
+#include "r1_voice/llm.hh"
+#include "r1_voice/stt.hh"
+#include "r1_voice/tts.hh"
+
+namespace r1_voice {
+
+struct ConversationConfig {
+    SttConfig stt;
+    LlmConfig llm;
+    TtsConfig tts;
+};
+
+class Conversation {
+public:
+    explicit Conversation(
+        ConversationConfig config
+    );
+
+    bool init(
+        const std::string& network_interface
+    );
+
+    void run();
+
+private:
+    ConversationConfig config_;
+
+    SpeechToText stt_;
+    LLM llm_;
+    TextToSpeech tts_;
+};
+
+} // namespace r1_voice
@@ -0,0 +1,57 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+namespace r1_voice {
+
+struct ChatMessage {
+    std::string role;
+    std::string content;
+};
+
+struct LlmConfig {
+    std::string url =
+        "http://127.0.0.1:8080/v1/chat/completions";
+
+    std::string model = "local";
+
+    float temperature = 0.7f;
+
+    int max_tokens = 256;
+
+    std::string system_prompt =
+        "You are R1, a humanoid robot conversational assistant. "
+        "Talk naturally with the person in front of you. "
+        "Keep answers reasonably short because they will be spoken aloud. "
+        "Answer in the same language as the user. "
+        "Do not use markdown unless necessary.";
+};
+
+class LLM {
+public:
+    explicit LLM(LlmConfig config);
+
+    std::string chat(
+        const std::string& user
+    );
+
+private:
+    LlmConfig config_;
+
+    std::vector<ChatMessage> history_;
+
+    std::string escapeJson(
+        const std::string& text
+    ) const;
+
+    std::string parseContent(
+        const std::string& json
+    ) const;
+
+    std::string httpPost(
+        const std::string& body
+    ) const;
+};
+
+} // namespace r1_voice
@@ -0,0 +1,49 @@
+#pragma once
+
+#include <string>
+
+namespace r1_voice {
+
+    struct SttConfig {
+        std::string whisper_cli;
+        std::string whisper_model;
+
+        std::string language = "cs";
+
+        int threads = 8;
+
+        static constexpr int sample_rate = 16000;
+        static constexpr int packet_bytes = 5120;
+
+        float speech_threshold = 0.010f;
+
+        int silence_ms = 900;
+        int min_speech_ms = 250;
+        int max_record_ms = 10000;
+
+        int preroll_ms = 800;
+    };
+
+class SpeechToText {
+public:
+    explicit SpeechToText(SttConfig config);
+
+    // Blocks until the user speaks, stops speaking,
+    // and Whisper has transcribed the utterance.
+    std::string listen();
+
+private:
+    SttConfig config_;
+
+    bool recordToWav(const std::string& path);
+
+    std::string transcribe(
+        const std::string& wav_path
+    );
+
+    std::string trim(
+        const std::string& text
+    );
+};
+
+} // namespace r1_voice
@@ -0,0 +1,65 @@
+#pragma once
+
+#include <cstdint>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <unitree/robot/r1/audio/audio_client.hpp>
+
+namespace r1_voice {
+
+struct TtsConfig {
+    std::string piper_command = "python3 -m piper";
+
+    std::string model =
+        "/home/jvizner/esarobotech/piper/cs_medium.onnx";
+
+    std::string output_wav =
+        "/tmp/r1_tts.wav";
+
+    int volume = 100;
+
+    std::string app_name = "r1_voice";
+};
+
+class TextToSpeech {
+public:
+    explicit TextToSpeech(TtsConfig config);
+
+    bool init(
+        const std::string& network_interface
+    );
+
+    bool speak(
+        const std::string& text
+    );
+
+private:
+    TtsConfig config_;
+
+    std::unique_ptr<
+        unitree::robot::r1::AudioClient
+    > client_;
+
+    bool initialized_ = false;
+
+    bool synthesize(
+        const std::string& text,
+        const std::string& wav_path
+    );
+
+    bool readWavPcm(
+        const std::string& wav_path,
+        std::vector<uint8_t>& pcm,
+        uint32_t& sample_rate,
+        uint16_t& channels,
+        uint16_t& bits_per_sample
+    );
+
+    bool playPcm(
+        const std::vector<uint8_t>& pcm
+    );
+};
+
+} // namespace r1_voice
@@ -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
@@ -0,0 +1,115 @@
+#pragma once
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <thread>
+#include <vector>
+#include <unistd.h>
+
+namespace r1_vision {
+
+struct Detection {
+    bool valid = false;
+    std::string class_name;
+    double timestamp = 0.0;
+    double confidence = 0.0;
+    double u = 0.0;
+    double v = 0.0;
+};
+
+inline bool ExtractNumber(const std::string& json, const std::string& key, double& value) {
+    const std::string search = "\"" + key + "\"";
+    const size_t p = json.find(search);
+    if (p == std::string::npos) return false;
+    const size_t colon = json.find(':', p);
+    if (colon == std::string::npos) return false;
+    try {
+        value = std::stod(json.substr(colon + 1));
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+inline bool ExtractString(const std::string& json, const std::string& key, std::string& value) {
+    const std::string search = "\"" + key + "\"";
+    const size_t p = json.find(search);
+    if (p == std::string::npos) return false;
+    const size_t colon = json.find(':', p);
+    if (colon == std::string::npos) return false;
+    const size_t q1 = json.find('"', colon + 1);
+    if (q1 == std::string::npos) return false;
+    const size_t q2 = json.find('"', q1 + 1);
+    if (q2 == std::string::npos) return false;
+    value = json.substr(q1 + 1, q2 - q1 - 1);
+    return true;
+}
+
+inline bool ReadDetection(const std::string& path, Detection& d) {
+    std::ifstream f(path);
+    if (!f) return false;
+    std::stringstream buffer;
+    buffer << f.rdbuf();
+    const std::string json = buffer.str();
+    if (json.empty()) return false;
+
+    if (!ExtractNumber(json, "timestamp", d.timestamp)) return false;
+    if (!ExtractNumber(json, "confidence", d.confidence)) return false;
+    if (!ExtractNumber(json, "u", d.u)) return false;
+    if (!ExtractNumber(json, "v", d.v)) return false;
+    ExtractString(json, "class", d.class_name);
+    d.valid = true;
+    return true;
+}
+
+inline bool IsFresh(const Detection& d, double max_age_sec = 0.5) {
+    const double now = std::chrono::duration<double>(
+        std::chrono::system_clock::now().time_since_epoch()).count();
+    return std::isfinite(d.timestamp) && std::abs(now - d.timestamp) <= max_age_sec;
+}
+
+inline bool GetStableBottleCenter(
+    const std::string& path,
+    double& u,
+    double& v,
+    int samples = 8,
+    double min_confidence = 0.7,
+    double timeout_sec = 5.0)
+{
+    std::vector<double> us;
+    std::vector<double> vs;
+    const auto start = std::chrono::steady_clock::now();
+
+    while (static_cast<int>(us.size()) < samples) {
+        Detection d;
+        if (ReadDetection(path, d) && d.valid &&
+            (d.class_name.empty() || d.class_name == "bottle") &&
+            d.confidence >= min_confidence && IsFresh(d)) {
+            us.push_back(d.u);
+            vs.push_back(d.v);
+        }
+
+        const double elapsed = std::chrono::duration<double>(
+            std::chrono::steady_clock::now() - start).count();
+        if (elapsed > timeout_sec) return false;
+        std::this_thread::sleep_for(std::chrono::milliseconds(25));
+    }
+
+    std::sort(us.begin(), us.end());
+    std::sort(vs.begin(), vs.end());
+    const auto median = [](const std::vector<double>& x) {
+        const size_t n = x.size();
+        if (n & 1u) return x[n / 2];
+        return 0.5 * (x[n / 2 - 1] + x[n / 2]);
+    };
+
+    u = median(us);
+    v = median(vs);
+    return true;
+}
+
+} // namespace r1_vision