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..030565b0 100644 --- a/simulate/config.yaml +++ b/simulate/config.yaml @@ -1,14 +1,15 @@ -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 +lowcmd_topic: "" # Empty uses robot default: G1 -> rt/user_lowcmd, others -> rt/lowcmd -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/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 7c13289a..59e6c4d4 100644 --- a/simulate/src/unitree_sdk2_bridge.h +++ b/simulate/src/unitree_sdk2_bridge.h @@ -8,11 +8,16 @@ #include #include #include +#include +#include +#include #include +#include #include "param.h" #include "physics_joystick.h" +#include "keyboard_joystick.h" #define MOTOR_SENSOR_NUM 3 @@ -31,6 +36,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 +46,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() {} @@ -155,14 +170,22 @@ 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 { + 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() @@ -256,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()); @@ -273,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 @@ -320,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_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/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 = { 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 d0ec042b..c6e29dbc 100644 Binary files a/unitree_robots/go2w/assets/terrain.stl and b/unitree_robots/go2w/assets/terrain.stl differ