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/5] 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/5] 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/5] 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 From 3dab02c2f93196d095d2380dd8a7409f5de20f64 Mon Sep 17 00:00:00 2001 From: heyh003 <3161787087@qq.com> Date: Fri, 19 Dec 2025 16:54:25 +0800 Subject: [PATCH 4/5] update --- src/humanoid_loco_mujoco/README.md | 85 +++++++++++++++++++---- src/humanoid_loco_mujoco/ros2/package.xml | 23 ++++++ src/humanoid_loco_mujoco/ros2/setup.cfg | 4 ++ src/humanoid_loco_mujoco/ros2/setup.py | 27 +++++++ 4 files changed, 124 insertions(+), 15 deletions(-) create mode 100644 src/humanoid_loco_mujoco/ros2/package.xml create mode 100644 src/humanoid_loco_mujoco/ros2/setup.cfg create mode 100644 src/humanoid_loco_mujoco/ros2/setup.py diff --git a/src/humanoid_loco_mujoco/README.md b/src/humanoid_loco_mujoco/README.md index 05ed6f7876..31d23c566e 100644 --- a/src/humanoid_loco_mujoco/README.md +++ b/src/humanoid_loco_mujoco/README.md @@ -1,4 +1,4 @@ -# _LocoMuJoCo_ +# LocoMuJoCo的介绍 LocoMuJoCo 是一款专为全身控制而设计的模仿学习基准。 它具有多种环境,包括四足动物、类人生物和(肌肉)骨骼人体模型, 每个都提供了全面的数据集(每个人形机器人超过 22,000 个样本)。 @@ -7,7 +7,7 @@ LocoMuJoCo 是一款专为全身控制而设计的模仿学习基准。 支持多种生物形态的运动开发,包括四足动物(如狗)、两足动物(如人类)以及肌肉骨骼驱动的人体模型。 内置真实世界数据(如动作捕捉数据、地面实测数据),算法可直接基于这些数据学习 “如何像生物一样行走、奔跑”。 -# _主要优势_ +## 主要优势 - 支持 MuJoCo(单一环境)和 MJX(并行环境) - 包括 12 个人形环境和 4 个四足环境,具有 4 个生物力学人体模型 - 干净的单文件 JAX 算法,用于快速基准测试(PPO、GAIL、AMP、DeepMimic) @@ -19,14 +19,16 @@ LocoMuJoCo 是一款专为全身控制而设计的模仿学习基准。 - 接口中 内置域和地形随机化 - 模块化设计:定义、交换和重用组件,如观察类型、奖励函数、终端状态处理程序和域随机化文档 -# _安装_ +# 安装说明 + +## 安装 安装 MuJoCo 并设置环境变量。 ``` cd loco-mujoco pip install -e . ``` -# _依赖_ +## 依赖 - numpy<2.0 - scipy>=1.14.0 @@ -36,24 +38,77 @@ pip install -e . - flax - gymnasium -# _模型地址_ +## 数据安装说明 + +### 1.模型地址 https://github.com/robfiras/loco-mujoco/tree/master/loco_mujoco/models/unitree_h1 -# _数据集百度网盘地址_ +### 2.数据集百度网盘地址 链接: https://pan.baidu.com/s/1Viqlg9VYZBuKgj7TiVo-Tw 提取码: 68aa 下载好的数据集放在当前文件夹下 -# _文件说明_ +# 文件使用说明 +## 文件结构 ``` locomujoco -├── ros2 #ros2接口 -│ ├── mujoco_ros2_sim # 使用mujoco进行的强化学习训练 -│ └── robot_controller # 控制节点 -├── main # 使用locomujoco的简单示例 -├── mujoco #使用mujoco的简单实例 -├── robot_RL # 强化学习算法 - -``` \ No newline at end of file +├── ros2 # ros2接口 +│ ├── launch # launch文件# 使用mujoco进行的强化学习训练 +│ ├── mujoco_ros2_cpp # c++_ros2封装 +│ | ├── mujoco_ros2_sim.cpp # mujoco与ros2的通信 +│ | └─── robot_control.cpp # 控制节点 +| └── mujoco_ros2_py # py_ros2封装 +│ ├── mujoco_ros2_sim.py # mujoco与ros2的通信 +│ └─── robot_control.py # 控制节点 +├── main.py # 使用locomujoco的简单示例 +├── mujoco.py #使用mujoco的简单实例 +├── robot_RL.py # 强化学习算法 + +``` + +## 文件使用 + +### 1.运行locomujoco的简单示例 + +``` +python main.py +``` +### 2.运行mujoco简单示例 + +``` +python mujoco.py +``` +### 3.运行强化学习行走 + +``` +python robot_RL.py +``` + +## MuJoCo ROS2 文件使用说明 + +### 1.创建功能包 + +``` +cd ros2/src +ros2 pkg create mujoco_ros2 --build-type ament_python --dependencies rclpy my_interfaces +``` + +### 2.运行mujoco_ros2_sim节点 + +``` +ros2 run mujoco_ros2 mujoco_ros2_sim +``` + +### 3.运行robot_control节点 + +``` +ros2 run mujoco_ros2 robot_control +``` + +# 项目参考 + +- [MuJoCo](https://github.com/deepmind/mujoco) - 高性能物理引擎 +- [ROS 2](https://github.com/ros2) - 机器人操作系统 +- [LocoMuJoCo](https://loco-mujoco.readthedocs.io) - locomujoco官方参考文档 \ No newline at end of file diff --git a/src/humanoid_loco_mujoco/ros2/package.xml b/src/humanoid_loco_mujoco/ros2/package.xml new file mode 100644 index 0000000000..007dc76183 --- /dev/null +++ b/src/humanoid_loco_mujoco/ros2/package.xml @@ -0,0 +1,23 @@ + + + + mujoco_ros2 + 0.0.0 + TODO: Package description + heyh + TODO: License declaration + + rclpy + std_msg + my_interfaces + + ament_copyright + ament_flake8 + ament_pep257 + ament_xmllint + python3-pytest + + + ament_python + + diff --git a/src/humanoid_loco_mujoco/ros2/setup.cfg b/src/humanoid_loco_mujoco/ros2/setup.cfg new file mode 100644 index 0000000000..a0703142ca --- /dev/null +++ b/src/humanoid_loco_mujoco/ros2/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/mujoco_ros2 +[install] +install_scripts=$base/lib/mujoco_ros2 diff --git a/src/humanoid_loco_mujoco/ros2/setup.py b/src/humanoid_loco_mujoco/ros2/setup.py new file mode 100644 index 0000000000..b4c5817d8f --- /dev/null +++ b/src/humanoid_loco_mujoco/ros2/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = 'mujoco_ros2' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='heyh', + maintainer_email='heyh@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'mujoco_ros2_sim=mujoco_ros2.mujoco_ros2_sim:main', + 'robot_controller=mujoco_ros2.robot_controller:main', + ], + }, +) From a8af58c3ce24b4361e9b3036e44437bb27e20f5b Mon Sep 17 00:00:00 2001 From: heyh003 <3161787087@qq.com> Date: Mon, 22 Dec 2025 10:07:24 +0800 Subject: [PATCH 5/5] update --- src/humanoid_loco_mujoco/main.py | 161 +++++++++++++++++++++++++------ 1 file changed, 131 insertions(+), 30 deletions(-) diff --git a/src/humanoid_loco_mujoco/main.py b/src/humanoid_loco_mujoco/main.py index 8d2b980da3..6e87d037c5 100644 --- a/src/humanoid_loco_mujoco/main.py +++ b/src/humanoid_loco_mujoco/main.py @@ -1,6 +1,129 @@ +from typing import List, Any, Union, Tuple, Dict +from types import ModuleType + import numpy as np -from loco_mujoco.task_factories import ImitationFactory, LAFAN1DatasetConf, DefaultDatasetConf, AMASSDatasetConf -from loco_mujoco.core import ObservationType +import jax +import jax.numpy as jnp +from flax import struct +from dataclasses import asdict + +from mujoco import MjSpec, MjModel, MjData +from mujoco.mjx import Model, Data + +import numpy as np +from loco_mujoco.task_factories import ImitationFactory, DefaultDatasetConf +from loco_mujoco.environments import UnitreeG1 + +from loco_mujoco.core.observations import Observation, StatefulObservation +from loco_mujoco.core.control_functions.pd import PDControl, PDControlState + + +# ----- CUSTOM ENVIRONMENT ----- +# custom environment class with fixed waist yaw joint +class CustomUnitreeG1(UnitreeG1): + def __init__(self, spec=None, **kwargs): + self._waist_yaw_joint_name = "waist_yaw_joint" + self._waist_yaw_actuator_name = "waist_yaw" + super().__init__(spec=spec or self.get_default_xml_file_path(), **kwargs) + + @staticmethod + def fix_waist_yaw(spec: MjSpec): + spec.joints = [j for j in spec.joints if j.name != "waist_yaw_joint"] + + def _get_observation_specification(self, spec: MjSpec) -> List[Observation]: + obs = super()._get_observation_specification(spec) + return [o for o in obs if o.xml_name != self._waist_yaw_joint_name] + + def _get_action_specification(self, spec: MjSpec) -> List[str]: + actions = super()._get_action_specification(spec) + return [a for a in actions if a != self._waist_yaw_actuator_name] + +# ----- CUSTOM CONTROL FUNCTION ----- +@struct.dataclass +class CustomControlFunctionState(PDControlState): + moving_average: Union[np.ndarray, jnp.ndarray] + +class CustomControlFunction(PDControl): + def generate_action(self, env, action, model, data, carry, backend): + orig_action, carry = super().generate_action(env, action, model, data, carry, backend) + ma = 0.99 * carry.control_func_state.moving_average + 0.01 * orig_action + state = carry.control_func_state.replace(moving_average=ma) + carry = carry.replace(control_func_state=state) + return ma, carry + + def init_state(self, env, key, model, data, backend): + orig_state = super().init_state(env, key, model, data, backend) + dim = env.info.action_space.shape[0] + ma = backend.zeros_like(dim) + return CustomControlFunctionState(moving_average=ma, **asdict(orig_state)) + + + +# ----- CUSTOM OBSERVATIONS ----- +# custom observation class to observe the center of mass position of the pelvis +class CustomBodyCOMPos(Observation): + dim = 3 + + def __init__(self, name: str, xml_name: str): + self.xml_name = xml_name + super().__init__(name) + + def _init_from_mj(self, env, model, data, current_obs_size): + self.min, self.max = [-np.inf] * self.dim, [np.inf] * self.dim + self.data_type_ind = np.array(self.to_list(data.body(self.xml_name).id)) + self.obs_ind = np.arange(current_obs_size, current_obs_size + self.dim) + self._initialized_from_mj = True + + @classmethod + def data_type(cls): + return "xipos" + + +# custom observation class to observe the moving average of the center of mass position of the pelvis +@struct.dataclass +class CustomBodyCOMPosMovingAverageState: + moving_average: Union[np.ndarray, jnp.ndarray] + +class CustomBodyCOMPosMovingAverage(StatefulObservation): + dim = 3 + + def __init__(self, name: str, xml_name: str): + self.xml_name = xml_name + super().__init__(name) + + def _init_from_mj(self, env, model, data, current_obs_size): + self.min, self.max = [-np.inf] * self.dim, [np.inf] * self.dim + self.obs_ind = np.arange(current_obs_size, current_obs_size + self.dim) + self.data_type_ind = np.array(self.to_list(data.body(self.xml_name).id)) + self._initialized_from_mj = True + + def init_state(self, env, key, model, data, backend): + return CustomBodyCOMPosMovingAverageState(moving_average=backend.zeros(self.dim)) + + def get_obs_and_update_state(self, env, model, data, carry, backend): + obs_states = carry.observation_states + obs_state = getattr(obs_states, self.name) + xipos = backend.squeeze(data.xipos[self.data_type_ind]) + ma = 0.9 * obs_state.moving_average + 0.1 * xipos + obs_states = obs_states.replace(**{self.name: obs_state.replace(moving_average=ma)}) + carry = carry.replace(observation_states=obs_states) + return backend.ravel(ma), carry + + +# ----- REGISTRATION ----- +CustomUnitreeG1.register() +CustomControlFunction.register() +CustomBodyCOMPos.register() +CustomBodyCOMPosMovingAverage.register() + + +# ----- ENVIRONMENT SETUP ----- +observation_spec = [ + CustomBodyCOMPos("pelvis_com", "pelvis"), + CustomBodyCOMPosMovingAverage("pelvis_com_mov_avg", "pelvis"), +] + + randomization_config = { # gravity "randomize_gravity": True, @@ -61,41 +184,19 @@ "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", +env = ImitationFactory.make("UnitreeG1", default_dataset_conf=DefaultDatasetConf(["squat", "walk"]), 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), + control_type="CustomControlFunction", + control_params={"p_gain": 100, "d_gain": 1}, n_substeps=20) -env.play_trajectory(n_episodes=3, n_steps_per_episode=500, render=True) +env.play_trajectory(n_episodes=3, n_steps_per_episode=500, render=True) \ No newline at end of file