diff --git a/src/Neuro_Mujoco/README.md b/src/Neuro_Mujoco/README.md index 280721adb1..7cae468cb6 100644 --- a/src/Neuro_Mujoco/README.md +++ b/src/Neuro_Mujoco/README.md @@ -1,169 +1,126 @@ -# 基于 MuJoCo 的神经网络代理实现 -实现基于 MuJoCo 物理引擎的机器人、机械结构等智能体的感知、规划与控制,结合神经网络实现动态环境中的自主决策与行为生成。新增**强化学习策略推理**与**ROS 1 实时通信**功能,支持端到端的智能控制流程。 - -# 环境配置 -平台:Windows 10/11,Ubuntu 20.04/22.04(ROS推荐),macOS(Intel/Apple Silicon) -软件:Python 3.7-3.12(需支持 3.7 及以上版本)、PyTorch(策略网络依赖) -核心依赖:MuJoCo 物理引擎、mujoco-python 绑定 - -# 基础依赖安装 -安装 Python 3.11(推荐版本) -安装核心依赖: -```shell -# 安装MuJoCo Python绑定 +# 基于 MuJoCo 的智能体控制工具 +整合 MuJoCo 物理引擎的模型加载、格式转换、速度测试、可视化功能,新增强化学习策略推理与ROS 1 实时通信能力,支持机器人端到端智能控制流程。 +## 环境配置 +1. 支持平台 +Windows 10/11、Ubuntu 20.04/22.04(ROS 推荐)、macOS(Intel/Apple Silicon) +2. 软件要求 +Python 3.7-3.12(推荐 3.11)、MuJoCo 物理引擎、PyTorch(策略网络依赖,可选)、ROS 1 Noetic(ROS 功能依赖,可选) +3. 核心依赖安装 +shell +## 安装 MuJoCo Python 绑定 pip install mujoco -i http://mirrors.aliyun.com/pypi/simple --trusted-host mirrors.aliyun.com -# 安装PyTorch(策略网络必需,根据系统配置选择) -pip3 install torch torchvision torchaudio -i http://mirrors.aliyun.com/pypi/simple --trusted-host mirrors.aliyun.com - -# 安装文档生成工具 -pip install mkdocs -i http://mirrors.aliyun.com/pypi/simple --trusted-host mirrors.aliyun.com -pip install -r requirements.txt -``` - -(可选)验证安装: -```shell -python -c "import mujoco; print('MuJoCo version:', mujoco.__version__)" -python -c "import torch; print('PyTorch version:', torch.__version__)" # 验证策略依赖 -mkdocs --version -``` - -# 核心功能使用 -## 1. 模型可视化与控制 -支持直接加载模型文件或目录,集成策略控制与ROS通信: -```shell +## PyTorch(策略网络推理必需) +pip install torch torchvision torchaudio -i https://mirrors.aliyun.com/pypi/simple/ --trusted-host mirrors.aliyun.com + +## ROS 1 依赖(仅 Ubuntu,需先安装 ROS Noetic) +sudo apt install ros-noetic-rospy ros-noetic-geometry-msgs ros-noetic-sensor-msgs +## 验证安装 +shell +### 验证 MuJoCo +python -c "import mujoco; print(f'MuJoCo 版本: {mujoco.__version__}')" + +# 验证 PyTorch(策略功能) +python -c "import torch; print(f'PyTorch 版本: {torch.__version__}')" + +### 验证 ROS(ROS 功能) +# python -c "import rospy; print('ROS 1 导入成功')" +## 快速开始 +### 模型可视化(核心功能) +支持加载单个模型文件或目录(自动识别目录内 .xml/.mjb 文件),控制优先级:ROS 指令 > 策略推理 > 无控制。 +1.1 运行命令 +shell # 基础可视化(无控制) -python mujoco_utils.py visualize /path/to/model.xml +python mujoco_utils.py visualize /path/to/your/model.xml -# 启用ROS 1模式(发布关节状态/基座姿态,接收控制指令) -python mujoco_utils.py visualize /path/to/model.xml --ros +# 启用 ROS 1 模式(发布关节状态/接收控制指令) +python mujoco_utils.py visualize /path/to/your/model.xml --ros # 加载预训练策略模型(自动生成控制指令) -python mujoco_utils.py visualize /path/to/model.xml --policy /path/to/policy.pth - -# 联合模式(ROS + 策略控制) -python mujoco_utils.py visualize /path/to/model.xml --ros --policy /path/to/policy.pth -``` -- 交互说明:可视化窗口支持鼠标拖拽旋转、滚轮缩放,按ESC键退出 -- 控制优先级:ROS指令 > 策略推理 > 无控制 - -## 2. 模型格式转换 -支持XML与MJB(二进制)格式互转(MJB加载速度更快): -```shell -# XML转MJB +python mujoco_utils.py visualize /path/to/your/model.xml --policy /path/to/your/policy.pth + +# 混合模式(ROS + 策略控制) +python mujoco_utils.py visualize /path/to/your/model.xml --ros --policy /path/to/your/policy.pth + +# 目录自动加载(选目录内第一个模型文件) +python mujoco_utils.py visualize /home/lan/桌面/nn/mujoco_menagerie/anybotics_anymal_b +1.2 交互说明 +窗口操作:鼠标拖拽旋转、滚轮缩放,按 ESC 键退出; +关节状态:仅发布非自由关节(排除 mjJNT_FREE 类型)的位置 / 速度; +策略映射:输出 [-1,1] 自动线性缩放至执行器 ctrlrange 范围,并强制裁剪至物理极限。 +### 模型格式转换 +2.1 运行命令 +shell +# XML 转 MJB(提升加载速度) python mujoco_utils.py convert input.xml output.mjb -# MJB转XML +# MJB 转 XML(便于编辑) python mujoco_utils.py convert input.mjb output.xml -``` - -## 3. 模拟速度测试 -多线程测试模型仿真性能,评估实时性: -```shell -# 默认参数(1线程,10000步) -python mujoco_utils.py testspeed /path/to/model.xml - -# 自定义配置(4线程,50000步,控制噪声0.02) -python mujoco_utils.py testspeed /path/to/model.xml --nthread 4 --nstep 50000 --ctrlnoise 0.02 -``` -输出指标包括:每秒步数、实时因子(仿真时间/真实时间)、线程耗时统计 - -# ROS 集成配置(Ubuntu 专属) -### 1. ROS 版本要求 -- Ubuntu 20.04 → ROS Noetic -- Ubuntu 22.04 → ROS Humble(需适配 Python 3.10+) - -### 2. 安装 ROS 依赖 -```shell -# ROS Noetic 依赖(Ubuntu 20.04) -sudo apt install ros-noetic-rospy ros-noetic-catkin ros-noetic-geometry-msgs ros-noetic-sensor-msgs -sudo apt install python3-catkin-tools python3-rosdep -``` - -### 3. Catkin 工作空间配置 -```shell -# 创建/进入 Catkin 工作空间(示例路径:~/桌面/nn/catkin_ws) -mkdir -p ~/桌面/nn/catkin_ws/src && cd ~/桌面/nn/catkin_ws/src - -# 将 mujoco_ros 功能包软链接到 Catkin 工作空间 -ln -s ~/桌面/nn/src/Neuro_Mujoco/mujoco_ros ./mujoco_ros - -# 初始化 rosdep(首次使用) -sudo rosdep init && rosdep update -rosdep install --from-paths ./ --ignore-src -r -y -``` - -### 4. 编译 Catkin 工作空间 -```shell -cd ~/桌面/nn/catkin_ws -# 清理旧编译缓存 -rm -rf build devel -# 编译(支持虚拟环境 Python) -catkin_make -DPYTHON_EXECUTABLE=$(which python3) -``` - - -# 策略网络说明 -## 网络结构 -轻量级多层感知器(MLP),适用于机器人关节控制: -```python +### 仿真速度测试 +多线程测试模型仿真性能,优化点:线程内动态生成控制噪声,降低内存占用。 +3.1 运行命令 +shell +# 默认参数(1线程,10000步,噪声0.01) +python mujoco_utils.py testspeed /path/to/your/model.xml + +# 自定义配置(4线程,50000步,噪声0.02) +python mujoco_utils.py testspeed /path/to/your/model.xml --nthread 4 --nstep 50000 --ctrlnoise 0.02 +3.2 输出指标 +总步数 / 总耗时 / 每秒步数; +实时因子(仿真时间 / 真实时间); +线程耗时统计(平均值 ± 标准差)。 +## ROS 1 集成(Ubuntu 专属) +1. ROS 话题说明 +类型 话题名 消息类型 说明 +发布 /mujoco/joint_states sensor_msgs/JointState 非自由关节位置 / 速度 +发布 /mujoco/pose geometry_msgs/PoseStamped 基座位置 / 姿态(world 帧) +订阅 /mujoco/ctrl_cmd std_msgs/Float32MultiArray 控制指令(长度 = 模型 nu) +2. 运行步骤 +shell +# 1. 启动 ROS 核心(新开终端) +roscore + +# 2. 启动 MuJoCo ROS 节点 +python mujoco_utils.py visualize /path/to/your/model.xml --ros + +# 3. (可选)发布测试控制指令(100Hz) +rostopic pub /mujoco/ctrl_cmd std_msgs/Float32MultiArray "data: [0.1, 0.2, 0.3]" -r 100 +## 策略网络说明 +1. 网络结构 +轻量级 MLP,适配机器人关节控制场景: +python +运行 class PolicyNetwork(nn.Module): def __init__(self, obs_dim: int, action_dim: int, hidden_dim: int = 64): super().__init__() self.net = nn.Sequential( - nn.Linear(obs_dim, hidden_dim), # 观测维度=关节位置数+速度数 - nn.Tanh(), - nn.Linear(hidden_dim, hidden_dim), - nn.Tanh(), - nn.Linear(hidden_dim, action_dim), # 输出维度=控制维度(nu) - nn.Tanh() # 输出范围[-1,1],自动映射到实际控制范围 + nn.Linear(obs_dim, hidden_dim), nn.Tanh(), + nn.Linear(hidden_dim, hidden_dim), nn.Tanh(), + nn.Linear(hidden_dim, action_dim), nn.Tanh() # 输出 [-1,1] ) -``` - -## 观测与控制映射 -- 输入观测:`[关节位置(qpos) + 关节速度(qvel)]` -- 输出控制:归一化指令`[-1,1]`,通过模型`actuator_ctrlrange`自动映射到实际范围 - -# 文档查看 -在命令行中进入项目根目录,运行: -```shell -mkdocs build -mkdocs serve -``` -使用浏览器打开 http://127.0.0.1:8000,查看项目文档是否正常显示。 - -# 常见问题解决 -1. **`ModuleNotFoundError: No module named 'rclpy'`** - → 错误导入 ROS 2 库,替换为 ROS 1 的 `rospy` 即可。 - -2. **`unrecognized arguments: __name:=mujoco_core`** - → 修改参数解析逻辑,忽略ROS自动添加的参数: - ```python - args, unknown = parser.parse_known_args() # 替代 parser.parse_args() - ``` - -3. **`模型文件不存在`** - → 检查模型路径是否正确,可视化命令支持自动搜索目录下的`.xml`/`.mjb`文件。 - -4. **`策略模型加载失败`** - → 确认:① 模型文件存在 ② PyTorch已安装 ③ 模型输入/输出维度与环境匹配(obs_dim=qpos+qvel维度,action_dim=nu) - -5. **`ROS话题无数据`** - → 检查:① `roscore`是否启动 ② Catkin环境是否加载(`source devel/setup.bash`)③ 关节名称映射是否正确 - -# 贡献指南 -提交代码前,请阅读 贡献指南。代码优化方向包括: -- 遵循 PEP 8 代码风格 并完善注释 -- 扩展策略网络类型(如CNN、RNN,适配视觉观测) -- 优化策略推理效率(如ONNX导出、TensorRT加速) -- 完善ROS功能(如服务调用、参数服务器配置) -- 增加自动化测试(模型加载/转换验证、策略性能基准测试) -- 支持多智能体仿真与分布式控制 - -# 参考资源 -- MuJoCo 官方文档 -- MuJoCo Python 绑定教程 -- MuJoCo 模型库(Menagerie) -- PyTorch 神经网络教程 -- ROS 1 Noetic 官方文档 -- 强化学习在MuJoCo中的应用(MJX框架) \ No newline at end of file +2. 输入输出 +输入:观测(关节位置 qpos + 关节速度 qvel); +输出:归一化控制指令([-1,1]),自动映射至模型 actuator_ctrlrange 范围: +python +运行 +### 核心映射逻辑 +action = ctrl_min + (ctrl_max - ctrl_min) * (action + 1) / 2 +action = np.clip(action, ctrl_min, ctrl_max) # 强制裁剪至物理极限 +## 性能优化点 +速度测试:线程内动态生成控制噪声,避免主线程预生成大数组; +策略推理:复用张量对象,避免重复内存分配; +控制指令:强制裁剪至执行器物理极限,防止超限报错; +路径处理:支持目录自动识别模型文件,提升易用性; +关节映射:精准过滤自由关节,仅发布有效关节状态。 +## 常见问题 +问题现象 解决方案 +模型加载失败 检查路径是否正确,确保文件为 .xml/.mjb 格式 +策略加载失败 确认 PyTorch 已安装、策略文件路径正确、输入输出维度匹配 +ROS 无数据 确保 roscore 已启动、话题名称匹配、控制指令长度 = 模型 nu +仿真实时性差 使用 MJB 格式模型、减少线程数、降低控制噪声强度 +控制指令无效果 检查模型 nu(控制维度)是否 > 0,策略输出是否映射至正确范围 +## 参考资源 +1、MuJoCo 官方文档 +2、PyTorch 神经网络教程 +3、ROS 1 Noetic 文档 +4、MuJoCo 官方模型库 \ No newline at end of file diff --git a/src/Neuro_Mujoco/main.py b/src/Neuro_Mujoco/main.py index 762bee10cc..0f9d25aa49 100644 --- a/src/Neuro_Mujoco/main.py +++ b/src/Neuro_Mujoco/main.py @@ -9,16 +9,16 @@ import mujoco from mujoco import viewer -# ===================== 机器学习(PyTorch)相关导入 ===================== +# ===================== 依赖导入 - 机器学习(PyTorch) ===================== try: import torch import torch.nn as nn TORCH_AVAILABLE = True except ImportError: TORCH_AVAILABLE = False - logging.warning("未检测到PyTorch,策略控制功能已禁用(安装:pip install torch)") + logging.warning("PyTorch未检测到,策略控制功能禁用(安装命令:pip install torch)") -# ===================== ROS 1 相关导入 ===================== +# ===================== 依赖导入 - ROS 1 ===================== try: import rospy from sensor_msgs.msg import JointState @@ -27,66 +27,75 @@ ROS_AVAILABLE = True except ImportError: ROS_AVAILABLE = False - logging.warning("未检测到 ROS 环境,ROS 功能已禁用(如需启用,请安装 ROS 1 Noetic)") + logging.warning("ROS环境未检测到,ROS功能禁用(需安装ROS 1 Noetic)") -# ===================== 日志系统配置 ===================== +# ===================== 日志配置 ===================== logging.basicConfig( level=logging.INFO, - format="%(asctime)s - %(levelname)s - %(message)s", + format="[%(asctime)s] [%(levelname)s] %(message)s", datefmt="%Y-%m-%d %H:%M:%S", handlers=[logging.StreamHandler(sys.stdout)] ) -logger = logging.getLogger("mujoco_utils") +logger = logging.getLogger("mujoco_control_tool") -# ===================== 强化学习策略网络 ===================== +# ===================== 强化学习策略网络定义 ===================== class PolicyNetwork(nn.Module): - """轻量级策略网络(适用于MuJoCo机器人控制) - 输入:观测(关节位置+速度),输出:归一化控制指令([-1,1]) + """ + 轻量级策略网络(适配MuJoCo机器人控制场景) + 输入:观测向量(关节位置 + 关节速度) + 输出:归一化控制指令(范围[-1, 1]) """ def __init__(self, obs_dim: int, action_dim: int, hidden_dim: int = 64): super().__init__() - self.net = nn.Sequential( + self.backbone = nn.Sequential( nn.Linear(obs_dim, hidden_dim), nn.Tanh(), nn.Linear(hidden_dim, hidden_dim), nn.Tanh(), nn.Linear(hidden_dim, action_dim), - nn.Tanh() # 输出范围[-1,1],后续映射到实际控制范围 + nn.Tanh() # 输出约束在[-1,1],后续映射到实际控制范围 ) - def forward(self, x: torch.Tensor) -> torch.Tensor: - """前向推理(禁用梯度计算以提升效率)""" + def forward(self, obs: torch.Tensor) -> torch.Tensor: + """前向推理(禁用梯度计算提升效率)""" with torch.no_grad(): - return self.net(x) + return self.backbone(obs) # ===================== 核心功能函数 ===================== -def load_model(model_path: str) -> Tuple[Optional[mujoco.MjModel], Optional[mujoco.MjData]]: +def load_mujoco_model(model_path: str) -> Tuple[Optional[mujoco.MjModel], Optional[mujoco.MjData]]: """ 加载MuJoCo模型(支持XML/MJB格式) + :param model_path: 模型文件路径 + :return: (MjModel实例, MjData实例),加载失败返回(None, None) """ if not os.path.exists(model_path): - logger.error(f"模型文件不存在: {model_path}") + logger.error(f"模型文件不存在:{model_path}") return None, None try: + # 区分二进制/XML格式加载 if model_path.endswith('.mjb'): model = mujoco.MjModel.from_binary_path(model_path) else: model = mujoco.MjModel.from_xml_path(model_path) + data = mujoco.MjData(model) - logger.info(f"成功加载模型: {model_path}") - logger.info(f"模型信息:控制维度(nu)={model.nu} | 关节数(njnt)={model.njnt} | 自由度(nq)={model.nq}") + logger.info(f"模型加载成功:{model_path}") + logger.info(f"模型参数:控制维度(nu)={model.nu} | 关节数(njnt)={model.njnt} | 自由度(nq)={model.nq}") return model, data except Exception as e: - logger.error(f"模型加载失败: {str(e)}", exc_info=True) + logger.error(f"模型加载失败:{str(e)}", exc_info=True) return None, None -def convert_model(input_path: str, output_path: str) -> bool: +def convert_mujoco_model(input_path: str, output_path: str) -> bool: """ - 转换模型格式(XML ↔ MJB) + 转换MuJoCo模型格式(XML ↔ MJB) + :param input_path: 输入模型路径 + :param output_path: 输出模型路径(指定.xml/.mjb后缀) + :return: 转换成功返回True,失败返回False """ - model, data = load_model(input_path) + model, data = load_mujoco_model(input_path) if not model or not data: return False @@ -95,330 +104,335 @@ def convert_model(input_path: str, output_path: str) -> bool: if output_dir and not os.path.exists(output_dir): try: os.makedirs(output_dir, exist_ok=True) - logger.info(f"创建输出目录: {output_dir}") + logger.info(f"创建输出目录:{output_dir}") except Exception as e: - logger.error(f"无法创建输出目录: {str(e)}") + logger.error(f"创建输出目录失败:{str(e)}") return False try: if output_path.endswith('.mjb'): + # 保存为二进制格式 mujoco.save_model(model, output_path) - logger.info(f"二进制模型已保存至: {output_path}") + logger.info(f"二进制模型已保存:{output_path}") else: + # 保存为XML格式 xml_content = mujoco.mj_saveLastXMLToString(data) with open(output_path, 'w', encoding='utf-8') as f: f.write(xml_content) - logger.info(f"XML模型已保存至: {output_path}") + logger.info(f"XML模型已保存:{output_path}") return True except Exception as e: - logger.error(f"模型转换失败: {str(e)}", exc_info=True) + logger.error(f"模型转换失败:{str(e)}", exc_info=True) return False -def test_speed( +def test_simulation_speed( model_path: str, - nstep: int = 10000, - nthread: int = 1, - ctrlnoise: float = 0.01 + step_num: int = 10000, + thread_num: int = 1, + ctrl_noise: float = 0.01 ) -> None: """ - 测试模型模拟速度(多线程) + 测试MuJoCo模型模拟速度(支持多线程) + :param model_path: 模型文件路径 + :param step_num: 每线程模拟步数 + :param thread_num: 测试线程数 + :param ctrl_noise: 控制指令噪声强度 """ - model, _ = load_model(model_path) + model, _ = load_mujoco_model(model_path) if not model: return - # 参数验证 - if nstep <= 0: - logger.error("步数必须为正数") + # 参数合法性校验 + if step_num <= 0: + logger.error("模拟步数必须为正整数") return - if nthread <= 0: - logger.error("线程数必须为正数") + if thread_num <= 0: + logger.error("线程数必须为正整数") return - # 生成控制噪声(处理nu=0的情况) - if model.nu == 0: - ctrl = None - logger.warning("模型无控制输入(nu=0),将跳过控制噪声") - else: - ctrl = ctrlnoise * np.random.randn(nstep, model.nu) - - logger.info(f"开始速度测试: 线程数={nthread}, 每线程步数={nstep}") + logger.info(f"开始模拟速度测试 | 线程数:{thread_num} | 每线程步数:{step_num}") - def simulate_thread(thread_id: int) -> float: - """单线程模拟函数""" + def single_thread_simulation(thread_id: int) -> float: + """单线程模拟函数(优化内存占用:逐步生成控制噪声)""" mj_data = mujoco.MjData(model) - start = time.perf_counter() - for i in range(nstep): - if ctrl is not None: - mj_data.ctrl[:] = ctrl[i] + start_time = time.perf_counter() + + for _ in range(step_num): + # 逐步生成控制噪声,避免预分配大数组 + if model.nu > 0: + mj_data.ctrl[:] = ctrl_noise * np.random.randn(model.nu) mujoco.mj_step(model, mj_data) - end = time.perf_counter() - duration = end - start - logger.debug(f"线程 {thread_id} 完成,耗时: {duration:.2f}秒") + + duration = time.perf_counter() - start_time + logger.debug(f"线程 {thread_id} 完成 | 耗时:{duration:.2f}秒") return duration - # 执行多线程测试 - start_time = time.perf_counter() - with ThreadPoolExecutor(max_workers=nthread) as executor: - thread_durations: List[float] = list(executor.map(simulate_thread, range(nthread))) - total_time = time.perf_counter() - start_time + # 多线程执行模拟 + total_start = time.perf_counter() + with ThreadPoolExecutor(max_workers=thread_num) as executor: + thread_durations = list(executor.map(single_thread_simulation, range(thread_num))) + total_duration = time.perf_counter() - total_start # 计算性能指标 - total_steps = nstep * nthread - steps_per_sec = total_steps / total_time - realtime_factor = (total_steps * model.opt.timestep) / total_time + total_steps = step_num * thread_num + steps_per_second = total_steps / total_duration + realtime_factor = (total_steps * model.opt.timestep) / total_duration - logger.info("\n===== 速度测试结果 =====") - logger.info(f"总步数: {total_steps:,}") - logger.info(f"总耗时: {total_time:.2f}秒") - logger.info(f"每秒步数: {steps_per_sec:.0f}") - logger.info(f"实时因子: {realtime_factor:.2f}x") - logger.info(f"线程平均耗时: {np.mean(thread_durations):.2f}秒 (±{np.std(thread_durations):.2f})") + logger.info("\n===== 模拟速度测试结果 =====") + logger.info(f"总模拟步数:{total_steps:,}") + logger.info(f"总耗时:{total_duration:.2f}秒") + logger.info(f"每秒步数:{steps_per_second:.0f} step/s") + logger.info(f"实时因子:{realtime_factor:.2f}x") + logger.info(f"线程平均耗时:{np.mean(thread_durations):.2f}秒(±{np.std(thread_durations):.2f})") -def visualize(model_path: str, use_ros: bool = False, policy_path: Optional[str] = None) -> None: +def visualize_model(model_path: str, use_ros_mode: bool = False, policy_model_path: Optional[str] = None) -> None: """ - 可视化模型并运行模拟(支持ROS/策略控制) - :param model_path: 模型文件/目录路径 - :param use_ros: 是否启用ROS模式 - :param policy_path: 预训练策略模型路径(.pth) + 可视化MuJoCo模型并运行模拟(支持ROS通信/策略控制) + :param model_path: 模型文件路径/目录路径 + :param use_ros_mode: 是否启用ROS 1通信模式 + :param policy_model_path: 预训练策略模型路径(.pth文件) """ - # 模型路径智能校验(支持目录自动找XML/MJB文件) + # 路径智能匹配:如果是目录,自动查找XML/MJB文件 if os.path.isdir(model_path): - model_files = [] - for file in os.listdir(model_path): - if file.endswith(('.xml', '.mjb')): - model_files.append(os.path.join(model_path, file)) + model_files = [ + os.path.join(model_path, f) + for f in os.listdir(model_path) + if f.endswith(('.xml', '.mjb')) + ] if not model_files: logger.error(f"目录 {model_path} 中未找到.xml/.mjb模型文件") return model_path = model_files[0] - logger.info(f"自动选择目录中的模型文件: {model_path}") + logger.info(f"自动选择模型文件:{model_path}") - model, data = load_model(model_path) + # 加载模型 + model, data = load_mujoco_model(model_path) if not model: return # ===================== 策略网络初始化 ===================== - policy = None - obs_dim = model.nq + model.nv # 观测维度:关节位置 + 关节速度 - action_dim = model.nu - ctrl_range = None # 控制指令实际范围 + policy_net = None + obs_dimension = model.nq + model.nv # 观测维度 = 关节位置数 + 关节速度数 + action_dimension = model.nu + control_range = None # 执行器控制范围(min, max) - if policy_path and TORCH_AVAILABLE and action_dim > 0: + if policy_model_path and TORCH_AVAILABLE and action_dimension > 0: try: - # 加载预训练策略模型 - policy = PolicyNetwork(obs_dim, action_dim) - policy.load_state_dict(torch.load(policy_path, map_location=torch.device('cpu'))) - policy.eval() # 推理模式 - logger.info(f"成功加载策略模型: {policy_path}") - - # 获取控制指令范围(映射[-1,1]到实际范围) - ctrl_range = [] - for i in range(action_dim): - if model.actuator_ctrllimited[i]: - ctrl_range.append(model.actuator_ctrlrange[i]) + # 初始化并加载预训练策略 + policy_net = PolicyNetwork(obs_dimension, action_dimension) + policy_net.load_state_dict(torch.load(policy_model_path, map_location=torch.device('cpu'))) + policy_net.eval() # 推理模式(禁用Dropout/BatchNorm) + logger.info(f"预训练策略模型加载成功:{policy_model_path}") + + # 获取执行器控制范围(用于映射归一化输出) + control_range = [] + for idx in range(action_dimension): + if model.actuator_ctrllimited[idx]: + control_range.append(model.actuator_ctrlrange[idx]) else: - ctrl_range.append((-1.0, 1.0)) - ctrl_range = np.array(ctrl_range) + control_range.append((-1.0, 1.0)) + control_range = np.array(control_range) except Exception as e: - logger.error(f"策略模型加载失败: {str(e)}", exc_info=True) - policy = None - elif policy_path: - logger.warning("策略功能需满足:PyTorch已安装 + 模型有控制维度(nu>0)") + logger.error(f"策略模型加载失败:{str(e)}", exc_info=True) + policy_net = None + elif policy_model_path: + logger.warning("策略控制需满足:PyTorch已安装 + 模型有控制维度(nu>0)") # ===================== ROS 初始化 ===================== - ros_publishers = None - ros_subscribers = None - ros_rate = None - ctrl_cmd = None - joint_msg = None - joint_ids = [] - joint_qpos_idxs = [] - joint_qvel_idxs = [] - - if use_ros: + ros_pubs = None + ros_subs = None + ros_loop_rate = None + ros_ctrl_cmd = None + joint_state_msg = None + joint_ids_list = [] + joint_qpos_indices = [] + joint_qvel_indices = [] + + if use_ros_mode: if not ROS_AVAILABLE: logger.error("ROS环境未就绪,无法启用ROS模式") return - rospy.init_node("mujoco_ros_node", anonymous=True) - ros_rate = rospy.Rate(100) # 100Hz匹配MuJoCo默认步长 + rospy.init_node("mujoco_visualizer_node", anonymous=True) + ros_loop_rate = rospy.Rate(100) # 100Hz(匹配MuJoCo默认步长) logger.info("="*60) - logger.info("ROS 1 模式已启用!") - logger.info(f"发布话题:/mujoco/joint_states、/mujoco/pose") - logger.info(f"订阅话题:/mujoco/ctrl_cmd(长度={model.nu})") + logger.info("ROS 1 通信模式已启用") + logger.info(f"发布话题:/mujoco/joint_states、/mujoco/base_pose") + logger.info(f"订阅话题:/mujoco/control_command(长度={model.nu})") logger.info("="*60) # 创建ROS发布者 joint_state_pub = rospy.Publisher("/mujoco/joint_states", JointState, queue_size=10) - pose_pub = rospy.Publisher("/mujoco/pose", PoseStamped, queue_size=10) - ros_publishers = (joint_state_pub, pose_pub) - - # 初始化关节状态消息(精准映射非自由关节) - joint_msg = JointState() - joint_msg.name = [] - for i in range(model.njnt): - joint_type = model.joint(i).type + base_pose_pub = rospy.Publisher("/mujoco/base_pose", PoseStamped, queue_size=10) + ros_pubs = (joint_state_pub, base_pose_pub) + + # 初始化关节状态消息(仅包含非自由关节) + joint_state_msg = JointState() + joint_state_msg.name = [] + for joint_idx in range(model.njnt): + joint_type = model.joint(joint_idx).type if joint_type != mujoco.mjtJoint.mjJNT_FREE: - joint_msg.name.append(model.joint(i).name) - joint_ids.append(i) - joint_qpos_idxs.append(model.jnt_qposadr[i]) - joint_qvel_idxs.append(model.jnt_dofadr[i]) + joint_state_msg.name.append(model.joint(joint_idx).name) + joint_ids_list.append(joint_idx) + joint_qpos_indices.append(model.jnt_qposadr[joint_idx]) + joint_qvel_indices.append(model.jnt_dofadr[joint_idx]) - njnt = len(joint_msg.name) - logger.info(f"ROS将发布 {njnt} 个非自由关节状态:{joint_msg.name}") - - # 创建ROS订阅者(接收控制指令) - ctrl_cmd = np.zeros(model.nu) if model.nu > 0 else None - def ctrl_callback(msg: Float32MultiArray): - nonlocal ctrl_cmd - if model.nu == len(msg.data): - ctrl_cmd = np.array(msg.data) - logger.debug(f"收到ROS控制指令:{ctrl_cmd[:5]}...") + valid_joint_num = len(joint_state_msg.name) + logger.info(f"ROS将发布 {valid_joint_num} 个非自由关节状态:{joint_state_msg.name}") + + # 创建ROS订阅者(接收外部控制指令) + ros_ctrl_cmd = np.zeros(model.nu) if model.nu > 0 else None + def control_cmd_callback(msg: Float32MultiArray): + nonlocal ros_ctrl_cmd + if len(msg.data) == model.nu: + ros_ctrl_cmd = np.array(msg.data) + logger.debug(f"收到ROS控制指令:{ros_ctrl_cmd[:5]}...") else: - logger.warning(f"控制指令长度不匹配!期望 {model.nu},实际 {len(msg.data)}") + logger.warning(f"控制指令长度不匹配 | 期望:{model.nu} | 实际:{len(msg.data)}") if model.nu > 0: - ros_subscribers = rospy.Subscriber( - "/mujoco/ctrl_cmd", Float32MultiArray, ctrl_callback, queue_size=5 + ros_subs = rospy.Subscriber( + "/mujoco/control_command", Float32MultiArray, control_cmd_callback, queue_size=5 ) else: - logger.warning("模型无控制输入(nu=0),不订阅控制指令话题") + logger.warning("模型无控制输入(nu=0),跳过控制指令订阅") # ===================== 可视化主循环 ===================== - logger.info("启动可视化窗口(按ESC键退出 | 鼠标交互:拖拽旋转、滚轮缩放)") # 修正缩进 + logger.info("启动MuJoCo可视化窗口 | 退出:ESC键 | 交互:鼠标拖拽(旋转)/滚轮(缩放)") try: - with viewer.launch_passive(model, data) as v: - # 预分配推理张量(复用,避免每次创建) - obs_tensor = None if policy is None else torch.zeros(1, obs_dim, dtype=torch.float32) + with viewer.launch_passive(model, data) as viewer_instance: + # 预分配观测张量(复用避免重复内存分配) + obs_tensor = torch.zeros(1, obs_dimension, dtype=torch.float32) if policy_net else None - while v.is_running() and (not use_ros or not rospy.is_shutdown()): + while viewer_instance.is_running() and (not use_ros_mode or not rospy.is_shutdown()): # 控制指令优先级:ROS指令 > 策略推理 > 无控制 - if use_ros and ctrl_cmd is not None: - data.ctrl[:] = ctrl_cmd - elif policy is not None: - # 提取观测:关节位置 + 关节速度 - obs = np.concatenate([data.qpos, data.qvel]) + if use_ros_mode and ros_ctrl_cmd is not None: + data.ctrl[:] = ros_ctrl_cmd + elif policy_net is not None: + # 提取观测向量:关节位置 + 关节速度 + observation = np.concatenate([data.qpos, data.qvel]) - # 优化1:复用张量,避免重复内存分配 + # 复用张量,避免重复创建 if obs_tensor is None: - obs_tensor = torch.tensor(obs, dtype=torch.float32).unsqueeze(0) + obs_tensor = torch.tensor(observation, dtype=torch.float32).unsqueeze(0) else: - obs_tensor[0] = torch.from_numpy(obs) + obs_tensor[0] = torch.from_numpy(observation) - # 策略推理 - action = policy(obs_tensor).squeeze().numpy() + # 策略推理得到归一化动作([-1,1]) + normalized_action = policy_net(obs_tensor).squeeze().numpy() - # 映射到实际控制范围 + 优化2:强制裁剪到物理极限(避免超限) - if ctrl_range is not None: - action = ctrl_range[:, 0] + (ctrl_range[:, 1] - ctrl_range[:, 0]) * (action + 1) / 2 # 核心映射:[-1,1]→[ctrl_min,ctrl_max] 线性缩放 - action = np.clip(action, ctrl_range[:, 0], ctrl_range[:, 1]) # 强制裁剪,保证指令符合执行器物理极限 - action = ctrl_range[:, 0] + (ctrl_range[:, 1] - ctrl_range[:, 0]) * (action + 1) / 2 # 核心映射:[-1,1]→[ctrl_min,ctrl_max] 线性缩放,保证指令符合执行器物理极限 + # 映射到执行器实际控制范围 + 裁剪(核心优化:删除重复映射) + if control_range is not None: + # 线性映射:[-1,1] → [ctrl_min, ctrl_max] + normalized_action = control_range[:, 0] + (control_range[:, 1] - control_range[:, 0]) * (normalized_action + 1) / 2 + # 强制裁剪到物理极限(避免超限损坏模拟) + normalized_action = np.clip(normalized_action, control_range[:, 0], control_range[:, 1]) - data.ctrl[:] = action + data.ctrl[:] = normalized_action - # 执行模拟步 + # 执行单步模拟 mujoco.mj_step(model, data) - v.sync() - - # ROS消息发布(修正缩进:从policy分支移出,作为while循环顶层逻辑) - if use_ros and ros_publishers is not None: - joint_state_pub, pose_pub = ros_publishers - - # 发布关节状态 - joint_msg.header.stamp = rospy.Time.now() - joint_msg.position = [] - joint_msg.velocity = [] - for joint_id, qpos_idx, qvel_idx in zip(joint_ids, joint_qpos_idxs, joint_qvel_idxs): - joint_type = model.joint(joint_id).type - if joint_type == mujoco.mjtJoint.mjJNT_BALL: - joint_msg.position.extend(data.qpos[qpos_idx:qpos_idx+3]) - joint_msg.velocity.extend(data.qvel[qvel_idx:qvel_idx+3]) - elif joint_type in [mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE]: - joint_msg.position.append(data.qpos[qpos_idx]) - joint_msg.velocity.append(data.qvel[qvel_idx]) + viewer_instance.sync() + + # ROS消息发布逻辑 + if use_ros_mode and ros_pubs is not None: + joint_state_pub, base_pose_pub = ros_pubs + + # 填充并发布关节状态 + joint_state_msg.header.stamp = rospy.Time.now() + joint_state_msg.position = [] + joint_state_msg.velocity = [] - joint_state_pub.publish(joint_msg) + for j_id, qp_idx, qv_idx in zip(joint_ids_list, joint_qpos_indices, joint_qvel_indices): + j_type = model.joint(j_id).type + if j_type == mujoco.mjtJoint.mjJNT_BALL: + # 球关节:3个位置/速度维度 + joint_state_msg.position.extend(data.qpos[qp_idx:qp_idx+3]) + joint_state_msg.velocity.extend(data.qvel[qv_idx:qv_idx+3]) + elif j_type in [mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE]: + # 铰链/滑动关节:1个位置/速度维度 + joint_state_msg.position.append(data.qpos[qp_idx]) + joint_state_msg.velocity.append(data.qvel[qv_idx]) + + joint_state_pub.publish(joint_state_msg) - # 发布基座姿态 - pose_msg = PoseStamped() - pose_msg.header.stamp = rospy.Time.now() - pose_msg.header.frame_id = "world" + # 填充并发布基座姿态 + base_pose_msg = PoseStamped() + base_pose_msg.header.stamp = rospy.Time.now() + base_pose_msg.header.frame_id = "world" + # 位置信息(前3维) if model.nq >= 1: - pose_msg.pose.position.x = data.qpos[0] + base_pose_msg.pose.position.x = data.qpos[0] if model.nq >= 2: - pose_msg.pose.position.y = data.qpos[1] + base_pose_msg.pose.position.y = data.qpos[1] if model.nq >= 3: - pose_msg.pose.position.z = data.qpos[2] + base_pose_msg.pose.position.z = data.qpos[2] + # 姿态四元数(后4维) if model.nq >= 4: - pose_msg.pose.orientation.x = data.qpos[3] + base_pose_msg.pose.orientation.x = data.qpos[3] if model.nq >= 5: - pose_msg.pose.orientation.y = data.qpos[4] + base_pose_msg.pose.orientation.y = data.qpos[4] if model.nq >= 6: - pose_msg.pose.orientation.z = data.qpos[5] + base_pose_msg.pose.orientation.z = data.qpos[5] if model.nq >= 7: - pose_msg.pose.orientation.w = data.qpos[6] + base_pose_msg.pose.orientation.w = data.qpos[6] - pose_pub.publish(pose_msg) - ros_rate.sleep() + base_pose_pub.publish(base_pose_msg) + ros_loop_rate.sleep() - logger.info("可视化窗口已关闭") + logger.info("MuJoCo可视化窗口已关闭") except Exception as e: - logger.error(f"可视化过程出错: {str(e)}", exc_info=True) + logger.error(f"可视化过程异常:{str(e)}", exc_info=True) -# ===================== 主函数(命令行入口) ===================== -def main() -> None: +# ===================== 命令行入口 ===================== +def main(): parser = argparse.ArgumentParser( - description="MuJoCo功能整合工具(支持强化学习策略控制 + ROS 1通信)", + description="MuJoCo模型工具集(支持可视化/速度测试/格式转换 + ROS/策略控制)", formatter_class=argparse.ArgumentDefaultsHelpFormatter ) - subparsers = parser.add_subparsers(dest="command", required=True) + subparsers = parser.add_subparsers(dest="subcommand", required=True, help="子命令列表") - # 1. 可视化命令(修复重复添加model参数问题) + # 子命令1:模型可视化 viz_parser = subparsers.add_parser("visualize", help="可视化模型并运行模拟") - viz_parser.add_argument( - "model", - help="模型文件路径/目录(示例:/home/lan/桌面/nn/mujoco_menagerie/anybotics_anymal_b)" - ) - viz_parser.add_argument("--ros", action="store_true", help="启用ROS 1模式(发布/订阅关节控制话题)") + viz_parser.add_argument("model_path", help="模型文件/目录路径(示例:./models/anymal.xml)") + viz_parser.add_argument("--ros", action="store_true", help="启用ROS 1通信模式") viz_parser.add_argument("--policy", help="预训练策略模型路径(.pth文件)") - # 2. 速度测试命令 - speed_parser = subparsers.add_parser("testspeed", help="测试模型模拟速度(多线程)") - speed_parser.add_argument("model", help="模型文件路径") - speed_parser.add_argument("--nstep", type=int, default=10000, help="每线程模拟步数") - speed_parser.add_argument("--nthread", type=int, default=1, help="测试线程数") - speed_parser.add_argument("--ctrlnoise", type=float, default=0.01, help="控制噪声强度") + # 子命令2:模拟速度测试 + speed_parser = subparsers.add_parser("test_speed", help="测试模型模拟速度(多线程)") + speed_parser.add_argument("model_path", help="模型文件路径") + speed_parser.add_argument("--step_num", type=int, default=10000, help="每线程模拟步数") + speed_parser.add_argument("--thread_num", type=int, default=1, help="测试线程数") + speed_parser.add_argument("--ctrl_noise", type=float, default=0.01, help="控制指令噪声强度") - # 3. 模型转换命令 + # 子命令3:模型格式转换 convert_parser = subparsers.add_parser("convert", help="转换模型格式(XML ↔ MJB)") - convert_parser.add_argument("input", help="输入模型路径") - convert_parser.add_argument("output", help="输出模型路径(指定.xml/.mjb)") + convert_parser.add_argument("input_path", help="输入模型路径") + convert_parser.add_argument("output_path", help="输出模型路径(指定.xml/.mjb后缀)") - # 解析命令行参数 - args, unknown = parser.parse_known_args() + # 解析参数 + args = parser.parse_args() - # 命令映射 - command_handlers: Dict[str, callable] = { - "visualize": lambda: visualize(args.model, use_ros=args.ros, policy_path=args.policy), - "testspeed": lambda: test_speed(args.model, args.nstep, args.nthread, args.ctrlnoise), - "convert": lambda: convert_model(args.input, args.output) + # 子命令映射 + subcommand_handlers = { + "visualize": lambda: visualize_model(args.model_path, args.ros, args.policy), + "test_speed": lambda: test_simulation_speed(args.model_path, args.step_num, args.thread_num, args.ctrl_noise), + "convert": lambda: convert_mujoco_model(args.input_path, args.output_path) } - # 执行命令 + # 执行子命令 try: - command_handlers[args.command]() + subcommand_handlers[args.subcommand]() except KeyError: - logger.error(f"未知命令: {args.command}") + logger.error(f"未知子命令:{args.subcommand}") sys.exit(1) except Exception as e: - logger.critical(f"程序执行失败: {str(e)}", exc_info=True) + logger.critical(f"程序执行失败:{str(e)}", exc_info=True) sys.exit(1) -# ===================== 程序入口 ===================== if __name__ == "__main__": main() \ No newline at end of file