From 3b1eac30b90b6af6cd5dc9d64c14bc5191d92b00 Mon Sep 17 00:00:00 2001 From: Zhang Zhen Date: Tue, 21 Apr 2026 10:03:55 +0800 Subject: [PATCH 1/2] add keyboard sim ctrl --- KEYBOARD_CONTROL.md | 136 ++++++++++++++++++++ simulate/config.yaml | 12 +- simulate/src/keyboard_joystick.h | 113 +++++++++++++++++ simulate/src/main.cc | 19 ++- simulate/src/unitree_sdk2_bridge.h | 18 +++ simulate_python/unitree_mujoco.py | 166 ++++++++++++------------- terrain_tool/gen_g1_rough.py | 47 +++++++ terrain_tool/scene_g1.xml | 23 ++++ unitree_robots/g1/scene_rough.xml | 22 ++++ unitree_robots/go2w/assets/terrain.stl | Bin 38084 -> 38084 bytes 10 files changed, 464 insertions(+), 92 deletions(-) create mode 100644 KEYBOARD_CONTROL.md create mode 100644 simulate/src/keyboard_joystick.h mode change 100755 => 100644 simulate/src/main.cc create mode 100644 terrain_tool/gen_g1_rough.py create mode 100644 terrain_tool/scene_g1.xml create mode 100644 unitree_robots/g1/scene_rough.xml diff --git a/KEYBOARD_CONTROL.md b/KEYBOARD_CONTROL.md new file mode 100644 index 00000000..8e69d790 --- /dev/null +++ b/KEYBOARD_CONTROL.md @@ -0,0 +1,136 @@ +# 键盘控制使用说明 + +## 概述 + +已为 unitree_mujoco 添加键盘控制功能，无需手柄即可控制机器人。 + +## 启用键盘控制 + +编辑 `simulate/config.yaml` 文件： + +```yaml +use_joystick: 1 # 启用控制器 +joystick_type: "keyboard" # 使用键盘模式（原来是 "xbox" 或 "switch"） +``` + +## 键盘映射 + +### 基础按键对照表 + +| 键盘按键 | 手柄按键 | 说明 | +|---------|---------|------| +| **左Ctrl** | LB (L1) | 左肩键 | +| **左Shift** | LT (L2) | 左触发器 | +| **右Ctrl** | RB (R1) | 右肩键 | +| **右Shift** | RT (R2) | 右触发器 | +| **A** | A | A 按钮 | +| **B** | B | B 按钮 | +| **X** | X | X 按钮 | +| **Y** | Y | Y 按钮 | +| **1** | F1 | 功能键1 | +| **2** | F2 | 功能键2 | +| **Tab** | Start | 开始键 | +| **Esc** | Select | 选择键 | +| **↑/←/↓/→** | D-pad ↑/←/↓/→ | 方向键 | + +### 摇杆控制 + +| 键盘按键 | 摇杆 | 说明 | +|---------|------|------| +| **I** | 左摇杆 ly +1.0 | 前进 | +| **K** | 左摇杆 ly -1.0 | 后退 | +| **J** | 左摇杆 lx -1.0 | 左移 | +| **L** | 左摇杆 lx +1.0 | 右移 | +| **U** | 右摇杆 rx -1.0 | 左转 | +| **O** | 右摇杆 rx +1.0 | 右转 | + +### 组合键示例（对应 IOSDK 遥控器操作） + +键盘支持同时按多个键，模拟遥控器组合键操作： + +| 键盘组合 | 手柄组合 | UserCommand | 功能 | +|---------|---------|-------------|------| +| **左Ctrl + A** | L1 + A | L1_A | - | +| **左Ctrl + B** | L1 + B | L1_B | charleston_dance | +| **左Ctrl + X** | L1 + X | L1_X | - | +| **左Ctrl + Y** | L1 + Y | L1_Y | - | +| **左Shift + A** | L2 + A | L2_A | - | +| **左Shift + B** | L2 + B | L2_B | CMCC_GPC_dance1 | +| **左Shift + X** | L2 + X | L2_X | - | +| **左Shift + Y** | L2 + Y | L2_Y | CMCC_GPC_dance2 | +| **右Ctrl + A** | R1 + A | R1_A | dancekgswing | +| **右Ctrl + B** | R1 + B | R1_B | dancedzht | +| **右Ctrl + X** | R1 + X | R1_X | dancekaraoke | +| **右Ctrl + Y** | R1 + Y | R1_Y | danceydd | +| **右Shift + A** | R2 + A | R2_A | 行走模式 | +| **右Shift + B** | R2 + B | R2_B | dancebcgm | +| **右Shift + X** | R2 + X | R2_X | dancepower | +| **右Shift + Y** | R2 + Y | R2_Y | dancemojito | +| **左Shift + 1** | L2 + F1 | L2_F1 | 紧急停止(PASSIVE) | +| **1 + A** | F1 + A | F1_A | signal_debug | + +### 弹性带控制（Elastic Band，用于吊起机器人） +需要在 `config.yaml` 中设置 `enable_elastic_band: 1` + +- **9**: 启用/禁用弹性带 +- **7**: 缩短弹性带（吊起机器人） +- **8**: 延长弹性带（释放机器人） + +### 其他 +- **Backspace**: 重置仿真 + +### 宏按键 + +| 按键 | 等效按下 | 说明 | +|------|----------|------| +| **3** | **L2 + D-pad Up** | 同时触发 L2 与 D-pad 上 | +| **4** | **R2 + A** | 同时触发 R2 与 A | + +## 使用示例 + +1. 修改配置文件： +```bash +cd ~/Documents/Code/unitree/unitree_mujoco/simulate +vim config.yaml +``` + +2. 修改以下行： +```yaml +use_joystick: 1 +joystick_type: "keyboard" +``` + +3. 编译并运行： +```bash +cd build +make -j4 +./unitree_mujoco +``` + +4. 使用键盘控制机器人移动 + +## 技术实现 + +### 新增文件 +- `simulate/src/keyboard_joystick.h`: 键盘控制类实现 + +### 修改文件 +- `simulate/src/unitree_sdk2_bridge.h`: 添加键盘支持和 `setGLFWWindow()` 方法 +- `simulate/src/main.cc`: 添加全局 bridge 指针和窗口设置逻辑 + +### 核心类 +```cpp +class KeyboardJoystick : public unitree::common::UnitreeJoystick +``` + +该类继承自 `UnitreeJoystick`，通过 GLFW 键盘事件模拟手柄输入。 + +## 注意事项 + +1. 键盘控制是数字输入（0 或 1），不像手柄有模拟量 +2. 对角线移动会自动归一化，保持速度一致 +3. 可以同时按多个键实现组合控制 +4. 键盘控制与原有手柄控制互不冲突，可通过配置文件切换 + +## 作者 +Zhang Zhen (zhangzhen@cmhi.chinamobile.com) diff --git a/simulate/config.yaml b/simulate/config.yaml index d2131bcc..8511b699 100644 --- a/simulate/config.yaml +++ b/simulate/config.yaml @@ -1,14 +1,14 @@ -robot: "go2" # Robot name, "go2", "b2", "b2w", "h1", "go2w", "g1" +robot: "g1" # Robot name, "go2", "b2", "b2w", "h1", "go2w", "g1" robot_scene: "scene.xml" # Robot scene, /unitree_robots/[robot]/scene.xml domain_id: 1 # Domain id interface: "lo" # Interface -use_joystick: 0 # Simulate Unitree WirelessController using a gamepad -joystick_type: "xbox" # support "xbox" and "switch" gamepad layout -joystick_device: "/dev/input/js0" # Device path -joystick_bits: 16 # Some game controllers may only have 8-bit accuracy +use_joystick: 1 # Simulate Unitree WirelessController using a gamepad +joystick_type: "keyboard" # support "xbox", "switch", and "keyboard" +joystick_device: "/dev/input/js0" # Device path (not used in keyboard mode) +joystick_bits: 16 # Some game controllers may only have 8-bit accuracy (not used in keyboard mode) print_scene_information: 1 # Print link, joint and sensors information of robot -enable_elastic_band: 0 # Virtual spring band, used for lifting h1 +enable_elastic_band: 1 # Virtual spring band, used for lifting h1 diff --git a/simulate/src/keyboard_joystick.h b/simulate/src/keyboard_joystick.h new file mode 100644 index 00000000..3ea80d3a --- /dev/null +++ b/simulate/src/keyboard_joystick.h @@ -0,0 +1,113 @@ +#pragma once + +#include +#include +#include +#include +#include + +/** + * @brief Keyboard-based joystick emulation for unitree_mujoco + * @author Zhang Zhen (zhangzhen@cmhi.chinamobile.com) + * + * Keyboard mapping: + * - IJKL: Left stick (lx, ly) - movement control + * - U/O: Right stick rx (left/right turn) + * - A/B/X/Y: A/B/X/Y buttons + * - LCtrl/LShift: L1/L2 + * - RCtrl/RShift: R1/R2 + * - Arrow keys: D-pad (up/left/down/right) + * - 1: F1 | 2: F2 + * - Tab: Start | Esc: Select + */ +class KeyboardJoystick : public unitree::common::UnitreeJoystick +{ +public: + KeyboardJoystick(GLFWwindow* window) + : unitree::common::UnitreeJoystick(), window_(window) + { + if (!window_) { + std::cout << "Error: GLFW window is null." << std::endl; + exit(1); + } + std::cout << "[KeyboardJoystick] Initialized. Use keyboard to control:" << std::endl; + std::cout << " IJKL: Move (left stick)" << std::endl; + std::cout << " U/O: Turn left/right (right stick rx)" << std::endl; + std::cout << " A/B/X/Y: A/B/X/Y buttons" << std::endl; + std::cout << " LCtrl/LShift: L1/L2 | RCtrl/RShift: R1/R2" << std::endl; + std::cout << " Arrow keys: D-pad (up/left/down/right)" << std::endl; + std::cout << " 1: F1 | 2: F2 | Tab: Start | Esc: Select" << std::endl; + std::cout << " 3: Lock Stand (L2+Up) | 4: Walk (R2+A)" << std::endl; + } + + void update() override + { + const bool macro_l2_up = (glfwGetKey(window_, GLFW_KEY_3) == GLFW_PRESS); + const bool macro_r2_a = (glfwGetKey(window_, GLFW_KEY_4) == GLFW_PRESS); + + // ABXY buttons (A/B/X/Y) + A((glfwGetKey(window_, GLFW_KEY_A) == GLFW_PRESS) || macro_r2_a); + B(glfwGetKey(window_, GLFW_KEY_B) == GLFW_PRESS); + X(glfwGetKey(window_, GLFW_KEY_X) == GLFW_PRESS); + Y(glfwGetKey(window_, GLFW_KEY_Y) == GLFW_PRESS); + + // Shoulder buttons: L1=LCtrl, L2=LShift, R1=RCtrl, R2=RShift + LB(glfwGetKey(window_, GLFW_KEY_LEFT_CONTROL) == GLFW_PRESS); // L1 + LT((glfwGetKey(window_, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS) || macro_l2_up); // L2 + RB(glfwGetKey(window_, GLFW_KEY_RIGHT_CONTROL) == GLFW_PRESS); // R1 + RT((glfwGetKey(window_, GLFW_KEY_RIGHT_SHIFT) == GLFW_PRESS) || macro_r2_a); // R2 + + // D-pad (Arrow keys) + up((glfwGetKey(window_, GLFW_KEY_UP) == GLFW_PRESS) || macro_l2_up); + left(glfwGetKey(window_, GLFW_KEY_LEFT) == GLFW_PRESS); + down(glfwGetKey(window_, GLFW_KEY_DOWN) == GLFW_PRESS); + right(glfwGetKey(window_, GLFW_KEY_RIGHT) == GLFW_PRESS); + + // Function keys: F1 = 1, F2 = 2 + F1(glfwGetKey(window_, GLFW_KEY_1) == GLFW_PRESS); + F2(glfwGetKey(window_, GLFW_KEY_2) == GLFW_PRESS); + + // Other + back(glfwGetKey(window_, GLFW_KEY_ESCAPE) == GLFW_PRESS); + start(glfwGetKey(window_, GLFW_KEY_TAB) == GLFW_PRESS); + + // Left stick (IJKL for movement) + double lx_val = 0.0; + double ly_val = 0.0; + + if (glfwGetKey(window_, GLFW_KEY_J) == GLFW_PRESS) lx_val -= 1.0; + if (glfwGetKey(window_, GLFW_KEY_L) == GLFW_PRESS) lx_val += 1.0; + if (glfwGetKey(window_, GLFW_KEY_I) == GLFW_PRESS) ly_val += 1.0; + if (glfwGetKey(window_, GLFW_KEY_K) == GLFW_PRESS) ly_val -= 1.0; + + // Normalize diagonal movement + if (lx_val != 0.0 && ly_val != 0.0) { + double norm = std::sqrt(lx_val * lx_val + ly_val * ly_val); + lx_val /= norm; + ly_val /= norm; + } + + lx(lx_val); + ly(ly_val); + + // Right stick (Arrow keys for rotation/camera) + double rx_val = 0.0; + double ry_val = 0.0; + + if (glfwGetKey(window_, GLFW_KEY_U) == GLFW_PRESS) rx_val -= 1.0; + if (glfwGetKey(window_, GLFW_KEY_O) == GLFW_PRESS) rx_val += 1.0; + + // Normalize diagonal movement + if (rx_val != 0.0 && ry_val != 0.0) { + double norm = std::sqrt(rx_val * rx_val + ry_val * ry_val); + rx_val /= norm; + ry_val /= norm; + } + + rx(rx_val); + ry(ry_val); + } + +private: + GLFWwindow* window_; +}; diff --git a/simulate/src/main.cc b/simulate/src/main.cc old mode 100755 new mode 100644 index 3368d916..e340976a --- a/simulate/src/main.cc +++ b/simulate/src/main.cc @@ -99,6 +99,9 @@ namespace // model and data mjModel *m = nullptr; mjData *d = nullptr; + + // global bridge pointer for keyboard joystick setup + UnitreeSDK2BridgeBase* g_bridge = nullptr; // control noise variables mjtNum *ctrlnoise = nullptr; @@ -599,6 +602,7 @@ void *UnitreeSdk2BridgeThread(void *arg) } else { interface = std::make_unique(m, d); } + g_bridge = interface.get(); // Save pointer for keyboard joystick setup interface->start(); while (true) @@ -625,9 +629,9 @@ void user_key_cb(GLFWwindow* window, int key, int scancode, int act, int mods) { if(param::config.enable_elastic_band == 1) { if (key==GLFW_KEY_9) { elastic_band.enable_ = !elastic_band.enable_; - } else if (key==GLFW_KEY_7 || key==GLFW_KEY_UP) { + } else if (key==GLFW_KEY_7) { elastic_band.length_ -= 0.1; - } else if (key==GLFW_KEY_8 || key==GLFW_KEY_DOWN) { + } else if (key==GLFW_KEY_8) { elastic_band.length_ += 0.1; } } @@ -666,6 +670,7 @@ int main(int argc, char **argv) mjvOption opt; mjv_defaultOption(&opt); + opt.flags[mjVIS_CONTACTPOINT] = 1; mjvPerturb pert; mjv_defaultPerturb(&pert); @@ -687,8 +692,16 @@ int main(int argc, char **argv) // start physics thread std::thread physicsthreadhandle(&PhysicsThread, sim.get(), param::config.robot_scene.c_str()); + + // Wait for bridge initialization and set GLFW window for keyboard joystick + while (!g_bridge) { + usleep(100000); // Wait 100ms + } + GLFWwindow* window = static_cast(sim->platform_ui.get())->window_; + g_bridge->setGLFWWindow(window); + // start simulation UI loop (blocking call) - glfwSetKeyCallback(static_cast(sim->platform_ui.get())->window_,user_key_cb); + glfwSetKeyCallback(window, user_key_cb); sim->RenderLoop(); physicsthreadhandle.join(); diff --git a/simulate/src/unitree_sdk2_bridge.h b/simulate/src/unitree_sdk2_bridge.h index 7c13289a..d8a19fcc 100644 --- a/simulate/src/unitree_sdk2_bridge.h +++ b/simulate/src/unitree_sdk2_bridge.h @@ -13,6 +13,7 @@ #include "param.h" #include "physics_joystick.h" +#include "keyboard_joystick.h" #define MOTOR_SENSOR_NUM 3 @@ -31,6 +32,9 @@ class UnitreeSDK2BridgeBase joystick = std::make_shared(param::config.joystick_device, param::config.joystick_bits); } else if(param::config.joystick_type == "switch") { joystick = std::make_shared(param::config.joystick_device, param::config.joystick_bits); + } else if(param::config.joystick_type == "keyboard") { + // Keyboard joystick will be initialized later via setGLFWWindow() + std::cout << "[Info] Keyboard joystick mode enabled. Waiting for GLFW window..." << std::endl; } else { std::cerr << "Unsupported joystick type: " << param::config.joystick_type << std::endl; exit(EXIT_FAILURE); @@ -38,6 +42,13 @@ class UnitreeSDK2BridgeBase } } + + // Set GLFW window for keyboard joystick + virtual void setGLFWWindow(GLFWwindow* window) { + if(param::config.use_joystick == 1 && param::config.joystick_type == "keyboard") { + joystick = std::make_shared(window); + } + } virtual void start() {} @@ -165,6 +176,13 @@ using WirelessController_t = unitree::robot::go2::publisher::WirelessController; wireless_controller->joystick = joystick; } + void setGLFWWindow(GLFWwindow* window) override { + UnitreeSDK2BridgeBase::setGLFWWindow(window); + // Sync joystick pointer to lowstate and wireless_controller + if(lowstate) lowstate->joystick = joystick; + if(wireless_controller) wireless_controller->joystick = joystick; + } + void start() { thread_ = std::make_shared( diff --git a/simulate_python/unitree_mujoco.py b/simulate_python/unitree_mujoco.py index 95b934fe..be9a1902 100755 --- a/simulate_python/unitree_mujoco.py +++ b/simulate_python/unitree_mujoco.py @@ -1,83 +1,83 @@ -import time -import mujoco -import mujoco.viewer -from threading import Thread -import threading - -from unitree_sdk2py.core.channel import ChannelFactoryInitialize -from unitree_sdk2py_bridge import UnitreeSdk2Bridge, ElasticBand - -import config - - -locker = threading.Lock() - -mj_model = mujoco.MjModel.from_xml_path(config.ROBOT_SCENE) -mj_data = mujoco.MjData(mj_model) - - -if config.ENABLE_ELASTIC_BAND: - elastic_band = ElasticBand() - if config.ROBOT == "h1" or config.ROBOT == "g1": - band_attached_link = mj_model.body("torso_link").id - else: - band_attached_link = mj_model.body("base_link").id - viewer = mujoco.viewer.launch_passive( - mj_model, mj_data, key_callback=elastic_band.MujuocoKeyCallback - ) -else: - viewer = mujoco.viewer.launch_passive(mj_model, mj_data) - -mj_model.opt.timestep = config.SIMULATE_DT -num_motor_ = mj_model.nu -dim_motor_sensor_ = 3 * num_motor_ - -time.sleep(0.2) - - -def SimulationThread(): - global mj_data, mj_model - - ChannelFactoryInitialize(config.DOMAIN_ID, config.INTERFACE) - unitree = UnitreeSdk2Bridge(mj_model, mj_data) - - if config.USE_JOYSTICK: - unitree.SetupJoystick(device_id=0, js_type=config.JOYSTICK_TYPE) - if config.PRINT_SCENE_INFORMATION: - unitree.PrintSceneInformation() - - while viewer.is_running(): - step_start = time.perf_counter() - - locker.acquire() - - if config.ENABLE_ELASTIC_BAND: - if elastic_band.enable: - mj_data.xfrc_applied[band_attached_link, :3] = elastic_band.Advance( - mj_data.qpos[:3], mj_data.qvel[:3] - ) - mujoco.mj_step(mj_model, mj_data) - - locker.release() - - time_until_next_step = mj_model.opt.timestep - ( - time.perf_counter() - step_start - ) - if time_until_next_step > 0: - time.sleep(time_until_next_step) - - -def PhysicsViewerThread(): - while viewer.is_running(): - locker.acquire() - viewer.sync() - locker.release() - time.sleep(config.VIEWER_DT) - - -if __name__ == "__main__": - viewer_thread = Thread(target=PhysicsViewerThread) - sim_thread = Thread(target=SimulationThread) - - viewer_thread.start() - sim_thread.start() +import time +import mujoco +import mujoco.viewer +from threading import Thread +import threading + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py_bridge import UnitreeSdk2Bridge, ElasticBand + +import config + + +locker = threading.Lock() + +mj_model = mujoco.MjModel.from_xml_path(config.ROBOT_SCENE) +mj_data = mujoco.MjData(mj_model) + + +if config.ENABLE_ELASTIC_BAND: + elastic_band = ElasticBand() + if config.ROBOT == "h1" or config.ROBOT == "g1": + band_attached_link = mj_model.body("torso_link").id + else: + band_attached_link = mj_model.body("base_link").id + viewer = mujoco.viewer.launch_passive( + mj_model, mj_data, key_callback=elastic_band.MujuocoKeyCallback + ) +else: + viewer = mujoco.viewer.launch_passive(mj_model, mj_data) + +mj_model.opt.timestep = config.SIMULATE_DT +num_motor_ = mj_model.nu +dim_motor_sensor_ = 3 * num_motor_ + +time.sleep(0.2) + + +def SimulationThread(): + global mj_data, mj_model + + ChannelFactoryInitialize(config.DOMAIN_ID, config.INTERFACE) + unitree = UnitreeSdk2Bridge(mj_model, mj_data) + + if config.USE_JOYSTICK: + unitree.SetupJoystick(device_id=0, js_type=config.JOYSTICK_TYPE) + if config.PRINT_SCENE_INFORMATION: + unitree.PrintSceneInformation() + + while viewer.is_running(): + step_start = time.perf_counter() + + locker.acquire() + + if config.ENABLE_ELASTIC_BAND: + if elastic_band.enable: + mj_data.xfrc_applied[band_attached_link, :3] = elastic_band.Advance( + mj_data.qpos[:3], mj_data.qvel[:3] + ) + mujoco.mj_step(mj_model, mj_data) + + locker.release() + + time_until_next_step = mj_model.opt.timestep - ( + time.perf_counter() - step_start + ) + if time_until_next_step > 0: + time.sleep(time_until_next_step) + + +def PhysicsViewerThread(): + while viewer.is_running(): + locker.acquire() + viewer.sync() + locker.release() + time.sleep(config.VIEWER_DT) + + +if __name__ == "__main__": + viewer_thread = Thread(target=PhysicsViewerThread) + sim_thread = Thread(target=SimulationThread) + + viewer_thread.start() + sim_thread.start() diff --git a/terrain_tool/gen_g1_rough.py b/terrain_tool/gen_g1_rough.py new file mode 100644 index 00000000..3f3167a6 --- /dev/null +++ b/terrain_tool/gen_g1_rough.py @@ -0,0 +1,47 @@ +""" +Generate a large rough ground scene for G1 robot. +Output: ../unitree_robots/g1/scene_rough.xml + +Usage: + cd terrain_tool/ + python3 gen_g1_rough.py + +Author: Zhang Zhen (zhangzhen@cmhi.chinamobile.com) +""" + +import xml.etree.ElementTree as xml_et +import numpy as np +import sys +import os + +# Reuse TerrainGenerator from terrain_generator.py +sys.path.insert(0, os.path.dirname(__file__)) + +# Override global config before importing +import terrain_generator as tg_mod +tg_mod.ROBOT = "g1" +tg_mod.INPUT_SCENE_PATH = os.path.join(os.path.dirname(__file__), "scene_g1.xml") +tg_mod.OUTPUT_SCENE_PATH = os.path.join(os.path.dirname(__file__), "../unitree_robots/g1/scene_rough.xml") + +from terrain_generator import TerrainGenerator + +if __name__ == "__main__": + tg = TerrainGenerator() + + # Large rough ground centered around the robot spawn point + # Total area ~10m x 8m, robot spawns near origin + tg.AddRoughGround( + init_pos=[-5.0, -4.0, 0.0], # offset so terrain surrounds origin + euler=[0, 0, 0.0], + nums=[20, 16], # 20x16 blocks -> ~10m x 8m + box_size=[0.5, 0.5, 0.08], # thin blocks, height 8cm + box_euler=[0.0, 0.0, 0.0], + separation=[0.5, 0.5], # spacing matches box size for full coverage + box_size_rand=[0.05, 0.05, 0.06], # random height variation up to ±6cm + box_euler_rand=[0.08, 0.08, 0.0], # slight random tilt + separation_rand=[0.02, 0.02], + ) + + tg.Save() + print(f"Saved to: {tg_mod.OUTPUT_SCENE_PATH}") + print("Run simulator with: ./unitree_mujoco -s scene_rough.xml") diff --git a/terrain_tool/scene_g1.xml b/terrain_tool/scene_g1.xml new file mode 100644 index 00000000..838de41a --- /dev/null +++ b/terrain_tool/scene_g1.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/unitree_robots/g1/scene_rough.xml b/unitree_robots/g1/scene_rough.xml new file mode 100644 index 00000000..140d3c8b --- /dev/null +++ b/unitree_robots/g1/scene_rough.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/unitree_robots/go2w/assets/terrain.stl b/unitree_robots/go2w/assets/terrain.stl index d0ec042bd09087ada645c33ab4189bf4db8d8f80..c6e29dbc06be0a324d2da625f36a2a3e24003340 100644 GIT binary patch literal 38084 zcmbuI2b5G*(#J1($cPA%k(?w=&oCfN+cV=5L{UW46_KQO1t22Zl}P(9!&V|NLIkm@_YJYdLrG#AIXqUSpQOdmUgQ!E*>|Bw#UjL?3s$qguNWXW_9}ZbpfU}CB2!fkCjX7 z_6_}Pw-EDll029RW~d`^8%V>uY5iBpcoead&ntZ|PSU*O<39 zmkn!N3B5_SqmhU}FRe#e6^*W5%JZ3I<;KP1rd$3x5$hY2N!b}ipcm>HL+4z2N!1Bo zh@xx!hwjujUC^@q{+jKtI<}1%%R8e}oF-V}7V;i%nz?d01NqN^Dd`J2& z#!WKKL@&e|WM9d5HCTuL_`OJ=m!lEM%4^RFWd(6vyD4Ziwe5&YxEe@s%hR`f)RHu8 zJ0coQGn4#nvr5vj3WbOu+qT1MNKc6&&)U~ zO0@&xCK*pwX-9+X%W^)f2DAf+^5EsRDJCn3>uQvx^WkWejcx}J=;hk1!Trih$4Pog zgeXhrk_qtZ6}Uam$=RuI?G2!(#iaOf*!+cQYl z2VLuYeZ*$MUJhZi>Q4Qt{J|#S>%$TpH_2pXm6P*91limUM}yBtEP-B*hC}$Fx4Us& zjeVf8FWe6IigF280|{<9^;x<;!dXRf-3g0rGN)sp5u;q&2W=R`EXjRy-+Xue7JF4jWH38%QD~P zt2NTE;$*^JIxCt1MCQ~OH^~giswC|og6#8jJ{%2WoH;cT=;dfQ1Y`wqU5(_3#-E|& znDY^v345VlRKJR7B-8nLD66uxg9x(EQ9G=L>Q_LZm!lCUD~Ri87<1O=(3e9$(fm=L zkJwDu%OR|W>Q`Y6j+uwjhz8l*4o3s}6%ggY%h7NMJRfdcS7WTtp??pJiEc-n zOxVjItcD)19F4KQB>&@C)ubJyLH1c{hob@Qh-hdpN5dhY9T3;mFviiKQ$e#OeLi9{ zVK0ZU8uwL9O|BeXd12gmvokv*W1>nN!kJUCm!n~`qC2O|UO7L!hdXyTxnIHEPVTiX z0kcV*k-0X{uU7P(BE(S24s&pt@_;A%GJ ze6Kgd$0|P;$0*$nH(R&mwjIt$J%;XTpck>ZGT~@YJ92RpRT>e3$3%6k;@)vc55|0B zKHIfR+Ck^VgZDnA5g|A$=M36*#JfAw9WJ8H_d)Ne`JyqTVk)%*2z5T(yIqt9%u6)w zM3OlJYA6EEIcP^6tMHYhXMHqtibUf1RhK!(oOV#Zf_b89hci>-tkkhmGi1!ljTA+| zOkZ8|kw|tH4t*C1m6bc!HfDL>OW}6p({=8yYof1n=UQ>IqW+bl2%?>p7t(TcQ+Ooh zUV<}vXI6}B13N?J*b#zqcUHXa`V3d8ypD=T*pV|H%><1Mp0I}B8DHhT6u!$)vNY~m zI@*^6}21X(YJCZaeI3Lhj)-;y8C?j3n2&WOcq*!xX#=qgVW_SPADJ4lQ#Z z8m}BZV|MM@B^ul#;}Mc!+?I<*c$O-2LmF|MnI1+mW=~k-1=ff|*sKyBD;hJLnXp4& z3AuA$J5TH~XPC5uu0;>dgdIXM{4ecrRzG?Lfsy1o7Y!P(s2%YL&I;S1dLoS(cBQCS zTNo)yBQ8N1Dh)mlZojgtYFW!hXk4Y*(T6LjelL&h&UL2=&Jf$NqVFXkhSMD<-c>8K zB2>$1G_bW7s(8>C<uaMs-d6R#- zJuR2WB8T9(#(1`#k(&NMs?aCPlLjjF(%I*spEgX8)G$t{}FOY2v6F;|@ z97CX&qhYg39$Bxb^FPOWHox23i*ct9w;CtftoBVKn>D^A+n5$am}L)nc102Bg?h$} zPpe#%+2<}#-|2UHG47~pR^vd!0^dloS>toEjd_NMRgd5AxhaZ3FVr)p>EcVl(PuI} z!S45aG4AYjo#a?m=w0ZWMK)`cCEF7HYiGp}=!JU5Jo512;7?<_c&h$=gcsvx-e)yh zF5B$OAe%LskZnvaA{un<;Heo!pcm>Hv%XwGuzaazo=2`38&C9N+(S#O#`e#4_zsfI8e7P=#K}pq1bU&K zF|W-W6g)KkT=If{PxfNm6J4#w`nkJ($H``m0NIvUH6)fmFVr(;-jN!?iH~kgKHTFe zFUCE0po1K%wGZs^{Y*A%d`Pw>wl9b!&1uO*u`^5mQ|k@GiS41r##XUxf&gVLuItXlgb^(&0q^4oS>|3hOt_uUp`vxe=v zOcbAtCD04?q_?M6{Z~pZUmqB^)-zUvufdvRvxdC}>557}^nNUXUZ^KmZTimqCAoZ^ zW8A{VR)fcqjbyV%QyEJZl`BZUxa@`8$D#=ILOo+XeE4wsy#Bj#L)%7pG4A-{>*ZJ_ z(KtAcY}P0(V=U1_q!NF zpcm>z;;FaouZT zZV}-7Ajb9YY%9lV7T;^hW{n)V*V28^`W)|9(W8CTAwUJ}>!2Y?uX=Jm;V6rXY94jQy3-zetMQ8A@%c}=k@eBasj(X8* zjQb|XHzhS3YurG# zC00`3jV90w^<>VF{`TpexiV*fxS3zJmSdH%Zi+91Y}V*Xwj~zRH5g5x7wQ?alSYO` z!xrQ|$a4mWd+KSc@%8G*d?(3fjRRyGbCihUZA)^8MG@$Qdd7S+Z+`l1U3=wz%X0>Z zd#JwEC^hU6-$AljPaNP~&PJz@#;LOo+XrIGqc8rxsuIRnH!@kuK=R*n8W+;^O8 z)~HFgy&If7womQ`NT3($8S@L>ak9%FTk{pq86fVt0al|^$`IerWV1$Vvh7{Xxkn*^ zUZ`ixb9CpNoVU8uMV>Q2+@a@M%HF6O?!VtRglyL6M7F&<-}?RYa(6}oy--i)40$D- zIRnIfW46`kMsb&r?P~Bk<@D`ovckhm&C}8-VTolklP~d{0peC^Wi|L%RVJG? zTFJ4ZmKSzBvM`xjjs$w49`)TXY6Q3S{v`Puo-;t)+S^*lv1&-?`C77B!=7^{0^?!{ z^g=!9CBgUaIhibT28i2osMX-U+k$M?uzi>248dx{VhQv@J?dBU<_9+}uHgBa=L`_H zRtc-|4PAFN$!3j%at$)EbzUriUZ^M6d2sYSjXZpvW8A`}&E;5S()h8FY~FKc#}XPD zf*-YwCD04?Xx&LGmWH=<@YJOh3&tJa!)m0{I5>`M(U38*ClR}kdOWqF2=qcdy;j4x z3w~-Q?clL}KH02c$97)bIcqf}&6FF8qr=CSN({@gceHRJzLOok&s9C#1xX$3l&AhM4*0Z$6 zX)bjJtI=5223(owM0EyRnQ(~=A~+NFLcQoZgB$nMmBylRQ0fegh5lUX47C0TjvHRr z!|M;s@5J8+#GD_%SQOg7VY z?aag?sxw3r=!JU5{6uwzFR9M(C(jum?$B{oW0lkytj02_0Wh(K>I~7u5F$7e_Ch_L zGc3(ZapnvV_l?R{gKHT}$Y$EsG7=H!g?e@-Q@LhNxX$3lt+J|-9IKC{&d^9S-jg~5 z%`&>5r#geoG9Z)6L~thTg?e@l_bAmFcn*hgYu{uw3g`@8OE%MM<($*`NWbyvV{7<) zAc0<}C)KF*r)Vz9^(&0q^4o^e4(_`x$Yu@Ocd6o){>O<}0=-br&X8xlIZ)=*7`N6l zR^yP=8LWn_Gi;zab#I^*7$GC-!EzM)eMzWckQBS?L@fF;AVnesApG@Luk!us|Ie|DRr&Q_3z1Kv&L6(>I^P{UZ`hR z!quqGV5sAca;&(HpG`JvOpnwV9^Vm5pcm@d6@Ar6oxzQ3{Pnl~O!p72{D-ow zMgwQg0CfhJKrhsbt~0oC{T^#?kUE3a$d6NJaEWXpxV7wsdeLPb6z4mXW# zrtKUKYciM#dZAu)oxzPe>P2ghl{$mf7!{|^;1VN=;7r&H^>p9GxU8C(LrP|sEk?tFG{xN6|WJ@H8$ zIaYP0&QM1*s>i7_xWsWHI1~0lJzF*Cy|Kg^u7zXVa|5hKd#N*6x_P9|Fm+?e7~*Fl zScAP#&)#bvufHZ-HE`n&Jy%=W(OK#YwS{gMsWYS%t&Smv5WyPkg?iC-1~={-v#p&b zbq1H_Dg)LTTw)0kjt1&Q*BRWnRa#lQ9kskN*-ST+W2Ng1E`eUC7hPv?I}6+!;U31 zX9(`6XOPjvMj|*9_Ch`TWc2*9F7jj)f-GGA8n~)F-lI3G_le8Fzyd z>6tKBYccMEpK3}wcx<0fHfz|io!4sp%H1N*gpoim)Qhe&xN)bvX6+$TXRsOrcn?^Qk*g2f8Gq?nLp`NWXTurqlTW4_Nj;dyDe$F+LY}VKr$=H?oM#yX5MFQUJqoD&2)oEonbCrgSO7#66l3`vi=Ar^{gu+1I9gdP6QX+_+U% zT_ttGDt$mZ#;C(CClFb_Sob!H$?T^P2=!JTA zF8ccqzlZlTxN%#4TTR-*eYXYKtYP~u^{e#qUtf$N&kMw(TF+RUufdvRvxdC} z`Pp6f@0`knOQ09(8N<61-dySIPH^KEHnukJVc1AEYc!OxgmyZmFQ#XZb_aw@pcm>z z?`Lr1jxVk%$BOH%Fg?iEZ8Qi!F##o!j_W5M9#%D6N>-`Ka zfnKN=U1xCPPN{2cu53>xn}65fXq-BOOQ09(8B>RLC-kOg^ao^jf*aSr)9$&PA$113 z&vI&q8ySn!AM^6&BB%n}I|$rLXt z8{ngRNA}GgoJS*}BAH=jsteJzeRmJe?vYTD%(HXr3DJLZJK9t2t__e-k<64ZF_;Kk ze;}bEnY>b0i^jT1=^k94A)z9f*(1vfQMI_Q2Um?qs7U7LS|K^Qzj=pwa0Q8kie$b| z{XvMYKA!Bsl`s-25?+ITXJ5E61JiK-w)6MAUpx%Z8MKzWTOD^}Yub9o61cY->BJhi zQyS^S1nzrAIx&H}n2}CQ;GSfp6BD?@80o|W?iWTnF@d{(kxoqDUS6aV6Sy-M>BI!? zvqhS!=UH=PY`^l{?WsGU(7wAO za3?6z#!L*4aQAL*rwH7oiL@~z!o-9q0{2`ZO|RjF$HWbj(v=48pj5PZCp;z|{Ci(T z;0{Wp>3y~En0UoIOcA)x5@};fcD7?;!N-#ofx8=#HfG(#f66FYyWR_mz`cq{8`H6F zy7b*C{debM^%TclH6EKPpRN~;CwjiBG;rr3>e0@aPrHz(zi4vLIw@FtMGd8Go|*lc zXlz;ek$B^M#^#mnQm~qh8cN%|vu2cNY#Q_rrGYySQICHM!2SxpxYG@3V?MrU z-*^75&K^bJzBQzcd49oc(daz7KoPia4QXSJ9Gxe`!~2#h0{4_5otVHKVn|c3Y+$o0 z`7e4W8Tu~n_tNyS+0RQ>znrIcd4a&)TSy!8#BY8fo-F=e5x6T1X=4_yIW2wn$3eYP z@HrRisjJzHNS-M~X7AA{`1B1470JxGyQ^GL)7QR~g3knzP?1chLH|zXYw+}Un^N#e zB@!xxYq=C!{~Vl61dj{X-(jMA0#kRVgmP? zAgu}9L4^cr3*qNu#TM@CLINXc0(XTWfsr(Ud*G13NSeU?a7bVzP2he(BruXDaPJ@z z7|EDHse9y#x-_k00bWH&U?gL@g^5m!)AEqO9YRRcuVGfPzkvCdGIbS!`+bn6s@fF3 zWBQA_PA#bj+`WUeF?-ur2gpJxQ7L4ngI-$BgDMITKU*= z+>wGb9o@#8g=o9zDMjG^5~PWEV_FIytEuG{rQ!||9DUTNWJ>QiB^py7T#=6iW{9*g zO$vw0Jh98=?gdC-R*IJHPFcxsa$q$RtDKJQyiLxKC9uXCj}8;f`qA%_`n|7AT%UrK zU!=9ho2|a&zUw_!=B7NXMK-B1A2|`g zr^QHXV%@L_LLAIL9KdJTNNb{S(R3k(WbX{%lXavu@o1Tsgt)$aVE~`^Bdv*N=e{mP z|IHr-a2Ep7nwSzM1`~mMBaqfaUa9$_v2Ic@fIBu6t$r_K>c|;FG%Vf|zQzwv z3cd6j(!nrMrCxc9`0{NJ*_cl&1sp*{6h5-)WEp<4e81gz7UNcUTzq`SV(K) z^0BJCHt;@r+l722&`ZA|?c5FiU2&iy&`ZDXTzhzP$*Sm)DvCfa{q}OXHIEC?=#$U$ zFe~)Z?;_7wu|SArCs!x}z4V*K+y1#+i2FCpPy~AE_k_(S_t6s{zjxipS36@?=%wHK zZP?;T(fD{oS*3wq`W;@XSqeclOLYd(OTUr3;$n^v=j-fI1bXTBYjYON7NXzi0!5&g zep`0m(Ro4)+P72@=%wF%r7EWoROM6zdg(V=yEd>{9oe6+2=vnLrBcmOG^l2&2=vl# znNI)BFT~v9?-hYw`khg#S;{p?HOoBQ`HojG_67P4y5yNcwCz1Q5BK9EtqJFfdS&fP zdH9w9(wd-}WiIzbs#)gYy9-Ebf@+olCa7kak8e~UtqCV9{x`nEhws|ycj6Ke=%wFq zLjvDLM7{qTfnNFzH`Ks)Dp4;nfp2;uotVJ)PmxYc;9IdsCnoS+U8EBe_y#f3i3xnK z8R^6XUO7nr-w5>5?+@Y`L=E)PZv!SG&`ZDDmxw?w{ia?b0=@KmcZmq}(r?ivBG5~} z6PJiUFa3sFA_Bei`)Y{@^wMviQO#0j8C0{($9KkXB+{c##T}=(%ITf?;EGfv&x{V2n$k*Okx@TX^rNCAbaF7ZEV7V;gf@nnN4YGW^@0@d)&CY^yPvzPm)< zb>ce$J!i4Gb6h7wt6{UU8uaaf$d{UsaB^oaMeGSAKT&BNLaf{c!tb<7+Z*$IJRxa|3=t$U@uwz+SZnHBK>OM zmJ8IEDoX$HZ23KGUk)X2DNntGeqEmW8~<^>X+e95A_RI}4&A$@A`sZG+_?1YS2T>N zmd12BBIt<>RbzBdbP4oAJ!9zTuB7j5h5MBo*U8Xo+$-&HG>mb+0H!l>y--iGf_`ON z&T$V|A?9G?L8X@{l+@**HekTqI^wN6t zy}Kf!Jof8#Zd^PHF)c4@HE?bWfnKQR?hxYd#9>ybfo;;?iPMC8+%#=}J=yQ&@5EsX zF%!&CehcgwURXm$BM)?mom|g(1Ii)B3>%1>rYN9EIp@5;M?ih)E+nP)qQ>wjZN<- zXR^(7kZgL2OvLmV@d?-Kk`T7#tNV2chPqxC*tP8mFUFm+^1Nu&&Fz%QHq*7qrX5>E zjBXsCaJ+O@Cq^y~PUu@N@XGKBUW{9%-Z{}osn;=+ZKjjSHin72AC6BrUOKDWgZqN- z9;*>}Ip<+7#{J;KpG2eUnRc0MGo41ZF-$c5+cj|s$4h6mqg?5t+9fLmvhTRpi*efw zKO-8qv~8QoHq$qgZ446+4vJ4WUOKC357aLjTT~*j@cUj~jQh}EeiV&YzHXVxHq$SY zZ448CPmNDFUOKDI%RNQiK0Ka#Zdi&J<95CGlxSpDZkEY5(>}6|VWL#KlW_^hOJ~(( zfVb$W4&UVluWs$dxLcQfmiZf9gJZwnm%%oFYi$(S#&jj(%e9Ad3*!)um(J=OWi@r) z$=of}4vhQq`dy;2^~N0;Y%{%yY-5-xzCJ$TcO#8_;hKUK(ui_Dom(J>K>g})5^)a2U z4~+Zsv2CI;t9X3|+f3(>Z449F()AIKaJ+O@W9X`_^mb<81G>&J?)1C1*z@s9eg@l2 z7m-bGpAj*buJh$_2**oj_0Sz%f){DL+Ct+M#yyj}Ni-&{3}mp)^aQevsYFECk-Y-N zaR|ptXSMIdq{8RN*ijVYhC+pz{5`6oH;$ti*YY^oFN*OPJfueHq%LD8z>$@@n=i?BLm(J>|9bX2k zcT5X>zTrVH#{FpTbkSIK_o@uGnO;UVz1_d_%iz%WuMTXFLpWYKt2W&$7Wuoi4UGSM zfEVN5xp=B*G=2G<47QnWNVYLd%stX3F5!6Tth^JN6%D%XQf|w8GQAl0*%zM`jSBg% zXRyt5S+Z#lJrVcys2A|WAsjEARc5R1MdeQ)&AspGwqA_8eUo1_j&E6z!8X(1l5GqV zc{}10j+f4=)QKKN(~h3XEql757vqk7dQc|M&IX-5?_-mksJC8ec40 z>0_Je?POCsh}f7NpK!c%R{ch83=SyWCJ;y(?Zvp|UcW^&`cHku$2QN9Gsvbr^h7*5 zG(O>Y>8x6%`hzcg+$nJ3;NxD5yL3@c(U{d}u8(b|r;u$76FKAJ6ONb8>ZiR|1)Gjc z4KyGAlo#X9zb{iX0`+J3*k*bu+4QT9L|pY)e8Tb4S^f4>TH*TIX@Sv&*8)fN!^9ia;uDUS&MN7dRo#DG(>c(*VYV0JHl0;dG%B^7>0_JeOKoP71{390 z$0r;woz=|U$GZ=2(>dTD^t2b_u1&jIG+ys8$Hz9)FOW^|#1YZ2S$x9r(plZ{LXGsA zol^t*dq3sHxLYq)6^#y|c|Nw8Za}s%OdJ>#pK!c%R#|u7klrsNCGf?EPk1rzwWlhH z#^AY&d~7p)E7`^{@pMXj!tv5s?R#!idPbY%z}c%Odok|lMioS3@1fUxY%?7so90(U zlpheEaJ+O@2d`V0{)Mk&;I88nycn0hPMN{?kCJ9P#XXzBqutq1G1*LA{m~g!?D-KQbM6Sh=memkBsuV;t;#DT%5~$(Y6wgop zqifulc*lzW-qE(l&HfF4EsegKhM7Q>LRUqI@H-lK&Yid%QEQjkab*n?!?%}I8d#;k zs)#Ye4zA{35b!Sbo=wKAu;!rYUp^@>&z+YYII0M&ARukb{y$QLX#3UAlp|2ju9U&Zwe} zF0P}Irf(umx>+>p+}cNJ;2IZc`fgvH8-%#aj8z1#agjErZ)f>M2k(|yvlM|VR;0Pt zxcv&pE9_Ugzs+A*L$X>uX1UV96)Wm#4IFn-Lu+GJcsD>oN2=Zbg7n=oPi|9L;Tjh+ z)LG$O4K=j(AD!2VMyUQdrGYC})YBSxcSa4ZEsPyY{EASc1dL-0`1z`}|8{2?cm=CS zOwb>PfH{@YfJhby6KY1L2#BNzoF`&dDv}~__KJjxWbVwlrW3cL!RlK*NZ|QE+L+?) zbGrON&xC&-?D+&|;YeU4`W?uDc7J=}_zHQrHb4R+8FP5~jePI$W}o_709PzXU?i%l zC4DW=wp)j0tcam^V#PvB67)cQZ39N)5otVJ72GWY~ zV`Tyf)Hdev8Xfq2(6i1ad3siavll)0VI6umoW2yxy*e`gAe(E*_IU(8o3MG=857R# zqV*z?OsS)-_=+l;Hn^SEpm~X-p`T66_{WbzjB4FT5$L6#G;~|mIhi%KbfI6Ofp(ym zer|B%@^M0pzowib&`UoBxb*x2A>P_}As02!OV{*UWql+>+KjIifnK^ozo&h%5K~KR zR|Iex9t!F-}+JRoWdQNi&(V#hl(m*d=3#U1Q5Hx2{1bXSpHq9A?pgDsg z&`Z~;v0vfJ1p64@;oN>ja|Y3%IfK%`xVlb_8o0tjJ*|NRuKSSIgs=Yv>ANSM*ru|= zOmt-%HE{ikdRn8X^IFkp)8L%az_>iN^9sq<+ff78;i#uI5)sM^t~BeuI$>5=5mIzQ0_#*r>#Xj`xn?c5oaPJxB+yHrkJMkAbo=x~cG0ws zdp2Ud4QWl#(egV-7ps6sCnoqV%)Nmtrbx#SEF)(S3G~uWGweBxCA1elo4|JPou0Ge zS~2SJ-I@R1K4U=wYsE+>Ca{8ybPN$`ITGllpMPLhScykEhKOh&fnNIg2f4>3&`Uqf PV0Y&X;xh=;)5QM(--#~s literal 38084 zcmbWA2Y6J~*7tXij&u}Msx;|kPDp4o!G;tIB@{&jOo9}pD~L!yI$U~-f^-EeSC|<< zgqZ+WS^@|N0Y&A~l%ja43PQfM*3M4;douGq-}CuA@9Vm8{nmf)bN1P1?REA!)2Va& z%6${xN*vmIcw$23KBFo>pE&G|_PzV4|IdH^%A?feyL+FAQpql*l08aQ-qCJSkKl(X zk9cCAD!t>SEIWqmjBRr~i|Ox5HQl#7EP-B`nU!{&%qBkRbS}7(n%GJq5pcm>XHFM{PwS_XOdZ`~ljLUs$pQT}1r8Sg_ zTU#&;fnKP`2RS(yrBrm^`1o}g7mu5v^|3Mpvq+$qu}!N|eN4ryTjRoWfHZ~5K5j}) zy1Q41#Q0-$7IV%Z5_Tk|mOG^3d2LL-MtBslw|1*y_i2KAVg3vE5NY#w-H-VAV;eig z=X-*6Qc@lX4t#d7=Zlm_-1J{v{mK54?DX_>*C9IRY%A3z0)bwn8T^h2j-=E_)Pg)} zu3Q=fTc|JETk7lB@=r_`;?v1`9gZ!hBt z-FVf!a!Sj*l1Od~7iX2|UtBD@F5&jH|Wl*ZZ)C*b;1^QeETYg*Y2q z%#Q?m+2@0=4f;RlTx_wmt5f#H^`&c@TI6FpiR^@=3!XJ(AE5KWHuqc;Y$1VO#-6ov z1ogQO9iw7db0WrV={hQUi4@mOb{c4Kbp9vVg9LgRjqFx==y;KuQW)3Rja^5xH5!tw zV}~P*274J>YqXD#uSJ|3VT_xRek!gX9V^y2K{ofpXz*FgO`w<2Fa+N>l6~#tkJZAs zZ1*RP`Kbg>QQ0{j+;-FXf=ZR!FGPKzomX?4V z=rO&B?CgGoC5#tGGI~mlq;YjWGqW+zyGDcKew^}1t5I1R%g8pggy#PzJx2n)jE2=J z86_Au$9}|Z==qYp;{7AXE5-6Mgw}W|ej=TZ%F?PF;t}_Nq>8OZ>v82CNcK)TR$4=k ztK0;78I9bn!nnrf!(wR4uf6=gbFL`kMdK>FCG2GgtZ{A4nUU=X|Bsuiq*f)oHbj1Zo&Uw+hckxVtmQHIewPt<=|9 z8b?V33DhuzZdDA7-O#)Qvka5f@X?(p{rEWkm=_7uKyA)NE7RnI?S4Z$;~v2znmXYR^wm6>084mUQ>wT9L+_48S+Y8hW| zGu~MY!L&_Khm%b|@SbXDzV{hh-#;=lmr1Mgsmjseh};@<4QBTP^AAIFE@Etbuk~d0 zd_&Wj-f#qa+5Mn7F}6|UKX46TXwKD*t#j&5Sv@beXG8691bf*Ubk)Mx)hk6_FBuwby*9{C z=bHOLRZbeOxe4~NHSk!OtB9{0L!+(NYQNCEi8U_7&UC^N>}6}bmeuoVh2C5r?yA)q zx*BD!kDO706$nFfrNG#FMgL-!#)v|{*9u3lm))vgVrNQ@c!BzZ1bUfS!c=kSXzAG} z842_X&OcKs?TX;W%E<)62y)@tmITPj8;(%-qJU*^jKbnZC5sWTWaCeX{+f70KT zno`~4y51p{a?E5}i)#&zo0N1R^##&9kp3WPTu9|74HYL|rgJxuh#O%D^fDURtqM|n zVwJ+U#=a*l)Ea+DtGG4c2&2JX#@4NR`*7`GpQoO4e*fI3+PmtgCwm+1!tQ zWGltQ=q0%c^fDT{Rp$qk54K&u(P{Q+TMx!fSax2H)#Z;_XxnzE`$@7{;~BE)e!l2VU|_w<&gBpSy--i7gKwM+ zbQ@U7aqaHw!MOcj&>DrSjdJ%Rn>8LFn=<77Cj&)SjB(C?YZK^&dPMj`vhsUCtInrL@fKRxYH+uKrhr&YSWx~ft5|Kx-LFG!h>;> zx@(Pm>EqmqWV1#dvXweYM7hKFUFSjw^g=y)+B3apphW8{uG#+`<-xc+emEojXfW#o zcPiPeQHN}L9z{ftJL#^4Aq0A%o>CouEE71Dyw3Gw>oFdTd+24Yar>JI?t^5r#tpKS zIzYsclKWi8LkRRjJ*5(=E!f_-%X-(n4r4tSx9ku9m40;VIMMwO*{l&ywo>k@3%1WG zFwqs&(I(If^=P$K^uB*wR`mnpmhY}Lx>MZ8$YzZ$qOq^|eZPv*^%5k|3-y%hSz|%V z!7aaU{5SOj<0hOqE&aGf$EOe3tZ_|_l|#ht?=LpK8$zHL>M3>nhcfO%yE8WaLPrFgnIqy!ylio%f&xR1_g?dVLn%2{u@z8@C*U~7#xalWZf7Fns4o|RUZ|(krP!jL!yWc}GbhG-Fz%swTH`0WFC8SC zH8SKLb(4sqTQau>KZx({O9!S%Gp0J^^pBAYdmPF>ZSLDLGbmDFa9&n>B7q&OrBb&yf5@eVhRxfnKPm z)Eh@?d#+zx>?=bA#;r9_YfQh>%3YIe)|g1PZdGOp-6|x|3-u%e@GRK)iW}hWq%(+dt9|&R9IJ0# z4cw2D%^Im>6G7*^Nxtpgxs)>?fnKPm)QdC{f7$Yh_b(c|7+PXpL20KI$$^Hft;;TNAf~&2kawg?dV*&A{n_A5jmx)5w;7kgbU*niE3_^g=z!8QfKy--izqpA#hxY5sa$HBPKJ5ESHiacJz?U2nH`N^h!&^_wuH>)*16GEUD>d|PY zJLhi?uWx*t?wlBROx%gom*^h#^vz=K_sC`q580adFmqil0=-aAsl!Fz_s3=B3>bHO zFhew+p}6D7&e7nNig^lv1bU&K~i+HfO64L0=-aA=hVw7r~Zwu4~%kGYP#;Olg%0*%QeVZ z_<{Fx6X=C{dTo&0y`ZcOFm8i(T7&13`egH-J3W{1+Tcdlg0eP10=-a=Mmw!>rl*x~ zJTzZn-1y_)OFurNd9X9ttnrD=i9RAG)+*;T4I$7A_2}^!t<~l~TiF>#vna+rm8dm% zZvT;N*3ff16Hfiy1bU$!J>8`h zos}U3dZC_DymsC(^xv{}#<&^tw8k{LzaAr-H73Ztma>fCp#D!fx9L8J1bU&KQX^r!$Cgrzx#bpK{TuWV1#ssWUKfn9g}9fnKOb`4v?SK6s|GbB)F> z#w{`XupBGSsf&}%8ak(Dq7T&>LJ9OjJ*DEQYLK*MjB|praEu%C*x}TUbpI$o>w##p zStF0E4Gz&Y*z0l$$DwNw3G_leT{Y;x?sb_NFz(Qm--*Ucw8j}sHfyw%I>T<7ODewJ z!%3nn0}1p(J*8+>6nMEt0f$#b7}r%pYviT1T4SghVe^Y!+*exN%J#$8fV zYw+6nBeGdTubuBuo#E2)>#k!V1bU&Kt}`TM)fq7ExlayCKf2Po{|woz(OGH$&8RX_ z?AmHqCf#e1KrhteoZ-Izepb$aaW7Wa8eGfxm2B3~wTwsvdZ8X|0i@h#U-QmVHNdzH z*Bp?3a2=)r**V9GD-$Dn)RM{s66l3`I)|J0Lbb-6GhkeIb*)j4&Us6+S);bp8K_d^ zuKCS|4SWWXKrhsz{q0mW@Dx7TXc3JPjGOXFy7Ys`?qaf8LyujqGniZy3G_lerD(Lf zJ2g%7@@U7nuhiBW9qGF3PBv@kI>T#}g;&pPz42BEfnKPm)ZYA?-Gl$r&AXVcT8z73 zLz?u1=aTtkvxc5acr|g)RoKU?2_(=9^^_V!RReWty!R;0R~UC&3$5`i&57TT%^Go1 zXP837*Z<7+riBpbg?f6ewuDhifq=t)vY}WW*>I_U&qdG$aU|&Px1vi*et%wMY7~p7Q7sWV6N^$*H+A@jt4O@n}Z^y--hA4TiU! zBXtIhyL6n^xI}A%C1kV4d8so{mf;>iwLZ==kU%fgqkApQ4Ec9=ld1v6JyB3=ET?r% z2HC9fQFfhS)ymuidZC_D@6immgz5}`(hP@jQ)cXzV|AII_V@p7CHV0=-aAR}HFCoq<=W z7&p(%Z>1lFXzd(DHf!XSI>XCUXL#zZYK_l@5a@+^x@z#m?2A%0z_@LTYKwDX!%%HodF5-LOm%NB%7U?827~)|B-&M#tUTUXb6*)WgvlGs3-l{-iLA@ zt{Pz6MMbm*AFBmqv&I8*to+p%Y_CIg27Wey1bU&KoR7e1sx$EUz_@Fs?~;D-IbTgS zYv^;njdHlpDTm7lAQRBQ0qz5k%r&|~*sl#8DGJKeP^gg`IU zqkd3My_)I_JlZktj@dhFS3KFQQC;c` zm#MaNbAEki9gPws&-wm9Psp)VAfN_87p*5bOJo*&btkF_(YRWPK&orAWqa6wK zLOq(VsLn8e>I^?o27qz%{NT3($$vP^~zGP#m8em*?eVglP zuWb{J%rxH5z_r(9bx%vR776r1J-R+-^bF*oI>Tg|sWI*=!?gxK&+bk(_oJ4qIk|@1 zXnZlLOdx??sHYTV0D*r0Tz7E>fN}fX_(uA{wS~T9v&L0f#TVOoRN{r@JLH9AV2f!62SuXI>1PXUlXFVxd@hPbTvyfCi++E(cY?_uzf zouh#{0}|+kdUUKPlQFpu#=SRWtL!P`WAzW&Ime3Y3~2#J`hf&`p`NZY?2U=7n*ShG`8RyNP78h90|IXLzegZUVhf zPjUwL@{&43#<(?aZ;^iRHCTge*3j1=T_5hh=G67_(+nig3-#zar?tV6gz{eAoq%!s zystHQ4?}OVS>wLUCG>2<-H+A=yaNIW^g=zQcz41_nU3sEz_?QjX$`KoP9~c*^qhF{ zjg#(vdEfBXr}+vA^g=y*KLf@sp0Zh=bDGbKkTK|dL}%XW@;qR3-y%Z z-3d?7vu)m;fN}fP(;8g=?n^fBfjlC027b0(Xj6tf+eQMtP*2wx{yqDOtb{Rc;XQgk zWM8_!79yK9ddt0*pV6l+oFUKXkw7ohljjDW72B2X2Hl-8uGw>myEJvqz%_uvB@4)Y z1|-bhO7=oMJum2;lk_G5wB2aveVsY`NNoc9fdodf2|T(;U?iKsbB+W?vI!iCNMIzJ z!0Q7EjARpd)gpnBY{Hs-aMvu%L^gpl9BN=BrE2{1!6rU~ziw8WaNb1%Bhho5i`UmP zQD^T62k!<*U?in{?@isv#PJ>j9K1&%fsvG|TQ;xk4{pA*mxK3OBruXv^A@;;Sa&bp z!TUK97>T}8Fm<>P`5$fPV3vUdMxyx6i{YP5qH84?(Y zo+$nv5TbP1+79N4NMIzT3ba2WMD*Q?4rZ@NU?inVKXFco3I|F%m{TKxk?7i9enW_> zlMB+G>YOzP5*UeAURlH~B5?hI1V*Cu$ENe5;XP2s!Br6w7)hz|n~w{Tzw_e`uGNsh zNJ`ajw?~L~r#5nM1&IVkqPz2*O+suf`ILhzVI(k;eGQs@;k>6E_ivlO^FH**1n$O0 zIx>NKtC5aO;7)0zBNMpq8R^Ib?qWtdGJ$)Nk&aB@4r8Pv6S!X(>Bt1`Uyk*24wS##neWougm?lwhQsWMq}V%@tHEduwBB27E5 zvgX7B2TEH6?(0NaslS%#Ir0AFf);_hG?7*+{|Zgi2_bOLCDL@y^y@is?!hva2JWCl zTB+$<^qlxmr^hVTB&tA-w@)xeB~?x_unC{RB`v)LTqgHfJNYLI;54llEz<7N)CQH z^1O8hac>;bO8tA6e$%^ltNj*%`__=Adg4>lL}O)TuSMX#HKeI>H)N&|U#n#nfqTl3 zrf;`JAaI8m(n`H{XsT!=%-CpY;0`gQl?wElAjI+-do2R@dm*jV17}AIF)H!2Mc`g6 zq?MX=X^V{An2r^q@p%-EW4xM`sxYp*5I@&y7LCumkibYe_wy1liP88Z4hf7zZ>*R6 z%EhDP)W(I;_4ElPwM0&w{j41s4q# zV&ut>ECP3@AWb_n8z&1f;rI@Vz`Z6&)0vq0FCo5l{;>$$If680$Ok_WV$#t@o3Q7& za|CJn{?*vCLcFnHzD3}!4y5Uei+f*+=23ECVE-n#lLKcToRui2zS}{vzHW_cZbkxI zh%{*wnk2-?wLL8YTZMG?XqRttU^Nq~oW?e9lQX5i;(;~RaI_}=TW56)6SL2hXo8hr zq-~8U`|9%1^?Y?D#gFx9q;2AldPRlE7`M=mRdS?lqQZNp`9A2`zjUr2YxhXo#Oc>Q z6QX9T_jXP7<8uzAZKC0G<%PI=bAlhAzF0Knk2&8QNS(TX@3o#s z{~7DYXF^EZ8v3j!ds^-t;m0RcNZZ8LF;n?o>)F*~fFGZqA#D>4%I0;L@a*j6$ESKo z+eGsPZXr_d#ryGDBhof;ZSrs-;va42$0wjj+eG1V^MtrlHO`OEWs$at6BpMB@l(}k zKRzu++9oFd5fEa+!?peR3>#^i_^s^`A%44C(T`8ok+zB2Pn;8?*n!f1eBO_=P1IX) zLx?9P7xd#U1f*>u|8h;_pHk3|dn1sxiJAWMqA}xO89(mWK-wl=+j3k8cc;hwxQ_&B zo9N$mj}UF9HS*)`7Nl*0YL-G!&C-v1ZjiPKbA9l?%@-*6D;LJ*J*oCrBO(##Wq$)A z5`kX!7abxI=w*MOArgUJ_SY6}G%G7J^^ngW-(-5rJx4G5+X*LLtK;MAymHrK)^|$K z%l;C=f<8B8z8bWvkVT-E{au4&ZI%d8^j2PrKrj0%1w$u2B*gK%nb~KJ`+;8eHwS*c zJd9WLp4NMBS_FF8Ul5o+rjX2un|kDo4Awv|`+ES@%T5rY{?5~u271|V_cvM)5W;uw zm_?vhn)L;NQ+5mX_Krj0}`umq^3DK;o-y+b^Ig+Kan6ru>8xStZ|V0+%(P_d*C zlc@rUvk%6#-;nNBX^s$|ml)#5eW9plYrItSx?CTJr~-)^7}tJ7y3>QhM5Dp*DSq7F zih8!jyh~d|qdC<}Py^%IZ%ETG35dqxl%Nk|A#D?_&pssiZd_Zn842{V-;g$UgO_6x zX>QNSAkoWy-+5rCdeW*R-OE@6df9I;504rrM8)wx`cVVD?01o!6CVnZdFl&`Krj2v z;u+gM7b11!EQ>%d`#s@OMO;mIZl5&z%@}MIdf9LNrUi7Kczaw0O9Q>^cX->()cx51 zWg&||FZ+$$ArqEMKfdx^v(7ns+3(jzUD8+7vYW>(0=?|FWh-1=FB+B70~Uc^_Pej2 z|DlPG4%h^G*>AGG`kHQ4W-Gf@=w-i`+T(|hrB&VQ|7o=fz3jJ4f2*=ch&HS8TSphY z>~}`zU5*vvhYm~qxbq#aU>pldZ6AA5uG*@#xB78EKGHT}uBZ|*=l%GW0Ma(mzf?mX zkHoy*hd1H73rO2Ug^GFQIxl>%*Cu?U0%@Btt>S;3A}QU&PC91)khZz^s?U{^IyZ5$I*VuNH|wFZ=DYK6}5D+2_K*OPlbWF`S9)+2_%XFAFhr z-+YTeFC0Czo2qk%Ej&s}d)j=B8t7%W>htyMvwtfkDQRIU|Gw5L`o=5$wohuWESr9h zCzXG9X@z_>oAN9As(iA{??|9mB1Ph=nkKkK=1ZO!*VtPs=8HCT%fcCSuZcvUm$9|R zlqplzQamq_L5yo!$Qq_qT7!DNmVQAg7lB?zBfC}bB~N{`lNsLtqc549M4f-$nO}? z6C1Mmx8dl^$*u;SYKhIi8Fp-A*DwTn@$ZM#BSJ^wUwyluIo(HB5RJ>6xb(l>38X<^ zF{j@>Xzy-Fg#HaT{&N2n`klCgAq0A%o>FsCQr7OsnSHY3l18v+mPQ)cb+R=wGsmqB zgdxxi_4Mz=(XY#8|4y8VORWu>e0KFDpkf6&8B?3vipG=*eCf7 zHzDXZ+@g`dBaJlwU5_jNZaDT3TY@d*Q9^sQX(fd$SV%Lr+=b_R{ri zZ2tWq8tMGKgScUIZuGCE{oe@V#l&0sY*L@I{^8zBEEAXd7l*c?4WPmj#>;M1jo(IY z&zS4+otZS2+Mg4*&NCm2#;iN%J!~^QgKVXST^hOl^rTGh=`ci&*K+82pkvd(H#6?i zS3E{bEN(l;-8X-UXtdjQPHd*zkWIUziRk%#3*V72gz>UlariYWQ)ZaubOt|4)7=|!j+(M&AzkCun95}tHzHio)eiDoO!ErApq=?2} zzy2aN6F14$#DE$N!V<>IZq@Drd4u(fRrh_+=@lz(!#@^@#`@l;JZv-lDcMRfvGVk=fl0_r-^`stKO&U zqsQD*z8Q3VVBD|UkI@?GZ+h4knrzx9LqzwRCBqWN%Wl;;x@wc@I$uXuEyi6@=v~p6 zzqOx-ZKh|EO>YAd@oS)*Z&et=c-gIbp;Obq|6(fm{zJ15#{FsS+oDm}O>Z@m&2&Dp zX+Jd)U1`4B8-_4mcB_8=b>#Ls8!E}Xi*XyfhKWWG`n?IZneHmT+QY;PzVL+cvRg&J z8x{fYVCu~1C&f{zS&RB_MenD`2 z?IxmeVSJ*8ZKh9?trQb)r-Ubrm))ulwmu#3H|pVQ*nG6aVhxU)cBHXr96i@ZY^Kx6 zRw|8%J_B3$>V_eVm))w_Q&t43?JeuOaAl|!x7v~hqOs@Wo*uTD-bpt71`iRPXLa`d zFAQP4xP?ZK-nHESQ=q}kdcLFIBuOmp2ghwbyRK;T-PKKOCVG>t)Ey#Ti!R~YABHeq zcB_iFEE3$bsZ{cbzY8iaCK$Nf9`*p3j z2M$ydjf4Si58F)lB%8iZO+;X6hPP1|!gz5DjUMHqzqSr`>wV5!@Jc0#C1aOjB_$S< zd@gP05;xmSq>@c@I}t_CTn$SYFS}LCjyDVDTllAUcT@u_?$Oc@ibkEAQ`~GbU72k9 z6;C1tuMAHZFS}KF-mMV)*M}v2A0BJt!MMZUEhHLcM-Fkb&9ow$<|`sj?kwzE9ELDn zcB?)~y&GsZyRz@OOFcap_pg=tMWb_CS2x>Cw&w*`h@s_pxt z;9DMyTYS+2qA?)(X*b)PA$KO5a#12$JX|L%VZ7{C$+%cDNxVNHq$G}roHDx)ch_yVZ7{C@p>Xk z#me0PMo-RqDZ|Esp#XQu1_{qafmn^2u~OJ#eArrMpdh<9fdD!MKx$ zY!i(=s)?IzroSRvDJK3n8lEs-cB@{QR>ocETD0$I+c6%D`)8XiqS5knGdJ5z*C1Od zCiWF=5|%JtcB@|L*4#bqvg14P#b^)4o%O0uG$wA2bFP^`-q6?W!f+49 zzr!XZnk;0yPP?eG?{ij*Kd7%Ol^hy3r{<6%{h&G%U8_J)Wx{R= z_k+FcRxwi~qbg;7$w5Q#-$LsehUWhknJ``)$=I6UItE+ejurpiX!CL7|IPm-d(P3X zjA2W#O2L2URUcPdw7=27b8h0Y2Wl(zlQ%Da55SYuyr89l)eqF8UmuFzBE$<%-f>~8 zu$E!dRYw-*X9k}3g)dkHRwj^Es`Z?fLQE=o)FQCXfHYOJ%e)~(_g$$LfmH*fNu&A{ zA!@Js(ju@1fHZwcXX!^mJl<-FMc|4aX?kC6#4Y(vy{FtO>%RKz%?$?N&} z7*}(n!#*ob?+cvKY!2|OQ2)0d`h^=Nu8bF8P`+#*{s3r7MYQ6}?} z-pjeX^9diW4UoV{^#0(uhxu;fS#}}LhbtB&Fp^TUU+gF2s@H@;4iZ?`K$@Oy|9G08 z4|=xtY-eEZOX!D72@*|jG}1;vm%}u zOt!>!t}G+?82v&wk81N@*yg&ieiDPvCa7J>tikQW>@HN#M56o0nd*(1sNS}=rNMcE z(XgLQ)M<4=h#~#TTLgO9PZ~12xLm9;cz1D&Krj2bLDJ2ELfp9YfJLB}{S@H42j>XU z@W!8BJXYvs*Yp?HUoXVXc0X7Idf65FlnFb9_@Kisi$E{C-adTOL@)PzUY_w*KhVpr zo>R^s8k93w8t7%$!k5%7CB(zY%`F1G?8^4DuDn82UGj=WpqE{z#&Lx!6C7jq*rl97 zMhWE%mIlVP>(r=$D=gHrHITq{AJR5)VMwa9igE_4RoD`{vW*(JenmZ7V@&huqCq)> zrGasIZs%)W*V|D8*WsvVYY@Q@U+2k4pch^R+T9RXPq68nImRZi%7Jt)jqFxo?FH$` z1XhHQj!a;k3Tc~&T61v&_ndMD9}?(gpN|bc-fKSce`ABgla_D8dK=OBt1uCXtRvC?wF!ewqQTvIz9zdo7Pd{lo-_WUR`fUStAm#Yjgcu!4<3ojk&Z|xB+$!#{s9^mfnN603?P#683gLt#Qy<6tZ}^n From 24ec64ffe82fe6eac628770b8fe581cad4b1dc9a Mon Sep 17 00:00:00 2001 From: zigzagson <1269925853@qq.com> Date: Wed, 29 Apr 2026 10:20:53 +0800 Subject: [PATCH 2/2] adapt to 1.4.9 userctrl version --- simulate/config.yaml | 1 + simulate/src/param.h | 5 +- simulate/src/unitree_sdk2_bridge.h | 205 ++++++++++++++++++++++- simulate_python/config.py | 1 + simulate_python/unitree_sdk2py_bridge.py | 3 +- 5 files changed, 210 insertions(+), 5 deletions(-) diff --git a/simulate/config.yaml b/simulate/config.yaml index 8511b699..030565b0 100644 --- a/simulate/config.yaml +++ b/simulate/config.yaml @@ -3,6 +3,7 @@ robot_scene: "scene.xml" # Robot scene, /unitree_robots/[robot]/scene.xml domain_id: 1 # Domain id interface: "lo" # Interface +lowcmd_topic: "" # Empty uses robot default: G1 -> rt/user_lowcmd, others -> rt/lowcmd use_joystick: 1 # Simulate Unitree WirelessController using a gamepad joystick_type: "keyboard" # support "xbox", "switch", and "keyboard" diff --git a/simulate/src/param.h b/simulate/src/param.h index 76d11378..db0f28c0 100644 --- a/simulate/src/param.h +++ b/simulate/src/param.h @@ -15,6 +15,7 @@ inline struct SimulationConfig int domain_id; std::string interface; + std::string lowcmd_topic; int use_joystick; std::string joystick_type; @@ -35,6 +36,7 @@ inline struct SimulationConfig robot_scene = cfg["robot_scene"].as(); domain_id = cfg["domain_id"].as(); interface = cfg["interface"].as(); + lowcmd_topic = cfg["lowcmd_topic"] ? cfg["lowcmd_topic"].as() : ""; use_joystick = cfg["use_joystick"].as(); joystick_type = cfg["joystick_type"].as(); joystick_device = cfg["joystick_device"].as(); @@ -63,6 +65,7 @@ inline po::variables_map helper(int argc, char** argv) ("network,n", po::value(&config.interface), "DDS network interface; -n eth0") ("robot,r", po::value(&config.robot), "Robot type; -r go2") ("scene,s", po::value(&config.robot_scene), "Robot scene file; -s scene_terrain.xml") + ("lowcmd_topic", po::value(&config.lowcmd_topic), "DDS LowCmd topic override; empty uses robot default") ; po::variables_map vm; @@ -78,4 +81,4 @@ inline po::variables_map helper(int argc, char** argv) return vm; } -} \ No newline at end of file +} diff --git a/simulate/src/unitree_sdk2_bridge.h b/simulate/src/unitree_sdk2_bridge.h index d8a19fcc..59e6c4d4 100644 --- a/simulate/src/unitree_sdk2_bridge.h +++ b/simulate/src/unitree_sdk2_bridge.h @@ -8,8 +8,12 @@ #include #include #include +#include +#include +#include #include +#include #include "param.h" #include "physics_joystick.h" @@ -166,14 +170,15 @@ using HighState_t = unitree::robot::go2::publisher::SportModeState; using WirelessController_t = unitree::robot::go2::publisher::WirelessController; public: - RobotBridge(mjModel *model, mjData *data) : UnitreeSDK2BridgeBase(model, data) + RobotBridge(mjModel *model, mjData *data, const std::string &lowcmd_topic = "rt/lowcmd") : UnitreeSDK2BridgeBase(model, data) { - lowcmd = std::make_shared("rt/lowcmd"); + lowcmd = std::make_shared(lowcmd_topic); lowstate = std::make_unique(); lowstate->joystick = joystick; highstate = std::make_unique(); wireless_controller = std::make_unique(); wireless_controller->joystick = joystick; + std::cout << "[Info] Subscribing LowCmd on " << lowcmd_topic << std::endl; } void setGLFWWindow(GLFWwindow* window) override { @@ -274,10 +279,190 @@ using WirelessController_t = unitree::robot::go2::publisher::WirelessController; using Go2Bridge = RobotBridge; +class G1LocoServer : public unitree::robot::Server +{ +public: + G1LocoServer() : unitree::robot::Server(unitree::robot::g1::LOCO_SERVICE_NAME) {} + + void Init() override + { + SetApiVersion(unitree::robot::g1::LOCO_API_VERSION); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_GET_FSM_ID, &G1LocoServer::GetFsmId); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_GET_FSM_MODE, &G1LocoServer::GetFsmMode); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_GET_BALANCE_MODE, &G1LocoServer::GetBalanceMode); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_GET_SWING_HEIGHT, &G1LocoServer::GetSwingHeight); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_GET_STAND_HEIGHT, &G1LocoServer::GetStandHeight); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_GET_PHASE, &G1LocoServer::GetPhase); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_SET_FSM_ID, &G1LocoServer::SetFsmId); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_SET_BALANCE_MODE, &G1LocoServer::SetBalanceMode); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_SET_SWING_HEIGHT, &G1LocoServer::SetSwingHeight); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, &G1LocoServer::SetStandHeight); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_SET_VELOCITY, &G1LocoServer::SetVelocity); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_SET_ARM_TASK, &G1LocoServer::SetArmTask); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_SET_SPEED_MODE, &G1LocoServer::SetSpeedMode); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_SWITCH_TO_USER_CTRL, &G1LocoServer::SwitchToUserCtrl); + UT_ROBOT_SERVER_REG_API_HANDLER_NO_LEASE(unitree::robot::g1::ROBOT_API_ID_LOCO_SWITCH_TO_INTERNAL_CTRL, &G1LocoServer::SwitchToInternalCtrl); + } + +private: + int fsm_id_ = 1; + int fsm_mode_ = 0; + int balance_mode_ = 0; + int speed_mode_ = 0; + float swing_height_ = 0.0f; + float stand_height_ = 0.0f; + bool user_ctrl_enabled_ = false; + std::mutex mutex_; + + static std::string DataInt(int value) + { + unitree::robot::go2::JsonizeDataInt json; + json.data = value; + return unitree::common::ToJsonString(json); + } + + static std::string DataFloat(float value) + { + unitree::robot::go2::JsonizeDataFloat json; + json.data = value; + return unitree::common::ToJsonString(json); + } + + static int ParseDataInt(const std::string ¶meter, int fallback) + { + if (parameter.empty()) { + return fallback; + } + try { + unitree::robot::go2::JsonizeDataInt json; + unitree::common::FromJsonString(parameter, json); + return json.data; + } catch (...) { + return fallback; + } + } + + static float ParseDataFloat(const std::string ¶meter, float fallback) + { + if (parameter.empty()) { + return fallback; + } + try { + unitree::robot::go2::JsonizeDataFloat json; + unitree::common::FromJsonString(parameter, json); + return json.data; + } catch (...) { + return fallback; + } + } + + int32_t GetFsmId(const std::string &, std::string &data) + { + std::lock_guard lock(mutex_); + data = DataInt(fsm_id_); + return 0; + } + + int32_t GetFsmMode(const std::string &, std::string &data) + { + std::lock_guard lock(mutex_); + data = DataInt(fsm_mode_); + return 0; + } + + int32_t GetBalanceMode(const std::string &, std::string &data) + { + std::lock_guard lock(mutex_); + data = DataInt(balance_mode_); + return 0; + } + + int32_t GetSwingHeight(const std::string &, std::string &data) + { + std::lock_guard lock(mutex_); + data = DataFloat(swing_height_); + return 0; + } + + int32_t GetStandHeight(const std::string &, std::string &data) + { + std::lock_guard lock(mutex_); + data = DataFloat(stand_height_); + return 0; + } + + int32_t GetPhase(const std::string &, std::string &data) + { + unitree::robot::g1::JsonizeDataVecFloat json; + json.data = {0.0f, 0.0f}; + data = unitree::common::ToJsonString(json); + return 0; + } + + int32_t SetFsmId(const std::string ¶meter, std::string &) + { + std::lock_guard lock(mutex_); + fsm_id_ = ParseDataInt(parameter, fsm_id_); + return 0; + } + + int32_t SetBalanceMode(const std::string ¶meter, std::string &) + { + std::lock_guard lock(mutex_); + balance_mode_ = ParseDataInt(parameter, balance_mode_); + return 0; + } + + int32_t SetSwingHeight(const std::string ¶meter, std::string &) + { + std::lock_guard lock(mutex_); + swing_height_ = ParseDataFloat(parameter, swing_height_); + return 0; + } + + int32_t SetStandHeight(const std::string ¶meter, std::string &) + { + std::lock_guard lock(mutex_); + stand_height_ = ParseDataFloat(parameter, stand_height_); + return 0; + } + + int32_t SetVelocity(const std::string &, std::string &) { return 0; } + int32_t SetArmTask(const std::string &, std::string &) { return 0; } + + int32_t SetSpeedMode(const std::string ¶meter, std::string &) + { + std::lock_guard lock(mutex_); + speed_mode_ = ParseDataInt(parameter, speed_mode_); + return 0; + } + + int32_t SwitchToUserCtrl(const std::string &, std::string &) + { + std::lock_guard lock(mutex_); + user_ctrl_enabled_ = true; + return 0; + } + + int32_t SwitchToInternalCtrl(const std::string ¶meter, std::string &) + { + std::lock_guard lock(mutex_); + user_ctrl_enabled_ = false; + int mode = ParseDataInt(parameter, 0); + if (mode == 1) { + fsm_id_ = 1; + } else if (mode == 2) { + fsm_id_ = 500; + } + return 0; + } +}; + class G1Bridge : public RobotBridge { public: - G1Bridge(mjModel *model, mjData *data) : RobotBridge(model, data) + G1Bridge(mjModel *model, mjData *data) + : RobotBridge(model, data, param::config.lowcmd_topic.empty() ? DefaultLowCmdTopic() : param::config.lowcmd_topic) { if (param::config.robot.find("g1") != std::string::npos) { auto* g1_lowstate = dynamic_cast(lowstate.get()); @@ -291,6 +476,13 @@ class G1Bridge : public RobotBridgemsg_.soc() = 100; secondary_imustate = std::make_unique("rt/secondary_imu"); + + if (param::config.robot.find("g1") != std::string::npos) { + loco_server_ = std::make_unique(); + loco_server_->Init(); + loco_server_->Start(); + std::cout << "[Info] Started simulated G1 loco service: " << unitree::robot::g1::LOCO_SERVICE_NAME << std::endl; + } } void run() override @@ -338,4 +530,11 @@ class G1Bridge : public RobotBridge; std::unique_ptr bmsstate; std::unique_ptr secondary_imustate; + std::unique_ptr loco_server_; + +private: + static std::string DefaultLowCmdTopic() + { + return param::config.robot.find("g1") != std::string::npos ? "rt/user_lowcmd" : "rt/lowcmd"; + } }; diff --git a/simulate_python/config.py b/simulate_python/config.py index 55ed0431..b3d581b2 100644 --- a/simulate_python/config.py +++ b/simulate_python/config.py @@ -2,6 +2,7 @@ ROBOT_SCENE = "../unitree_robots/" + ROBOT + "/scene.xml" # Robot scene DOMAIN_ID = 1 # Domain id INTERFACE = "lo" # Interface +LOWCMD_TOPIC = "" # Empty uses robot default: G1 -> rt/user_lowcmd, others -> rt/lowcmd USE_JOYSTICK = 1 # Simulate Unitree WirelessController using a gamepad JOYSTICK_TYPE = "xbox" # support "xbox" and "switch" gamepad layout diff --git a/simulate_python/unitree_sdk2py_bridge.py b/simulate_python/unitree_sdk2py_bridge.py index a9ca97ae..646f1c23 100644 --- a/simulate_python/unitree_sdk2py_bridge.py +++ b/simulate_python/unitree_sdk2py_bridge.py @@ -22,7 +22,7 @@ from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_ as LowState_default -TOPIC_LOWCMD = "rt/lowcmd" +TOPIC_LOWCMD = config.LOWCMD_TOPIC or ("rt/user_lowcmd" if config.ROBOT == "g1" else "rt/lowcmd") TOPIC_LOWSTATE = "rt/lowstate" TOPIC_HIGHSTATE = "rt/sportmodestate" TOPIC_WIRELESS_CONTROLLER = "rt/wirelesscontroller" @@ -87,6 +87,7 @@ def __init__(self, mj_model, mj_data): self.low_cmd_suber = ChannelSubscriber(TOPIC_LOWCMD, LowCmd_) self.low_cmd_suber.Init(self.LowCmdHandler, 10) + print(f"[Info] Subscribing LowCmd on {TOPIC_LOWCMD}") # joystick self.key_map = {