From 0e8baad7369c525998d33d438256babdc5b6e90e Mon Sep 17 00:00:00 2001 From: heyh003 <3161787087@qq.com> Date: Thu, 18 Dec 2025 22:06:25 +0800 Subject: [PATCH 1/3] update --- src/humanoid_loco_mujoco/README.md | 14 +- src/humanoid_loco_mujoco/main.py | 97 +++++++++++- .../ros2/mujoco_ros2_sim.cpp | 139 ++++++++++++++++++ .../ros2/robot_controller.py | 95 ++++++++++++ 4 files changed, 335 insertions(+), 10 deletions(-) create mode 100644 src/humanoid_loco_mujoco/ros2/mujoco_ros2_sim.cpp create mode 100644 src/humanoid_loco_mujoco/ros2/robot_controller.py diff --git a/src/humanoid_loco_mujoco/README.md b/src/humanoid_loco_mujoco/README.md index 18af99e349..05ed6f7876 100644 --- a/src/humanoid_loco_mujoco/README.md +++ b/src/humanoid_loco_mujoco/README.md @@ -47,7 +47,13 @@ https://github.com/robfiras/loco-mujoco/tree/master/loco_mujoco/models/unitree_h # _文件说明_ -- `main`:使用locomujoco的简单示例 -- `mujoco`:使用mujoco的简单实例 -- `robot_RL`:使用mujoco进行的强化学习训练 -- `mujoco_ros2_sim`:使用mujoco和ros2进行仿真 \ No newline at end of file +``` +locomujoco +├── ros2 #ros2接口 +│ ├── mujoco_ros2_sim # 使用mujoco进行的强化学习训练 +│ └── robot_controller # 控制节点 +├── main # 使用locomujoco的简单示例 +├── mujoco #使用mujoco的简单实例 +├── robot_RL # 强化学习算法 + +``` \ No newline at end of file diff --git a/src/humanoid_loco_mujoco/main.py b/src/humanoid_loco_mujoco/main.py index 658c36a02e..8d2b980da3 100644 --- a/src/humanoid_loco_mujoco/main.py +++ b/src/humanoid_loco_mujoco/main.py @@ -1,16 +1,101 @@ import numpy as np from loco_mujoco.task_factories import ImitationFactory, LAFAN1DatasetConf, DefaultDatasetConf, AMASSDatasetConf +from loco_mujoco.core import ObservationType +randomization_config = { + # gravity + "randomize_gravity": True, + "gravity_range": [9.51, 10.11], + # geom properties + "randomize_geom_friction_tangential": True, + "geom_friction_tangential_range": [0.8, 1.2], + "randomize_geom_friction_torsional": True, + "geom_friction_torsional_range": [0.003, 0.007], + "randomize_geom_friction_rolling": True, + "geom_friction_rolling_range": [0.00008, 0.00012], + "randomize_geom_damping": True, + "geom_damping_range": [72, 88], + "randomize_geom_stiffness": True, + "geom_stiffness_range": [900, 1100], + + # joint properties + "randomize_joint_damping": True, + "joint_damping_range": [0.3, 1.5], + "randomize_joint_stiffness": True, + "joint_stiffness_range": [0.9, 1.1], + "randomize_joint_friction_loss": True, + "joint_friction_loss_range": [0.0, 0.2], + "randomize_joint_armature": True, + "joint_armature_range": [0.08, 0.12], + + # base mass + "randomize_base_mass": True, + "base_mass_to_add_range": [-2.0, 2.0], + + # COM + "randomize_com_displacement": True, + "com_displacement_range": [-0.15, 0.15], + + # link mass + "randomize_link_mass": True, + "link_mass_multiplier_range": { + "root_body": [0.5, 1.9], + "other_bodies": [0.8, 1.2], + }, + + # PD Gains (if PDControl is used) + "add_p_gains_noise": True, + "add_d_gains_noise": True, + "p_gains_noise_scale": 0.1, + "d_gains_noise_scale": 0.1, + + # Observation Noise + "add_joint_pos_noise": True, + "joint_pos_noise_scale": 0.003, + "add_joint_vel_noise": True, + "joint_vel_noise_scale": 0.08, + "add_gravity_noise": True, + "gravity_noise_scale": 0.015, + "add_free_joint_lin_vel_noise": True, + "lin_vel_noise_scale": 0.1, + "add_free_joint_ang_vel_noise": True, + "ang_vel_noise_scale": 0.02, +} +observation_spec = [ + # prioritized observations + ObservationType.FreeJointPosNoXY(obs_name="free_joint", xml_name="root", group="prioritized", allow_randomization=False), + ObservationType.FreeJointVel(obs_name="free_joint_vel", xml_name="root", group="prioritized", allow_randomization=False), + # 修正左髋关节名称(之前的KeyError问题) + ObservationType.JointPos(obs_name="joint_pos", xml_name="hip_flexion_l", group="prioritized", allow_randomization=False), + # 修正右髋关节名称 + ObservationType.JointVel(obs_name="joint_vel1", xml_name="hip_flexion_r", group="prioritized", allow_randomization=False), + # 修正左膝关节名称 + ObservationType.JointVel(obs_name="joint_vel2", xml_name="knee_angle_l", group="prioritized", allow_randomization=False), + # 将head替换为有效的身体名称(例如torso_link) + ObservationType.BodyPos(obs_name="head_pos", xml_name="torso_link", group="prioritized", allow_randomization=False), + # policy observations --> these will be noisy + ObservationType.ProjectedGravityVector(obs_name="proj_grav", xml_name="root", group="policy", allow_randomization=True), + ObservationType.FreeJointVel(obs_name="free_joint_vel_pi", xml_name="root", group="policy", allow_randomization=True), + # 修正左髋关节名称 + ObservationType.JointPos(obs_name="joint_pos_pi", xml_name="hip_flexion_l", group="policy", allow_randomization=True), + # 修正右髋关节名称 + ObservationType.JointVel(obs_name="joint_vel1_pi", xml_name="hip_flexion_r", group="policy", allow_randomization=True), + # 修正左膝关节名称 + ObservationType.JointVel(obs_name="joint_vel2_pi", xml_name="knee_angle_l", group="policy", allow_randomization=True), + # 将head替换为有效的身体名称(例如torso_link) + ObservationType.BodyPos(obs_name="head_pos_pi", xml_name="torso_link", group="policy", allow_randomization=True), + ObservationType.LastAction(obs_name="last_action_pi", group="policy", allow_randomization=True) +] # # example --> you can add as many datasets as you want in the lists! env = ImitationFactory.make("UnitreeH1", default_dataset_conf=DefaultDatasetConf(["squat", "walk"]), - lafan1_dataset_conf=LAFAN1DatasetConf(["dance2_subject4"]), - # if SMPL and AMASS are installed, you can use the following: - # amass_dataset_conf=AMASSDatasetConf(["DanceDB/DanceDB/20120911_TheodorosSourmelis/Capoeira_Theodoros_v2_C3D_poses", - # "KIT/12/WalkInClockwiseCircle11_poses", - # "HUMAN4D/HUMAN4D/Subject3_Medhi/INF_JumpingJack_S3_01_poses", - # 'KIT/359/walking_fast05_poses']), + terrain_type="RoughTerrain", + terrain_params=dict(random_min_height=-0.1,random_max_height=0.1,random_downsampled_scale=0.5), + domain_randomization_type="DefaultRandomizer", + domain_randomization_params=randomization_config, + observation_spec=observation_spec, + control_type="PDControl", control_params=dict(p_gain=100, d_gain=1), n_substeps=20) env.play_trajectory(n_episodes=3, n_steps_per_episode=500, render=True) diff --git a/src/humanoid_loco_mujoco/ros2/mujoco_ros2_sim.cpp b/src/humanoid_loco_mujoco/ros2/mujoco_ros2_sim.cpp new file mode 100644 index 0000000000..8965ba04e4 --- /dev/null +++ b/src/humanoid_loco_mujoco/ros2/mujoco_ros2_sim.cpp @@ -0,0 +1,139 @@ +#include "rclcpp/rclcpp.hpp" +#include "my_interfaces/srv/robot_ctrl.hpp" +#include "my_interfaces/msg/robot_state.hpp" +#include +#include +#include +#include + +// 全局变量 +std::string current_action = "stop"; + +class MujocoRos2Sim : public rclcpp::Node { +public: + MujocoRos2Sim() : Node("mujoco_ros2_sim") { + RCLCPP_INFO(this->get_logger(), "节点已启动:mujoco_ros2_sim!"); + + // 加载模型 + char error[1000]; + model_ = mj_loadXML("RobotH.xml", nullptr, error, 1000); + if (!model_) { + RCLCPP_ERROR(this->get_logger(), "模型加载失败: %s", error); + exit(1); + } + data_ = mj_makeData(model_); + + // 加载轨迹数据(这里简化处理,实际需要根据npz格式解析) + load_trajectory_data(); + + // 创建viewer + viewer_ = mjViewerCreate(); + mjv_defaultCamera(&cam_); + mjv_defaultOption(&opt_); + mjr_defaultContext(&con_); + mjv_makeScene(model_, &scene_, 1000); + mjr_makeContext(model_, &con_, mjFONTSCALE_150); + mjViewerSetup(viewer_, &scene_, &cam_, &opt_, &con_); + + // 创建服务 + service_ = this->create_service( + "robot_control", + std::bind(&MujocoRos2Sim::control_callback, this, std::placeholders::_1, std::placeholders::_2) + ); + + // 创建发布者 + publisher_ = this->create_publisher("robot_state", 10); + + // 启动模拟线程 + sim_thread_ = std::thread(&MujocoRos2Sim::run_sim, this); + } + + ~MujocoRos2Sim() { + if (sim_thread_.joinable()) { + sim_thread_.join(); + } + mjViewerClose(viewer_); + mjr_freeContext(&con_); + mjv_freeScene(&scene_); + mj_deleteData(data_); + mj_deleteModel(model_); + } + +private: + mjModel* model_ = nullptr; + mjData* data_ = nullptr; + mjViewer* viewer_ = nullptr; + mjvScene scene_; + mjvCamera cam_; + mjvOption opt_; + mjrContext con_; + rclcpp::Service::SharedPtr service_; + rclcpp::Publisher::SharedPtr publisher_; + std::thread sim_thread_; + + // 轨迹数据(示例) + std::vector walk_qpos, walk_qvel; + std::vector squat_qpos, squat_qvel; + + void load_trajectory_data() { + // 实际应用中需要解析npz文件 + // 这里仅做示例初始化 + walk_qpos.push_back(0.0); + walk_qvel.push_back(0.1); + squat_qpos.push_back(-0.5); + squat_qvel.push_back(0.0); + } + + void control_callback( + const std::shared_ptr request, + std::shared_ptr response + ) { + RCLCPP_INFO(this->get_logger(), "收到指令: %s", request->command.c_str()); + current_action = request->command; + response->result = true; + response->message = "指令已接收"; + } + + void publish_status() { + auto msg = my_interfaces::msg::RobotState(); + msg.action = current_action; + publisher_->publish(msg); + } + + void run_sim() { + while (rclcpp::ok()) { + // 控制动作 + if (current_action == "squat" && !squat_qpos.empty()) { + data_->qpos[0] = squat_qpos[0]; + data_->qvel[0] = squat_qvel[0]; + } else if (current_action == "walk" && !walk_qpos.empty()) { + data_->qpos[0] = walk_qpos[0]; + data_->qvel[0] = walk_qvel[0]; + } else if (current_action == "stop") { + data_->qpos[0] = 0; + data_->qvel[0] = 0; + } + + // 模拟一步 + mj_step(model_, data_); + + // 渲染 + mjv_updateScene(model_, data_, &opt_, nullptr, &cam_, mjCAT_ALL, &scene_); + mjViewerRender(viewer_, &scene_, &con_); + + // 发布状态 + publish_status(); + + // 小延迟 + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } +}; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/humanoid_loco_mujoco/ros2/robot_controller.py b/src/humanoid_loco_mujoco/ros2/robot_controller.py new file mode 100644 index 0000000000..43dcf9b4ef --- /dev/null +++ b/src/humanoid_loco_mujoco/ros2/robot_controller.py @@ -0,0 +1,95 @@ +import sys +import select +import rclpy +from rclpy.node import Node +from my_interfaces.srv import RobotCtrl + +# 全局变量(提前初始化,避免未绑定) +current_action_key = 'stop' +action_switch_flag = False + + +class RobotController(Node): + def __init__(self, name): + super().__init__(name) + # 创建服务客户端,连接机器人控制服务 + self.client = self.create_client(RobotCtrl, 'robot_control') + # 等待服务上线 + while not self.client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('等待服务 "robot_control" 启动...') + self.get_logger().info("操作提示:w=行走 | s=下蹲 | q=退出 | 其他键=停止") + + def result_callback(self, future): + """服务调用结果的回调函数""" + + response = future.result() + self.get_logger().info(f'指令执行成功:{response.message}') + + + def send_request(self, action): + """发送控制指令到服务端""" + + request = RobotCtrl.Request() + request.command = action # 传递动作指令 + self.client.call_async(request).add_done_callback(self.result_callback) + self.get_logger().info(f'已发送指令:{action}') + + + +def read_keypress_non_blocking(): + """非阻塞读取终端按键""" + if select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []): + key = sys.stdin.read(1) + return key.strip() + return None + + +def main(args=None): + # 关键:声明使用全局变量(修复核心错误) + global current_action_key, action_switch_flag + + rclpy.init(args=args) + robot_controller = RobotController("robot_controller") + + # 初始发送停止指令(此时current_action_key已绑定全局值'stop') + robot_controller.send_request(current_action_key) + + try: + # 主循环:非阻塞读取按键 + 处理ROS事件 + while rclpy.ok(): + # 1. 读取终端按键 + key = read_keypress_non_blocking() + if key: + # 按键逻辑 + if key == 'w' and current_action_key != "walk": + current_action_key = "walk" + action_switch_flag = True + elif key == 's' and current_action_key != "squat": + current_action_key = "squat" + action_switch_flag = True + elif key == 'q': # q键退出 + robot_controller.get_logger().info("收到退出指令,程序终止") + break + elif key not in ['w', 's']: # 其他键停止 + if current_action_key != "stop": + current_action_key = "stop" + action_switch_flag = True + + # 2. 检测动作切换并发送指令 + if action_switch_flag: + robot_controller.send_request(current_action_key) + action_switch_flag = False # 重置标记 + + # 3. 非阻塞处理ROS回调 + rclpy.spin_once(robot_controller, timeout_sec=0.01) + + except KeyboardInterrupt: + robot_controller.get_logger().info("用户强制退出") + finally: + robot_controller.destroy_node() + rclpy.shutdown() + print("程序已退出") + + +if __name__ == '__main__': + main() From bdc3717b6f0be94cbb8eb9c3c53de7421b423580 Mon Sep 17 00:00:00 2001 From: heyh003 <3161787087@qq.com> Date: Fri, 19 Dec 2025 09:25:44 +0800 Subject: [PATCH 2/3] update --- .../ros2/robot_controller.cpp | 121 ++++++++++++++++++ 1 file changed, 121 insertions(+) create mode 100644 src/humanoid_loco_mujoco/ros2/robot_controller.cpp diff --git a/src/humanoid_loco_mujoco/ros2/robot_controller.cpp b/src/humanoid_loco_mujoco/ros2/robot_controller.cpp new file mode 100644 index 0000000000..5080713bcf --- /dev/null +++ b/src/humanoid_loco_mujoco/ros2/robot_controller.cpp @@ -0,0 +1,121 @@ +#include "rclcpp/rclcpp.hpp" +#include "my_interfaces/srv/robot_ctrl.hpp" +#include +#include +#include +#include + +// 全局变量 +std::string current_action_key = "stop"; +bool action_switch_flag = false; + +// 非阻塞键盘读取 +char read_keypress_non_blocking() { + struct termios oldt, newt; + int ch; + int oldf; + + // 获取终端设置 + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + // 禁用 canonical 模式和回显 + newt.c_lflag &= ~(ICANON | ECHO); + tcsetattr(STDIN_FILENO, TCSANOW, &newt); + // 设置非阻塞模式 + oldf = fcntl(STDIN_FILENO, F_GETFL, 0); + fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); + + ch = getchar(); + + // 恢复终端设置 + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + fcntl(STDIN_FILENO, F_SETFL, oldf); + + return (ch != EOF) ? ch : 0; +} + +class RobotController : public rclcpp::Node { +public: + RobotController() : Node("robot_controller") { + // 创建服务客户端 + client_ = this->create_client("robot_control"); + + // 等待服务可用 + while (!client_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "等待服务时被中断"); + return; + } + RCLCPP_INFO(this->get_logger(), "等待服务 'robot_control' 启动..."); + } + + RCLCPP_INFO(this->get_logger(), "操作提示:w=行走 | s=下蹲 | q=退出 | 其他键=停止"); + } + + void send_request(const std::string &action) { + auto request = std::make_shared(); + request->command = action; + + // 发送异步请求 + auto result_future = client_->async_send_request( + request, + std::bind(&RobotController::result_callback, this, std::placeholders::_1) + ); + RCLCPP_INFO(this->get_logger(), "已发送指令:%s", action.c_str()); + } + +private: + rclcpp::Client::SharedPtr client_; + + void result_callback(rclcpp::Client::SharedFuture future) { + auto response = future.get(); + RCLCPP_INFO(this->get_logger(), "指令执行成功:%s", response->message.c_str()); + } +}; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + // 初始发送停止指令 + node->send_request(current_action_key); + + try { + while (rclcpp::ok()) { + // 读取键盘输入 + char key = read_keypress_non_blocking(); + if (key) { + if (key == 'w' && current_action_key != "walk") { + current_action_key = "walk"; + action_switch_flag = true; + } else if (key == 's' && current_action_key != "squat") { + current_action_key = "squat"; + action_switch_flag = true; + } else if (key == 'q') { // 退出 + RCLCPP_INFO(node->get_logger(), "收到退出指令,程序终止"); + break; + } else if (key != 'w' && key != 's') { // 其他键停止 + if (current_action_key != "stop") { + current_action_key = "stop"; + action_switch_flag = true; + } + } + } + + // 发送指令 + if (action_switch_flag) { + node->send_request(current_action_key); + action_switch_flag = false; + } + + // 处理ROS事件 + rclcpp::spin_some(node); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } catch (const std::exception &e) { + RCLCPP_ERROR(node->get_logger(), "异常:%s", e.what()); + } + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file From 2a6c1b364942080b6e854a965524af330dcae48c Mon Sep 17 00:00:00 2001 From: heyh003 <3161787087@qq.com> Date: Fri, 19 Dec 2025 12:45:57 +0800 Subject: [PATCH 3/3] update --- .../ros2/launch/mujoco_ros2.launch.py | 21 +++++++++++++++++++ .../{ => mujoco_ros2_cpp}/mujoco_ros2_sim.py | 0 .../robot_controller.cpp | 0 .../{ => mujoco_ros2_py}/mujoco_ros2_sim.cpp | 0 .../{ => mujoco_ros2_py}/robot_controller.py | 0 5 files changed, 21 insertions(+) create mode 100644 src/humanoid_loco_mujoco/ros2/launch/mujoco_ros2.launch.py rename src/humanoid_loco_mujoco/ros2/{ => mujoco_ros2_cpp}/mujoco_ros2_sim.py (100%) rename src/humanoid_loco_mujoco/ros2/{ => mujoco_ros2_cpp}/robot_controller.cpp (100%) rename src/humanoid_loco_mujoco/ros2/{ => mujoco_ros2_py}/mujoco_ros2_sim.cpp (100%) rename src/humanoid_loco_mujoco/ros2/{ => mujoco_ros2_py}/robot_controller.py (100%) diff --git a/src/humanoid_loco_mujoco/ros2/launch/mujoco_ros2.launch.py b/src/humanoid_loco_mujoco/ros2/launch/mujoco_ros2.launch.py new file mode 100644 index 0000000000..c73d6ae174 --- /dev/null +++ b/src/humanoid_loco_mujoco/ros2/launch/mujoco_ros2.launch.py @@ -0,0 +1,21 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='mujoco_ros2', + executable='mujoco_ros2_sim', + name='mujoco_ros2_sim', + output='screen', + parameters=[], + ), + + Node( + package='mujoco_ros2', + executable='robot_controller', + name='robot_controller', + output='screen', + parameters=[], + ) + ]) \ No newline at end of file diff --git a/src/humanoid_loco_mujoco/ros2/mujoco_ros2_sim.py b/src/humanoid_loco_mujoco/ros2/mujoco_ros2_cpp/mujoco_ros2_sim.py similarity index 100% rename from src/humanoid_loco_mujoco/ros2/mujoco_ros2_sim.py rename to src/humanoid_loco_mujoco/ros2/mujoco_ros2_cpp/mujoco_ros2_sim.py diff --git a/src/humanoid_loco_mujoco/ros2/robot_controller.cpp b/src/humanoid_loco_mujoco/ros2/mujoco_ros2_cpp/robot_controller.cpp similarity index 100% rename from src/humanoid_loco_mujoco/ros2/robot_controller.cpp rename to src/humanoid_loco_mujoco/ros2/mujoco_ros2_cpp/robot_controller.cpp diff --git a/src/humanoid_loco_mujoco/ros2/mujoco_ros2_sim.cpp b/src/humanoid_loco_mujoco/ros2/mujoco_ros2_py/mujoco_ros2_sim.cpp similarity index 100% rename from src/humanoid_loco_mujoco/ros2/mujoco_ros2_sim.cpp rename to src/humanoid_loco_mujoco/ros2/mujoco_ros2_py/mujoco_ros2_sim.cpp diff --git a/src/humanoid_loco_mujoco/ros2/robot_controller.py b/src/humanoid_loco_mujoco/ros2/mujoco_ros2_py/robot_controller.py similarity index 100% rename from src/humanoid_loco_mujoco/ros2/robot_controller.py rename to src/humanoid_loco_mujoco/ros2/mujoco_ros2_py/robot_controller.py