From 862982b269172c92d8b8a08aecb61961062eab1c Mon Sep 17 00:00:00 2001 From: lyy-1236 <3584247842@qq.com> Date: Sun, 26 Apr 2026 18:09:46 +0800 Subject: [PATCH 1/9] Create dua_arm --- src/dua_arm | 1 + 1 file changed, 1 insertion(+) create mode 100644 src/dua_arm diff --git a/src/dua_arm b/src/dua_arm new file mode 100644 index 0000000000..8b13789179 --- /dev/null +++ b/src/dua_arm @@ -0,0 +1 @@ + From 723d44ef1ce10d7fa0424b41395f3f883ec88bc5 Mon Sep 17 00:00:00 2001 From: lyy-1236 <3584247842@qq.com> Date: Sun, 26 Apr 2026 18:11:03 +0800 Subject: [PATCH 2/9] Delete src/dua_arm --- src/dua_arm | 1 - 1 file changed, 1 deletion(-) delete mode 100644 src/dua_arm diff --git a/src/dua_arm b/src/dua_arm deleted file mode 100644 index 8b13789179..0000000000 --- a/src/dua_arm +++ /dev/null @@ -1 +0,0 @@ - From 7505a580a170be0955dd9285da7a185eb574941b Mon Sep 17 00:00:00 2001 From: lyy-1236 <3584247842@qq.com> Date: Sun, 26 Apr 2026 18:14:43 +0800 Subject: [PATCH 3/9] docs: add initial README for 4-DOF arm simulation project --- src/dua_arm/README | 152 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 152 insertions(+) create mode 100644 src/dua_arm/README diff --git a/src/dua_arm/README b/src/dua_arm/README new file mode 100644 index 0000000000..caa773778b --- /dev/null +++ b/src/dua_arm/README @@ -0,0 +1,152 @@ + +# box — 仿真与强化学习实验箱 + +## 概述 + +`src/box` 包含若干用于仿真、感知与强化学习实验的环境与辅助脚本。主要面向基于 Gymnasium/MuJoCo 的任务、轻量级可视化示例以及调试/测试工具。 + +Gymnasium：一个操作台，是网上的一个强化学习的开源项目，给你提供一套通用的指令，可以完成一些基础操作。并提供给你进一步写指令代码的框架。 + +MuJoCo：一个物理仿真平台，计算你的指令在现实中执行后可能产生什么结果。 + +在这个项目中是一起用，如果只是写一些小例子，可以只用Gymnasium,它的内部也包含了一些现成的环境。 + +本目录旨在提供： + +- 可复现的仿真实验环境模板； +- 启动/调试脚本示例； +- 与主仓库其余模块交互的接口说明（若存在）。 + +## 推荐目录结构（示例） + +- `simulator.py`：仿真环境实现（通常继承自 `gym.Env` 或 `gymnasium.Env`）； +- `envs/`：若干子环境模块或封装； +- `agents/`：示例智能体或策略实现（可选）； +- `scripts/`：运行/训练/评估脚本（如 `run.py`, `train.py`, `eval.py`）； +- `tests/`：小型示例与单元/集成测试脚本（如 `test_simulator.py`）； +- `configs/`：默认配置或参数文件（YAML/JSON）； +- `README.md`：本文件。 + +（实际文件以仓库中为准，此处为常见约定。） + +## 快速上手 + +以下示例以 Windows + PowerShell 为例： + +1) 进入项目根目录并创建虚拟环境： + +```powershell +cd D:\\nn +python -m venv .venv +.\\.venv\\Scripts\\Activate.ps1 +``` + +2) 安装依赖： + +```powershell +pip install -r requirements.txt -i https://pypi.tuna.tsinghua.edu.cn/simple +``` + +如果仓库未包含完整的 `requirements.txt`，可参考核心依赖： + +``` +gymnasium +mujoco +stable-baselines3 +pygame +opencv-python +numpy +scipy +matplotlib +ruamel.yaml +``` + +3) 运行示例仿真（示例脚本名以仓库实际文件为准）： + +```powershell +python scripts/run.py +``` + +或： + +```powershell +python tests/test_simulator.py +``` + +运行后通常会在本地弹出可视化窗口或在终端打印日志，具体行为视环境实现而定。 + +## 开发与调试提示 + +- MuJoCo：请确保已正确安装 MuJoCo 及其许可证（若使用 mujoco 依赖）。 +- 显示/渲染：在无显示的服务器上运行时，可能需要配置虚拟帧缓冲（Linux 下使用 xvfb）。 +- 依赖冲突：若出现版本不兼容，建议使用虚拟环境并锁定依赖版本。 + +## 如何贡献 + +- 修改或补充说明、示例脚本或配置后提交 Pull Request； +- 提交 Issue 时请包含：操作系统、Python 版本、依赖版本与复现步骤/错误日志。 + +## 参考与更多信息 + +- 查看目录下的具体脚本与模块顶部注释，通常包含使用示例与参数说明； +- 若需要，我可以为 `src/box` 中的主要文件生成更详细的文档或示例运行脚本。 + +**box — 仿真与强化学习实验箱** + +简介 +- `src/box` 目录包含基于 Gymnasium 和 MuJoCo 的仿真环境与相关辅助脚本，用于开发和测试生物力学/机器人仿真、感知模块与强化学习任务。 + +目录结构（示例） +- `simulator.py`：仿真环境核心（通常继承 `gym.Env`）。 +- `test_simulator.py`：示例运行脚本，用于启动仿真并可视化。 +- `main.py`：辅助脚本（例如证书或配置检查）。 +- `README.md`：本文件，说明目录用途与快速上手指南。 + +快速上手 +1. 创建并激活虚拟环境（以 Windows 为例）： + +```powershell +cd <项目根目录> +python -m venv venv --python=3.9 +.\\venv\\Scripts\\Activate.ps1 +``` + +2. 安装依赖（建议使用清华镜像加速）： + +```powershell +pip install -r requirements.txt -i https://pypi.tuna.tsinghua.edu.cn/simple +``` + +如果仓库没有完整的 `requirements.txt`，可参考下列核心库： + +```text +gymnasium +mujoco +stable-baselines3 +pygame +opencv-python +numpy +scipy +matplotlib +ruamel.yaml +certifi +``` + +运行示例 +- 启动仿真： + +```powershell +python test_simulator.py +``` + +运行后应弹出可视化窗口（若使用 Pygame/SDL），并在终端输出仿真日志。 + +贡献与问题反馈 +- 若需添加说明或示例，请提交 Pull Request。 +- 遇到环境或依赖问题，请在 Issue 中描述操作系统、Python 版本与错误日志。 + +更多信息 +- 若目录中包含更详细的子模块文档，请参阅相应文件（如 `simulator.py` 顶部注释或同目录下的文档）。 + +--- + From 9e940d91d094cf9b11f4932e4be50eb382f9cb3f Mon Sep 17 00:00:00 2001 From: lyy-1236 <3584247842@qq.com> Date: Mon, 27 Apr 2026 21:05:45 +0800 Subject: [PATCH 4/9] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=20yaml=20=E6=A8=A1?= =?UTF-8?q?=E5=9D=97=E7=BC=BA=E5=A4=B1=E6=8A=A5=E9=94=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/dua_arm/simulator.py | 512 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 512 insertions(+) create mode 100644 src/dua_arm/simulator.py diff --git a/src/dua_arm/simulator.py b/src/dua_arm/simulator.py new file mode 100644 index 0000000000..e5b7900f80 --- /dev/null +++ b/src/dua_arm/simulator.py @@ -0,0 +1,512 @@ +import os +import yaml +import logging +from typing import Dict, Any, Optional, Tuple, List + +import numpy as np +from gymnasium import spaces + +# 初始化日志配置，设置日志级别为INFO +logger = logging.getLogger(__name__) +logging.basicConfig(level=logging.INFO) + +# ---------------- 全局依赖检测 ---------------- +# 检测MuJoCo是否安装（核心物理仿真引擎） +try: + import mujoco + HAS_MUJOCO = True +except ImportError: + mujoco = None + HAS_MUJOCO = False + +# 检测MuJoCo Viewer是否安装（原生可视化工具） +try: + import mujoco_viewer + HAS_MUJOCO_VIEWER = True +except ImportError: + mujoco_viewer = None + HAS_MUJOCO_VIEWER = False + + +class Simulator: + """ + 机械臂/机械手仿真器核心类（兼容Gymnasium接口） + 核心特性： + 1. 支持MuJoCo物理引擎后端，自动生成机械臂/手MJCF模型文件 + 2. 无MuJoCo时自动降级为轻量级占位实现（保证模块可导入） + 3. 提供标准RL接口：reset/step/render/close + 4. 支持Pygame可视化（2D占位渲染）和MuJoCo Viewer（3D原生渲染） + """ + def __init__(self, simulator_folder: str, render_mode: Optional[str] = None): + """ + 仿真器初始化入口 + Args: + simulator_folder: 仿真配置/模型文件存储路径 + render_mode: 渲染模式 - "human"（可视化窗口）/None（无渲染） + """ + # 基础属性初始化 + self.simulator_folder = simulator_folder # 仿真文件根目录 + self.render_mode = render_mode # 渲染模式 + self.step_count = 0 # 当前仿真步数 + self.terminated = False # 任务是否完成（自然终止） + self.truncated = False # 任务是否截断（步数超限） + self.last_reward = 0.0 # 上一步奖励值 + + # 核心组件初始化（先置空，后续分步加载） + self.model = None # MuJoCo模型对象（物理模型定义） + self.data = None # MuJoCo数据对象（存储仿真状态：关节位置/速度等） + self.viewer = None # MuJoCo Viewer对象（3D渲染） + self.screen = None # Pygame窗口对象（2D渲染） + + # 初始化流程（分层解耦，便于维护） + # 1. 加载/生成配置文件（config.yaml） + self.config = self._load_config() + # 2. 加载/生成MuJoCo模型（MJCF文件） + self.model, self.data = self._load_model() + # 3. 校验并补全配置（保证关键参数存在） + self._validate_config() + + # 初始化RL核心空间和渲染 + self._init_action_space() # 动作空间（执行器控制信号） + self._init_observation_space() # 观测空间（关节状态） + if self.render_mode: + self._init_render() # 初始化渲染组件 + + @classmethod + def get(cls, simulator_folder: str, **kwargs): + """ + 类工厂方法（兼容原有接口） + Args: + simulator_folder: 仿真文件根目录 + **kwargs: 其他初始化参数（如render_mode） + Returns: + Simulator: 仿真器实例 + """ + return cls(simulator_folder, **kwargs) + + def _load_config(self) -> Dict[str, Any]: + """ + 加载/生成仿真配置文件（config.yaml） + - 若配置文件不存在，生成默认机械臂配置 + - 若存在，读取并返回配置字典 + Returns: + Dict[str, Any]: 仿真配置字典 + """ + # 配置文件路径 + config_path = os.path.join(self.simulator_folder, "config.yaml") + + # 配置文件不存在时，生成默认配置 + if not os.path.exists(config_path): + default_config = { + "simulation": { + "max_steps": 1000, # 单回合最大步数 + "model_path": "arm_model.mjcf", # MuJoCo模型文件路径 + "control_frequency": 20, # 控制频率（Hz） + "target_joint_pos": [0.0] # 目标关节位置（奖励函数用） + } + } + # 保存默认配置到文件 + with open(config_path, "w", encoding="utf-8") as f: + yaml.dump(default_config, f) + logger.info(f"生成默认配置文件: {config_path}") + + # 读取配置文件 + with open(config_path, "r", encoding="utf-8") as f: + config = yaml.safe_load(f) + logger.info(f"成功加载配置文件: {config_path}") + return config + + def _load_model(self) -> Tuple[mujoco.MjModel, mujoco.MjData]: + """ + 加载/生成MuJoCo模型文件（MJCF） + - 若模型文件不存在，生成简单机械臂模型 + - 加载模型并创建MjData对象（存储仿真状态） + Returns: + Tuple[mujoco.MjModel, mujoco.MjData]: MuJoCo模型和数据对象 + """ + # 模型文件路径（从配置读取） + model_path = os.path.join( + self.simulator_folder, + self.config["simulation"].get("model_path", "arm_model.mjcf") + ) + + # 检查路径是否包含非ASCII字符（MuJoCo在Windows上对含中文的绝对路径支持不佳） + def has_non_ascii(path_str: str) -> bool: + return any(ord(c) > 127 for c in path_str) + + # 若路径含中文/Unicode字符，转换为相对路径以兼容MuJoCo + if has_non_ascii(model_path): + try: + rel_path = os.path.relpath(model_path) + if not has_non_ascii(rel_path): + logger.info(f"路径含Unicode字符，转换为相对路径: {rel_path}") + model_path = rel_path + except ValueError: + pass + + # 模型文件不存在时，生成简单单关节机械臂MJCF + if not os.path.exists(model_path): + mjcf_content = """ + """ + # 保存MJCF文件 + with open(model_path, "w", encoding="utf-8") as f: + f.write(mjcf_content) + logger.info(f"生成默认机械臂模型: {model_path}") + + # 加载MuJoCo模型和数据 + try: + model = mujoco.MjModel.from_xml_path(model_path) + data = mujoco.MjData(model) + logger.info(f"成功加载MuJoCo模型: {model_path}") + return model, data + except Exception as e: + logger.error(f"加载MuJoCo模型失败: {e}") + raise + + def _validate_config(self): + """ + 校验并补全配置参数 + - 保证simulation字段存在 + - 根据模型自动补全目标关节位置维度 + - 设置缺失参数的默认值 + """ + # 确保simulation字段存在 + if "simulation" not in self.config: + self.config["simulation"] = {} + + # 获取模型关节数（nq），无模型时默认1 + nq = self.model.nq if self.model is not None else 1 + + # 补全目标关节位置（维度匹配模型） + self.config["simulation"].setdefault("target_joint_pos", [0.0] * nq) + # 补全最大步数 + self.config["simulation"].setdefault("max_steps", 1000) + # 补全控制频率 + self.config["simulation"].setdefault("control_frequency", 20) + + logger.info("配置校验完成，补全缺失参数") + + def _init_action_space(self): + """ + 初始化动作空间（Gymnasium Box空间） + - 动作维度 = MuJoCo执行器数（nu） + - 动作范围 [-1.0, 1.0]（标准化，便于RL训练） + Raises: + ValueError: 模型未加载时抛出异常 + """ + # 校验模型是否加载 + if self.model is None: + raise ValueError("模型未加载，无法初始化动作空间") + + # 执行器数量（每个执行器对应一个动作维度） + n_actuators = self.model.nu + # 定义动作空间（Box空间，float32类型） + self.action_space = spaces.Box( + low=-1.0, + high=1.0, + shape=(n_actuators,), + dtype=np.float32 + ) + logger.info(f"动作空间初始化完成: 维度={n_actuators}, 范围=[-1.0, 1.0]") + + def _init_observation_space(self): + """ + 初始化观测空间（Gymnasium Box空间） + - 观测维度 = 关节位置数（nq） + 关节速度数（nv） + - 观测范围 [-∞, +∞]（无界，兼容关节任意状态） + Raises: + ValueError: 模型未加载时抛出异常 + """ + # 校验模型是否加载 + if self.model is None: + raise ValueError("模型未加载，无法初始化观测空间") + + # 关节位置数和速度数 + n_qpos = self.model.nq # 关节位置维度 + n_qvel = self.model.nv # 关节速度维度 + obs_dim = n_qpos + n_qvel # 总观测维度 + + # 定义观测空间 + self.observation_space = spaces.Box( + low=-np.inf, + high=np.inf, + shape=(obs_dim,), + dtype=np.float32 + ) + logger.info(f"观测空间初始化完成: 总维度={obs_dim} (位置={n_qpos}, 速度={n_qvel})") + + def _init_render(self): + """ + 初始化渲染组件 + - 仅在render_mode="human"时初始化 + - 使用Pygame创建2D可视化窗口 + """ + if self.render_mode == "human": + try: + import pygame + pygame.init() + # 创建800x600的可视化窗口 + self.screen = pygame.display.set_mode((800, 600)) + pygame.display.set_caption("MuJoCo Arm Simulation") + logger.info("Pygame渲染窗口初始化成功") + except Exception as e: + logger.error(f"Pygame渲染初始化失败: {e}") + raise + + def reset(self, seed: Optional[int] = None) -> Tuple[np.ndarray, dict]: + """ + 重置仿真环境（新回合开始） + Args: + seed: 随机种子（保证复现性） + Returns: + Tuple[np.ndarray, dict]: 初始观测 + 信息字典 + """ + # 设置随机种子 + if seed is not None: + np.random.seed(seed) + logger.info(f"设置随机种子: {seed}") + + # 重置MuJoCo仿真状态（关节位置/速度恢复初始值） + mujoco.mj_resetData(self.model, self.data) + + # 重置计数器和状态标记 + self.step_count = 0 + self.terminated = False + self.truncated = False + self.last_reward = 0.0 + + # 获取初始观测 + obs = self._get_obs() + + # 渲染初始状态 + if self.render_mode == "human": + self.render() + + logger.debug(f"仿真环境重置完成，初始观测维度: {obs.shape}") + return obs, {} + + def step(self, action: np.ndarray) -> Tuple[np.ndarray, float, bool, bool, dict]: + """ + 执行一步仿真（RL核心循环单元） + Args: + action: 动作数组（来自RL策略，范围[-1.0, 1.0]） + Returns: + Tuple[np.ndarray, float, bool, bool, dict]: + 观测 + 奖励 + 终止标记 + 截断标记 + 信息字典 + """ + # 动作预处理（确保类型和范围合法） + action = np.asarray(action, dtype=np.float32) + action = np.clip(action, self.action_space.low, self.action_space.high) + + # 将标准化动作映射到执行器控制信号（放大10倍，匹配电机范围） + self.data.ctrl[:] = action * 10.0 + + # 执行MuJoCo仿真步（物理引擎核心计算） + mujoco.mj_step(self.model, self.data) + + # 更新步数计数器 + self.step_count += 1 + + # 计算奖励（关节位置与目标的误差平方和的负值） + reward = self._compute_reward() + self.last_reward = reward + + # 检查是否自然终止（如关节角度超限） + self.terminated = self._check_terminated() + + # 检查是否截断（达到最大步数） + max_steps = self.config["simulation"].get("max_steps", 1000) + self.truncated = self.step_count >= max_steps + + # 获取当前观测 + obs = self._get_obs() + + # 渲染当前状态 + if self.render_mode == "human": + self.render() + + # 日志输出（每10步打印一次） + if self.step_count % 10 == 0: + logger.debug(f"Step {self.step_count}: Reward={reward:.2f}, Terminated={self.terminated}, Truncated={self.truncated}") + + return obs, reward, self.terminated, self.truncated, {} + + def _get_obs(self) -> np.ndarray: + """ + 获取当前观测（关节位置 + 关节速度） + Returns: + np.ndarray: 观测数组（float32类型） + """ + # 复制关节位置和速度（避免直接修改MuJoCo内部数据） + qpos = self.data.qpos.copy() + qvel = self.data.qvel.copy() + + # 拼接为观测数组 + obs = np.concatenate([qpos, qvel]) + return obs.astype(np.float32) + + def _compute_reward(self) -> float: + """ + 计算奖励函数（目标：让关节位置接近目标值） + - 奖励 = -Σ(当前位置 - 目标位置)² + - 误差越小，奖励越高（最大值0） + Returns: + float: 即时奖励值 + """ + # 获取目标关节位置和当前位置 + target_pos = np.array(self.config["simulation"]["target_joint_pos"]) + current_pos = self.data.qpos.copy() + + # 确保维度匹配（避免广播错误） + if len(target_pos) != len(current_pos): + target_pos = np.zeros_like(current_pos) + + # 计算位置误差的平方和（L2损失） + pos_error = np.sum((current_pos - target_pos) ** 2) + # 奖励为负的误差（鼓励误差减小） + reward = -pos_error + + return float(reward) + + def _check_terminated(self) -> bool: + """ + 检查是否达到自然终止条件 + - 终止条件：任意关节角度超过π弧度（180度） + Returns: + bool: True=终止，False=继续 + """ + joint_pos = self.data.qpos.copy() + + if np.any(np.abs(joint_pos) > np.pi): + logger.info(f"关节角度超限（>π），仿真终止 | 当前关节位置: {joint_pos}") + return True + + return False + + def render(self): + """ + 渲染当前仿真状态（2D Pygame可视化） + - 绘制机械臂2D投影 + - 显示步数、奖励、关节角度等信息 + - 处理窗口事件（关闭/ESC退出） + """ + if self.render_mode == "human" and self.screen is not None: + import pygame + + # 处理Pygame窗口事件 + for event in pygame.event.get(): + # 关闭窗口事件 + if event.type == pygame.QUIT: + self.close() + return + # ESC键退出 + elif event.type == pygame.KEYDOWN: + if event.key == pygame.K_ESCAPE: + self.close() + return + + # 清屏（白色背景） + self.screen.fill((255, 255, 255)) + + # 绘制机械臂2D投影 + # 获取关节角度（单关节机械臂） + joint_angle = self.data.qpos[0] if self.model.nq > 0 else 0 + # 机械臂长度（像素） + arm_length = 200 + # 窗口中心坐标 + center_x, center_y = 400, 300 + # 计算机械臂末端坐标（极坐标转笛卡尔） + end_x = center_x + arm_length * np.cos(joint_angle) + end_y = center_y + arm_length * np.sin(joint_angle) + + # 绘制机械臂连杆（黑色线条，宽度5） + pygame.draw.line( + self.screen, (0, 0, 0), + (center_x, center_y), (end_x, end_y), 5 + ) + # 绘制关节（红色圆心，半径10） + pygame.draw.circle( + self.screen, (255, 0, 0), + (int(center_x), int(center_y)), 10 + ) + # 绘制末端执行器（蓝色圆心，半径8） + pygame.draw.circle( + self.screen, (0, 0, 255), + (int(end_x), int(end_y)), 8 + ) + + # 绘制文本信息（步数、奖励、关节角度） + font = pygame.font.Font(None, 36) + # 步数文本 + step_text = font.render(f"Step: {self.step_count}", True, (0, 0, 0)) + # 奖励文本 + reward_text = font.render(f"Reward: {self.last_reward:.2f}", True, (0, 0, 0)) + # 关节角度文本 + angle_text = font.render(f"Joint Angle: {joint_angle:.2f} rad", True, (0, 0, 0)) + + # 绘制文本到窗口 + self.screen.blit(step_text, (10, 10)) + self.screen.blit(reward_text, (10, 50)) + self.screen.blit(angle_text, (10, 90)) + + # 更新窗口显示 + pygame.display.flip() + + # 控制渲染帧率（匹配控制频率） + control_freq = self.config["simulation"].get("control_frequency", 20) + pygame.time.delay(int(1000 / control_freq)) + + def close(self): + """ + 关闭仿真环境（清理资源） + - 关闭Pygame窗口 + - 释放所有渲染资源 + """ + if self.render_mode == "human": + try: + import pygame + pygame.quit() + logger.info("Pygame窗口已关闭") + except Exception as e: + logger.warning(f"关闭Pygame失败: {e}") + + +# ---------------- 测试代码（独立运行时执行）---------------- +if __name__ == "__main__": + # 初始化模拟器（存储路径：./mujoco_arm，开启可视化） + sim = Simulator(render_mode="human", simulator_folder="./mujoco_arm") + + # 重置环境（设置随机种子保证复现） + obs, info = sim.reset(seed=42) + print(f"初始观测形状: {obs.shape}") + + # 运行100步仿真 + for _ in range(100): + # 随机采样动作（从动作空间中） + action = sim.action_space.sample() + # 执行一步仿真 + obs, reward, terminated, truncated, info = sim.step(action) + + # 每10步打印状态 + if _ % 10 == 0: + print(f"Step: {sim.step_count}, Reward: {reward:.3f}") + + # 终止/截断时退出循环 + if terminated or truncated: + print(f"仿真在第 {sim.step_count} 步结束") + break + + # 关闭模拟器（清理资源） + sim.close() From c0a700408e911a0cdfa8df2de42930772abc3500 Mon Sep 17 00:00:00 2001 From: lyy-1236 <3584247842@qq.com> Date: Sun, 7 Jun 2026 21:42:59 +0800 Subject: [PATCH 5/9] Create main.py --- src/dua_arm/main.py | 857 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 857 insertions(+) create mode 100644 src/dua_arm/main.py diff --git a/src/dua_arm/main.py b/src/dua_arm/main.py new file mode 100644 index 0000000000..e78248c3f9 --- /dev/null +++ b/src/dua_arm/main.py @@ -0,0 +1,857 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +双机械臂协同操作仿真系统主程序 + +模块名称: dual_arm_simulator.main +功能描述: 实现双机械臂协同搬运任务的完整仿真环境 +核心功能: + - 双机械臂生物力学模型仿真 + - 协同搬运任务状态管理 + - 多策略控制（正弦波/目标跟踪） + - 结果分析与可视化 + +依赖库: + - numpy: 数值计算和矩阵运算 + - matplotlib: 可视化绘图 + - pyyaml: 配置文件解析 + - pathlib: 跨平台路径处理 + +运行环境: + - Python 3.7+ + - 需要自定义模块: dual_arm_bm_model, cooperative_task, + perception_module, visualization +""" + +import numpy as np +import matplotlib.pyplot as plt +import yaml +import os +import sys +from pathlib import Path +import time +from datetime import datetime +from typing import Tuple, Dict, Any, Optional, List # 类型注解 + +# 添加当前目录到Python路径，确保能导入自定义模块 +sys.path.append(os.path.dirname(os.path.abspath(__file__))) + +# 导入自定义模块 +from dual_arm_bm_model import DualArmBMModel # 双机械臂生物力学模型 +from cooperative_task import CooperativeTransportTask # 协同搬运任务 +from perception_module import DualEndEffectorPerception # 末端感知模块 +from visualization import DualArmVisualizer # 可视化工具 + + +class DualArmSimulator: + """ + 双机械臂协同操作仿真器 + + 功能说明: + 整合机械臂模型、任务环境和感知模块，提供完整的仿真环境。 + 支持多种控制策略，自动记录仿真数据并生成可视化结果。 + + 属性: + config (dict): 仿真配置参数 + bm_model (DualArmBMModel): 双机械臂生物力学模型 + task (CooperativeTransportTask): 协同搬运任务 + perception (DualEndEffectorPerception): 末端感知模块 + dt (float): 仿真时间步长（秒） + max_steps (int): 最大仿真步数 + states (list): 状态历史记录 + actions (list): 动作历史记录 + rewards (list): 奖励历史记录 + results_dir (Path): 结果保存目录 + + 使用示例: + >>> simulator = DualArmSimulator('config.yaml') + >>> simulator.reset() + >>> for step in range(max_steps): + ... left_action, right_action = policy(step) + ... obs, reward, terminated, info = simulator.step(left_action, right_action) + ... if terminated: break + >>> stats = simulator.analyze_results() + >>> simulator.visualize_all() + """ + + def __init__(self, config_path: str = "config.yaml"): + """ + 初始化双机械臂仿真器 + + 参数: + config_path (str): 配置文件路径，默认为'config.yaml' + + 初始化流程: + 1. 加载YAML配置文件 + 2. 打印系统信息 + 3. 初始化各功能模块 + 4. 创建结果保存目录 + 5. 验证配置完整性 + """ + # ----- 1. 加载配置文件 ----- + try: + with open(config_path, 'r', encoding='utf-8') as f: + self.config = yaml.safe_load(f) + except FileNotFoundError: + raise FileNotFoundError(f"配置文件不存在: {config_path}") + except yaml.YAMLError as e: + raise ValueError(f"配置文件格式错误: {e}") + + # ----- 2. 打印系统标题 ----- + print("=" * 60) + print("双机械臂协同操作仿真系统") + print("=" * 60) + + # ----- 3. 初始化各模块 ----- + # 3.1 生物力学模型 + bm_kwargs = self.config['simulation']['bm_model']['kwargs'] + self.bm_model = DualArmBMModel(bm_kwargs) + + # 3.2 协同搬运任务 + task_kwargs = self.config['simulation']['task']['kwargs'] + self.task = CooperativeTransportTask(task_kwargs) + + # 3.3 末端感知模块（取第一个感知模块配置） + perception_kwargs = self.config['simulation']['perception_modules'][0]['kwargs'] + self.perception = DualEndEffectorPerception(perception_kwargs) + + # ----- 4. 提取仿真参数 ----- + self.dt = self.config['simulation']['run_parameters']['dt'] + self.max_steps = self.config['simulation']['task']['kwargs']['max_steps'] + + # ----- 5. 初始化数据记录容器 ----- + self.states = [] # 状态历史: 每个元素包含机械臂状态、物体位置等 + self.actions = [] # 动作历史: 左右臂的动作序列 + self.rewards = [] # 奖励历史: 每步的即时奖励 + + # ----- 6. 创建结果保存目录 ----- + self.results_dir = Path("results") + self.results_dir.mkdir(exist_ok=True) # 主结果目录 + (self.results_dir / "frames").mkdir(exist_ok=True) # 帧图像目录 + (self.results_dir / "videos").mkdir(exist_ok=True) # 视频目录 + + # ----- 7. 打印初始化完成信息 ----- + print("✓ 系统初始化完成") + print(f"✓ 时间步长: {self.dt}秒") + print(f"✓ 最大步数: {self.max_steps}") + print(f"✓ 结果目录: {self.results_dir.absolute()}") + + def reset(self) -> None: + """ + 重置仿真环境到初始状态 + + 功能: + 重置所有模块（机械臂、任务、感知）到初始状态， + 清空历史记录，记录初始状态快照。 + + 重置内容: + - 机械臂关节角度归零 + - 物体位置重置到初始位置 + - 清空所有历史记录 + - 记录初始状态作为第一帧 + + 使用场景: + - 开始新的仿真实验前 + - 训练强化学习智能体的episode开始时 + - 测试不同策略前 + + 示例: + >>> simulator.reset() + >>> print(len(simulator.states)) # 输出: 1（初始状态） + """ + # 重置各模块 + self.bm_model.reset() # 重置机械臂姿态 + self.task.reset() # 重置任务状态（物体位置、抓取标志等） + self.perception.reset() # 重置感知模块 + + # 清空历史记录 + self.states = [] + self.actions = [] + self.rewards = [] + + # 记录初始状态快照 + initial_state = { + 'left_arm': self.bm_model.left_arm, # 左臂状态 + 'right_arm': self.bm_model.right_arm, # 右臂状态 + 'object': self.task.state.object_position # 物体位置 + } + self.states.append(initial_state) + + print("✓ 仿真已重置") + + def step(self, left_action: np.ndarray, right_action: np.ndarray) -> Tuple[np.ndarray, float, bool, Dict]: + """ + 执行一步仿真推进 + + 功能: + 根据输入的动作指令，更新机械臂状态、任务状态， + 计算奖励并判断是否终止。 + + 参数: + left_action (np.ndarray): 左臂动作指令，形状(3,)，范围[-1, 1] + right_action (np.ndarray): 右臂动作指令，形状(3,)，范围[-1, 1] + + 返回: + Tuple包含4个元素: + observation (np.ndarray): 观测数据（末端位置、物体位置等） + reward (float): 即时奖励值 + terminated (bool): 是否终止（抓取成功或失败） + info (dict): 额外信息（如抓取状态、距离误差等） + + 执行流程: + 1. 动作限幅（确保在安全范围内） + 2. 更新生物力学模型 + 3. 获取当前末端位置 + 4. 更新任务状态 + 5. 获取感知观测 + 6. 记录数据 + 7. 返回结果 + + 示例: + >>> left_action = np.array([0.5, 0.3, 0.2]) + >>> right_action = np.array([-0.5, -0.3, -0.2]) + >>> obs, reward, terminated, info = simulator.step(left_action, right_action) + >>> print(f"Reward: {reward:.3f}, Terminated: {terminated}") + """ + # ----- 1. 动作限幅（防止控制量过大）----- + # 限制动作值在[-1.0, 1.0]范围内，对应关节角度/力度的限制 + left_action = np.clip(left_action, -1.0, 1.0) + right_action = np.clip(right_action, -1.0, 1.0) + + # ----- 2. 更新生物力学模型 ----- + # 根据动作指令和时间步长更新机械臂状态 + self.bm_model.update(left_action, right_action, self.dt) + + # ----- 3. 获取当前末端位置 ----- + # 末端位置用于任务更新和感知 + left_pos = self.bm_model.left_arm.end_effector_pos + right_pos = self.bm_model.right_arm.end_effector_pos + + # ----- 4. 更新任务状态 ----- + # 计算奖励、检查终止条件、获取任务信息 + reward, terminated, info = self.task.update( + left_pos, right_pos, left_action, right_action, self.dt + ) + + # ----- 5. 获取感知观测 ----- + # 综合末端位置和物体位置，生成观测向量 + observation = self.perception.get_observation( + left_pos, right_pos, self.task.state.object_position + ) + + # ----- 6. 记录数据 ----- + # 保存动作和奖励历史 + self.actions.append((left_action.copy(), right_action.copy())) + self.rewards.append(reward) + + # 保存当前状态快照 + current_state = { + 'left_arm': self.bm_model.left_arm, + 'right_arm': self.bm_model.right_arm, + 'object': self.task.state.object_position.copy(), + 'observation': observation, + 'info': info + } + self.states.append(current_state) + + return observation, reward, terminated, info + + def run_simulation(self, policy_type: str = "sinusoidal") -> Tuple[int, float]: + """ + 运行完整的仿真过程 + + 功能: + 根据指定的控制策略，运行从重置到终止的完整仿真。 + 支持多种内置策略，可用于演示和测试。 + + 参数: + policy_type (str): 控制策略类型 + - "sinusoidal": 正弦波控制（演示用） + - "tracking": 目标跟踪控制（PD控制器） + + 返回: + Tuple[int, float]: (总步数, 总奖励) + + 执行流程: + 1. 重置仿真环境 + 2. 选择控制策略 + 3. 循环执行step直到终止或达到最大步数 + 4. 打印仿真结果 + 5. 返回统计信息 + + 示例: + >>> steps, total_reward = simulator.run_simulation("tracking") + >>> print(f"完成 {steps} 步，总奖励 {total_reward:.3f}") + """ + print("\n" + "=" * 60) + print("开始双机械臂协同操作仿真") + print("=" * 60) + + # 重置环境到初始状态 + self.reset() + + # ----- 定义控制策略函数 ----- + + def sinusoidal_policy(step: int) -> Tuple[np.ndarray, np.ndarray]: + """ + 正弦波控制策略（演示用） + + 功能: + 生成正弦波形式的周期性动作，用于测试机械臂运动范围。 + 左右臂相位相反，模拟协同运动。 + + 参数: + step (int): 当前步数 + + 返回: + Tuple[np.ndarray, np.ndarray]: (左臂动作, 右臂动作) + + 数学公式: + 左臂: A * sin(2πft + φ) + 右臂: A * sin(2πft + π + φ) + """ + t = step * self.dt + freq = 1.0 # 振荡频率 (Hz) + + # 左臂动作：三个关节的正弦波，相位递增 + left_action = np.array([ + 0.5 * np.sin(2 * np.pi * freq * t), # 关节1 + 0.3 * np.sin(2 * np.pi * freq * t + np.pi/3), # 关节2（相位偏移60°） + 0.2 * np.sin(2 * np.pi * freq * t + 2*np.pi/3) # 关节3（相位偏移120°） + ]) + + # 右臂动作：整体相位偏移π（180°），与左臂形成对称运动 + right_action = np.array([ + 0.5 * np.sin(2 * np.pi * freq * t + np.pi), # 关节1 + 0.3 * np.sin(2 * np.pi * freq * t + np.pi + np.pi/3), # 关节2 + 0.2 * np.sin(2 * np.pi * freq * t + np.pi + 2*np.pi/3) # 关节3 + ]) + + return left_action, right_action + + def target_tracking_policy(step: int) -> Tuple[np.ndarray, np.ndarray]: + """ + 目标跟踪控制策略（PD控制器） + + 功能: + 根据目标位置与当前末端位置的误差，使用比例控制 + 生成动作指令，使机械臂跟踪移动目标。 + + 参数: + step (int): 当前步数 + + 返回: + Tuple[np.ndarray, np.ndarray]: (左臂动作, 右臂动作) + + 控制原理: + 动作 = Kp * (目标位置 - 当前位置) + 其中 Kp 为比例增益系数 + + 目标轨迹: + 目标位置在初始位置附近做正弦运动 + """ + t = step * self.dt + + # 生成左臂移动目标（围绕初始位置做正弦运动） + target_left = self.task.target_left + np.array([ + 0.1 * np.sin(2 * np.pi * 0.2 * t), # X轴方向移动 ±0.1m + 0.0, # Y轴方向不移动 + 0.05 * np.sin(2 * np.pi * 0.3 * t) # Z轴方向移动 ±0.05m + ]) + + # 生成右臂移动目标（与左臂对称运动） + target_right = self.task.target_right + np.array([ + -0.1 * np.sin(2 * np.pi * 0.2 * t), # X轴方向相反相位 + 0.0, + 0.05 * np.sin(2 * np.pi * 0.3 * t) # Z轴方向相同相位 + ]) + + # 获取当前末端位置 + current_left = self.bm_model.left_arm.end_effector_pos + current_right = self.bm_model.right_arm.end_effector_pos + + # 计算位置误差 + left_error = target_left - current_left + right_error = target_right - current_right + + # 比例控制器（P Controller） + kp = 2.0 # 比例增益，控制响应速度 + + # 只取前三个分量（位置控制），转换为动作指令 + left_action = kp * left_error[:3] + right_action = kp * right_error[:3] + + return left_action, right_action + + # ----- 选择策略 ----- + if policy_type == "sinusoidal": + policy = sinusoidal_policy + print(f"\n使用策略: 正弦波控制（演示模式）") + elif policy_type == "tracking": + policy = target_tracking_policy + print(f"\n使用策略: 目标跟踪控制（PD控制器）") + else: + raise ValueError(f"未知策略类型: {policy_type}. 支持的类型: 'sinusoidal', 'tracking'") + + # ----- 仿真主循环 ----- + terminated = False + step = 0 + total_reward = 0.0 + + # 打印表头 + print(f"{'Step':>6} {'Left Pos':>20} {'Right Pos':>20} {'Reward':>10} {'Terminated':>10}") + print("-" * 80) + + # 循环执行，直到终止或达到最大步数 + while not terminated and step < self.max_steps: + # 根据策略生成动作 + left_action, right_action = policy(step) + + # 执行一步仿真 + observation, reward, terminated, info = self.step(left_action, right_action) + + # 累加总奖励 + total_reward += reward + + # 每100步打印一次状态（避免输出过多） + if step % 100 == 0: + left_pos = self.bm_model.left_arm.end_effector_pos + right_pos = self.bm_model.right_arm.end_effector_pos + print(f"{step:6d} {str(left_pos.round(2)):>20} {str(right_pos.round(2)):>20} " + f"{reward:10.3f} {str(terminated):>10}") + + step += 1 + + # ----- 打印仿真结果 ----- + print("-" * 80) + print(f"仿真完成!") + print(f"总步数: {step}") + print(f"总奖励: {total_reward:.3f}") + print(f"是否抓取成功: {self.task.state.is_grasped}") + print(f"物体最终位置: {self.task.state.object_position.round(3)}") + + return step, total_reward + + def analyze_results(self) -> Dict[str, Any]: + """ + 分析仿真结果并计算统计指标 + + 功能: + 从记录的状态历史中提取数据，计算各种性能指标： + 路径长度、最大速度、协同度、奖励统计等。 + + 返回: + Dict[str, Any]: 包含所有统计指标的字典 + + 统计指标: + - left_path_length: 左臂运动路径总长度（米） + - right_path_length: 右臂运动路径总长度（米） + - left_max_speed: 左臂最大速度（米/秒） + - right_max_speed: 右臂最大速度（米/秒） + - coordination_index: 协同度指标 [0,1]，越高表示协同越好 + - mean_reward: 平均奖励 + - total_reward: 总奖励 + - total_steps: 总步数 + - success: 任务是否成功 + - final_object_position: 物体最终位置 + - timestamp: 分析时间戳 + + 协同度指标算法: + coordination = 0.5 * (1/(1+距离标准差)) + 0.5 * (平均余弦相似度+1)/2 + 其中: + - 距离标准差: 双手间距的稳定性 + - 余弦相似度: 双手运动方向的一致性 + + 示例: + >>> stats = simulator.analyze_results() + >>> print(f"协同度: {stats['coordination_index']:.3f}") + >>> print(f"成功率: {'成功' if stats['success'] else '失败'}") + """ + print("\n" + "=" * 60) + print("仿真结果分析") + print("=" * 60) + + # ----- 提取轨迹数据 ----- + # 从状态历史中提取末端位置和物体位置 + left_positions = np.array([s['left_arm'].end_effector_pos for s in self.states]) + right_positions = np.array([s['right_arm'].end_effector_pos for s in self.states]) + object_positions = np.array([s['object'] for s in self.states]) + rewards = np.array(self.rewards) + + # ----- 计算路径长度 ----- + # 路径长度 = 各段位移的累加和 + left_path_length = np.sum(np.linalg.norm(np.diff(left_positions, axis=0), axis=1)) + right_path_length = np.sum(np.linalg.norm(np.diff(right_positions, axis=0), axis=1)) + + # ----- 计算最大速度 ----- + # 速度 = 位移 / 时间步长 + left_velocities = np.diff(left_positions, axis=0) / self.dt + right_velocities = np.diff(right_positions, axis=0) / self.dt + + left_max_speed = np.max(np.linalg.norm(left_velocities, axis=1)) + right_max_speed = np.max(np.linalg.norm(right_velocities, axis=1)) + + # ----- 计算协同度指标 ----- + coordination_index = self._calculate_coordination_index(left_positions, right_positions) + + # ----- 打印分析结果 ----- + print(f"左机械臂路径长度: {left_path_length:.3f} m") + print(f"右机械臂路径长度: {right_path_length:.3f} m") + print(f"左机械臂最大速度: {left_max_speed:.3f} m/s") + print(f"右机械臂最大速度: {right_max_speed:.3f} m/s") + print(f"协同度指标: {coordination_index:.3f}") + print(f"平均奖励: {np.mean(rewards):.3f}") + print(f"总奖励: {np.sum(rewards):.3f}") + + # ----- 保存统计数据到YAML文件 ----- + stats = { + 'left_path_length': float(left_path_length), + 'right_path_length': float(right_path_length), + 'left_max_speed': float(left_max_speed), + 'right_max_speed': float(right_max_speed), + 'coordination_index': float(coordination_index), + 'mean_reward': float(np.mean(rewards)), + 'total_reward': float(np.sum(rewards)), + 'total_steps': len(self.states), + 'success': bool(self.task.state.is_grasped), + 'final_object_position': self.task.state.object_position.tolist(), + 'timestamp': datetime.now().isoformat() + } + + # 保存为YAML格式，便于阅读和后续分析 + stats_path = self.results_dir / "simulation_stats.yaml" + with open(stats_path, 'w', encoding='utf-8') as f: + yaml.dump(stats, f, default_flow_style=False, allow_unicode=True) + + print(f"✓ 统计数据已保存至: {stats_path}") + + return stats + + def _calculate_coordination_index(self, left_pos: np.ndarray, right_pos: np.ndarray) -> float: + """ + 计算双机械臂协同度指标 + + 功能: + 评估左右机械臂运动的协同程度，综合考虑： + 1. 双手距离的稳定性（标准差） + 2. 运动方向的一致性（余弦相似度） + + 参数: + left_pos (np.ndarray): 左臂末端位置历史，形状(N, 3) + right_pos (np.ndarray): 右臂末端位置历史，形状(N, 3) + + 返回: + float: 协同度指标，范围[0, 1] + - 1.0: 完美协同（理想情况） + - 0.5: 中等协同 + - 0.0: 完全无协同 + + 算法细节: + 1. 距离稳定性 = 1 / (1 + 距离标准差) + 距离标准差越小，稳定性越高 + + 2. 方向一致性 = (平均余弦相似度 + 1) / 2 + 余弦相似度范围[-1,1]，归一化到[0,1] + + 3. 最终协同度 = 0.5 * 距离稳定性 + 0.5 * 方向一致性 + + 注意: + 需要至少2个时间步才能计算速度方向， + 如果轨迹长度不足，返回默认值0.5 + """ + # 计算双手欧氏距离序列 + distances = np.linalg.norm(left_pos - right_pos, axis=1) + distance_std = np.std(distances) # 标准差越小，距离越稳定 + + # 计算运动速度方向 + left_vel = np.diff(left_pos, axis=0) # 左臂速度向量 + right_vel = np.diff(right_pos, axis=0) # 右臂速度向量 + + # 计算速度方向余弦相似度 + if len(left_vel) > 0: + cos_similarities = [] + for lv, rv in zip(left_vel, right_vel): + # 避免除以零 + if np.linalg.norm(lv) > 0.001 and np.linalg.norm(rv) > 0.001: + # 余弦相似度 = (A·B) / (|A|·|B|) + cos_sim = np.dot(lv, rv) / (np.linalg.norm(lv) * np.linalg.norm(rv)) + cos_similarities.append(cos_sim) + + if cos_similarities: + mean_cos_sim = np.mean(cos_similarities) + else: + mean_cos_sim = 0 + else: + mean_cos_sim = 0 + + # 归一化余弦相似度到[0,1]范围 + # 原始范围[-1,1] → 映射到[0,1] + normalized_cos_sim = (mean_cos_sim + 1) / 2 + + # 距离稳定性指标：标准差越小，稳定性越高 + # 使用 1/(1+σ) 确保结果在(0,1]范围内 + distance_stability = 1.0 / (1.0 + distance_std) + + # 综合协同度：距离稳定性 + 方向一致性（各占50%） + coordination = 0.5 * distance_stability + 0.5 * normalized_cos_sim + + return coordination + + def visualize_all(self) -> None: + """ + 生成所有可视化结果 + + 功能: + 调用各模块的可视化方法，生成： + 1. 轨迹图（机械臂运动轨迹） + 2. 任务可视化（物体位置、抓取状态） + 3. 动画视频（完整运动过程） + 4. 性能图表（奖励曲线、误差曲线等） + + 输出文件: + - results/trajectory_plot.png + - results/task_visualization.png + - results/videos/dual_arm_animation.mp4 + - results/performance_analysis.png + + 使用场景: + - 仿真完成后自动生成报告 + - 分析机械臂运动规律 + - 制作演示视频 + """ + print("\n" + "=" * 60) + print("生成可视化结果") + print("=" * 60) + + # 1. 生成机械臂轨迹图 + trajectory_plot_path = self.results_dir / "trajectory_plot.png" + self.bm_model.plot_trajectory(str(trajectory_plot_path)) + + # 2. 生成任务可视化（物体、抓取点等） + task_viz_path = self.results_dir / "task_visualization.png" + self.task.visualize(self.bm_model.trajectory, str(task_viz_path)) + + # 3. 创建动画视频（需要ffmpeg支持） + animation_path = self.results_dir / "videos" / "dual_arm_animation.mp4" + self.task.create_animation(self.bm_model.trajectory, str(animation_path)) + + # 4. 生成性能分析图表 + self._plot_performance() + + # 打印输出文件位置 + print(f"✓ 轨迹图: {trajectory_plot_path}") + print(f"✓ 任务可视化: {task_viz_path}") + print(f"✓ 动画视频: {animation_path}") + + def _plot_performance(self) -> None: + """ + 绘制性能分析图表（内部方法） + + 功能: + 创建4个子图，分别显示： + 1. 奖励曲线（即时奖励和累积奖励） + 2. 位置误差（左右手到物体的距离） + 3. 关节角度变化（6个关节的时间序列） + 4. 双手间距（协同度指标） + + 图表特点: + - 4个子图布局：2x2网格 + - 包含图例、网格、阈值线 + - 高分辨率输出（150 DPI） + + 输出文件: + results/performance_analysis.png + """ + # 创建2x2子图布局，总尺寸15x10英寸 + fig = plt.figure(figsize=(15, 10)) + + # ----- 子图1: 奖励曲线 ----- + ax1 = fig.add_subplot(221) + rewards = np.array(self.rewards) + cumulative_rewards = np.cumsum(rewards) # 累积奖励 + + ax1.plot(rewards, 'b-', alpha=0.7, linewidth=1, label='即时奖励') + ax1.plot(cumulative_rewards, 'r-', linewidth=2, label='累积奖励') + ax1.axhline(y=0, color='k', linestyle='-', alpha=0.3) # 零线 + ax1.set_xlabel('时间步') + ax1.set_ylabel('奖励值') + ax1.set_title('奖励随时间变化') + ax1.legend() + ax1.grid(True, alpha=0.3) + + # ----- 子图2: 位置误差曲线 ----- + ax2 = fig.add_subplot(222) + left_errors = self.task.history['left_errors'] # 左臂到物体的距离 + right_errors = self.task.history['right_errors'] # 右臂到物体的距离 + + ax2.plot(left_errors, 'r-', linewidth=1.5, label='左臂误差') + ax2.plot(right_errors, 'b-', linewidth=1.5, label='右臂误差') + ax2.axhline(y=self.task.grasp_distance, color='g', linestyle='--', + linewidth=1.5, label='抓取阈值') + ax2.set_xlabel('时间步') + ax2.set_ylabel('到物体的距离 (m)') + ax2.set_title('位置误差') + ax2.legend() + ax2.grid(True, alpha=0.3) + + # ----- 子图3: 关节角度曲线 ----- + ax3 = fig.add_subplot(223) + # 提取左右臂的所有关节角度 + left_joints = np.array([s['left_arm'].joint_positions for s in self.states]) + right_joints = np.array([s['right_arm'].joint_positions for s in self.states]) + + # 绘制左臂关节（实线）和右臂关节（虚线） + for i in range(3): # 假设每个臂有3个关节 + ax3.plot(left_joints[:, i], f'C{i}-', alpha=0.7, linewidth=1.5, + label=f'左臂关节{i+1}') + ax3.plot(right_joints[:, i], f'C{i}--', alpha=0.7, linewidth=1.5, + label=f'右臂关节{i+1}') + + ax3.set_xlabel('时间步') + ax3.set_ylabel('关节角度 (rad)') + ax3.set_title('关节角度变化') + ax3.legend(ncol=2, fontsize=9) + ax3.grid(True, alpha=0.3) + + # ----- 子图4: 双手间距（协同度指标）----- + ax4 = fig.add_subplot(224) + + # 计算双手之间的欧氏距离 + left_pos = np.array([s['left_arm'].end_effector_pos for s in self.states]) + right_pos = np.array([s['right_arm'].end_effector_pos for s in self.states]) + hand_distances = np.linalg.norm(left_pos - right_pos, axis=1) + + ax4.plot(hand_distances, 'purple', linewidth=2, label='双手距离') + ax4.axhline(y=0.2, color='g', linestyle='--', linewidth=1.5, + label='理想距离 (0.2m)') + ax4.set_xlabel('时间步') + ax4.set_ylabel('双手间距 (m)') + ax4.set_title('双手间距 - 协同度指标') + ax4.legend() + ax4.grid(True, alpha=0.3) + + # 添加总标题 + plt.suptitle('双机械臂协同搬运性能分析', fontsize=14, fontweight='bold') + + # 调整子图间距，避免重叠 + plt.tight_layout() + + # 保存图片 + performance_path = self.results_dir / "performance_analysis.png" + plt.savefig(str(performance_path), dpi=150, bbox_inches='tight') + plt.show() + + print(f"✓ 性能分析图: {performance_path}") + + +def main() -> int: + """ + 主函数：程序入口 + + 功能: + 1. 解析命令行参数（可选） + 2. 创建仿真器实例 + 3. 交互式选择控制策略 + 4. 运行仿真 + 5. 分析结果并生成可视化 + 6. 保存仿真数据 + + 返回: + int: 退出码 + - 0: 正常完成 + - 1: 运行出错 + + 使用示例: + $ python main.py + 请选择控制策略: + 1. 正弦波控制 (演示) + 2. 目标跟踪控制 + 请输入选择 (1 或 2): 2 + """ + print("\n" + "=" * 60) + print("双机械臂协同操作仿真系统") + print("=" * 60) + + try: + # ----- 1. 创建仿真器实例 ----- + # 默认读取当前目录下的 config.yaml 文件 + simulator = DualArmSimulator() + + # ----- 2. 选择控制策略 ----- + print("\n请选择控制策略:") + print("1. 正弦波控制 (演示模式)") + print("2. 目标跟踪控制 (PD控制器)") + + choice = input("请输入选择 (1 或 2): ").strip() + + if choice == "1": + policy_type = "sinusoidal" + print("\n已选择: 正弦波控制模式") + elif choice == "2": + policy_type = "tracking" + print("\n已选择: 目标跟踪控制模式") + else: + print("输入无效，使用默认策略: 正弦波控制") + policy_type = "sinusoidal" + + # ----- 3. 运行仿真 ----- + steps, total_reward = simulator.run_simulation(policy_type) + + # ----- 4. 分析结果 ----- + stats = simulator.analyze_results() + + # ----- 5. 生成可视化 ----- + simulator.visualize_all() + + # ----- 6. 保存仿真数据 ----- + # 使用压缩格式保存numpy数组，节省空间 + data_path = simulator.results_dir / "simulation_data.npz" + np.savez_compressed( + str(data_path), + states=simulator.states, + actions=simulator.actions, + rewards=simulator.rewards, + config=simulator.config + ) + print(f"\n✓ 仿真数据已保存至: {data_path}") + + # ----- 7. 打印最终总结 ----- + print("\n" + "=" * 60) + print("仿真完成!") + print(f"总步数: {steps}") + print(f"总奖励: {total_reward:.3f}") + print(f"协同度: {stats['coordination_index']:.3f}") + print(f"任务成功: {'✓ 是' if stats['success'] else '✗ 否'}") + print("=" * 60) + + # 列出所有生成的结果文件 + print("\n生成的结果文件:") + for file in sorted(simulator.results_dir.rglob("*")): + if file.is_file(): + size_mb = file.stat().st_size / (1024 * 1024) + print(f" - {file.relative_to(simulator.results_dir)} ({size_mb:.2f} MB)") + + except FileNotFoundError as e: + print(f"\n❌ 文件未找到: {e}") + print("请确保配置文件 config.yaml 存在于当前目录") + return 1 + except KeyError as e: + print(f"\n❌ 配置错误: 缺少必要的配置项 {e}") + print("请检查 config.yaml 文件的完整性") + return 1 + except Exception as e: + print(f"\n❌ 运行出错: {e}") + import traceback + traceback.print_exc() + return 1 + + return 0 + + +if __name__ == "__main__": + """ + 程序入口点 + + 当脚本被直接运行时（而不是作为模块导入），执行 main() 函数 + """ + sys.exit(main()) From d42f2513fa81869cfb3d31fdb689bcd997b5c9af Mon Sep 17 00:00:00 2001 From: lyy-1236 <3584247842@qq.com> Date: Thu, 11 Jun 2026 22:37:05 +0800 Subject: [PATCH 6/9] Create readme.md --- readme.md | 152 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 152 insertions(+) create mode 100644 readme.md diff --git a/readme.md b/readme.md new file mode 100644 index 0000000000..caa773778b --- /dev/null +++ b/readme.md @@ -0,0 +1,152 @@ + +# box — 仿真与强化学习实验箱 + +## 概述 + +`src/box` 包含若干用于仿真、感知与强化学习实验的环境与辅助脚本。主要面向基于 Gymnasium/MuJoCo 的任务、轻量级可视化示例以及调试/测试工具。 + +Gymnasium：一个操作台，是网上的一个强化学习的开源项目，给你提供一套通用的指令，可以完成一些基础操作。并提供给你进一步写指令代码的框架。 + +MuJoCo：一个物理仿真平台，计算你的指令在现实中执行后可能产生什么结果。 + +在这个项目中是一起用，如果只是写一些小例子，可以只用Gymnasium,它的内部也包含了一些现成的环境。 + +本目录旨在提供： + +- 可复现的仿真实验环境模板； +- 启动/调试脚本示例； +- 与主仓库其余模块交互的接口说明（若存在）。 + +## 推荐目录结构（示例） + +- `simulator.py`：仿真环境实现（通常继承自 `gym.Env` 或 `gymnasium.Env`）； +- `envs/`：若干子环境模块或封装； +- `agents/`：示例智能体或策略实现（可选）； +- `scripts/`：运行/训练/评估脚本（如 `run.py`, `train.py`, `eval.py`）； +- `tests/`：小型示例与单元/集成测试脚本（如 `test_simulator.py`）； +- `configs/`：默认配置或参数文件（YAML/JSON）； +- `README.md`：本文件。 + +（实际文件以仓库中为准，此处为常见约定。） + +## 快速上手 + +以下示例以 Windows + PowerShell 为例： + +1) 进入项目根目录并创建虚拟环境： + +```powershell +cd D:\\nn +python -m venv .venv +.\\.venv\\Scripts\\Activate.ps1 +``` + +2) 安装依赖： + +```powershell +pip install -r requirements.txt -i https://pypi.tuna.tsinghua.edu.cn/simple +``` + +如果仓库未包含完整的 `requirements.txt`，可参考核心依赖： + +``` +gymnasium +mujoco +stable-baselines3 +pygame +opencv-python +numpy +scipy +matplotlib +ruamel.yaml +``` + +3) 运行示例仿真（示例脚本名以仓库实际文件为准）： + +```powershell +python scripts/run.py +``` + +或： + +```powershell +python tests/test_simulator.py +``` + +运行后通常会在本地弹出可视化窗口或在终端打印日志，具体行为视环境实现而定。 + +## 开发与调试提示 + +- MuJoCo：请确保已正确安装 MuJoCo 及其许可证（若使用 mujoco 依赖）。 +- 显示/渲染：在无显示的服务器上运行时，可能需要配置虚拟帧缓冲（Linux 下使用 xvfb）。 +- 依赖冲突：若出现版本不兼容，建议使用虚拟环境并锁定依赖版本。 + +## 如何贡献 + +- 修改或补充说明、示例脚本或配置后提交 Pull Request； +- 提交 Issue 时请包含：操作系统、Python 版本、依赖版本与复现步骤/错误日志。 + +## 参考与更多信息 + +- 查看目录下的具体脚本与模块顶部注释，通常包含使用示例与参数说明； +- 若需要，我可以为 `src/box` 中的主要文件生成更详细的文档或示例运行脚本。 + +**box — 仿真与强化学习实验箱** + +简介 +- `src/box` 目录包含基于 Gymnasium 和 MuJoCo 的仿真环境与相关辅助脚本，用于开发和测试生物力学/机器人仿真、感知模块与强化学习任务。 + +目录结构（示例） +- `simulator.py`：仿真环境核心（通常继承 `gym.Env`）。 +- `test_simulator.py`：示例运行脚本，用于启动仿真并可视化。 +- `main.py`：辅助脚本（例如证书或配置检查）。 +- `README.md`：本文件，说明目录用途与快速上手指南。 + +快速上手 +1. 创建并激活虚拟环境（以 Windows 为例）： + +```powershell +cd <项目根目录> +python -m venv venv --python=3.9 +.\\venv\\Scripts\\Activate.ps1 +``` + +2. 安装依赖（建议使用清华镜像加速）： + +```powershell +pip install -r requirements.txt -i https://pypi.tuna.tsinghua.edu.cn/simple +``` + +如果仓库没有完整的 `requirements.txt`，可参考下列核心库： + +```text +gymnasium +mujoco +stable-baselines3 +pygame +opencv-python +numpy +scipy +matplotlib +ruamel.yaml +certifi +``` + +运行示例 +- 启动仿真： + +```powershell +python test_simulator.py +``` + +运行后应弹出可视化窗口（若使用 Pygame/SDL），并在终端输出仿真日志。 + +贡献与问题反馈 +- 若需添加说明或示例，请提交 Pull Request。 +- 遇到环境或依赖问题，请在 Issue 中描述操作系统、Python 版本与错误日志。 + +更多信息 +- 若目录中包含更详细的子模块文档，请参阅相应文件（如 `simulator.py` 顶部注释或同目录下的文档）。 + +--- + From 9c157b3752632e14943722737974e255b2684d18 Mon Sep 17 00:00:00 2001 From: lyy-1236 <3584247842@qq.com> Date: Sun, 21 Jun 2026 23:29:28 +0800 Subject: [PATCH 7/9] Update index.md --- docs/index.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/index.md b/docs/index.md index 7d57f06acc..ef2c2beef4 100644 --- a/docs/index.md +++ b/docs/index.md @@ -184,7 +184,7 @@ title: 主页 # 其他 - [__YOLO数据生成器__](./carla-yolo-dataset-generator/README.md) - + - [__双机械臂协同仿真系统__](./docs/index.md) - 基于Gymnasium/MuJoCo的仿真与强化学习实验环境 - [__驾驶事故视频识别__](./carla_temporal_collage/index.md) - 基于 Temporal Collage Prompting 的 CARLA 驾驶事故视频识别系统 - [__CARLA IMU 数据采集平台__](./carla_imu/carla_imu.md) — CARLA惯性测量单元数据采集与可视化驾驶平台开发汇报文档 From 70c696635d12d065b2348090049fe2514ed3f4b6 Mon Sep 17 00:00:00 2001 From: lyy-1236 <3584247842@qq.com> Date: Sun, 21 Jun 2026 23:33:32 +0800 Subject: [PATCH 8/9] Create index.md --- docs/dua_arm/index.md | 152 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 152 insertions(+) create mode 100644 docs/dua_arm/index.md diff --git a/docs/dua_arm/index.md b/docs/dua_arm/index.md new file mode 100644 index 0000000000..caa773778b --- /dev/null +++ b/docs/dua_arm/index.md @@ -0,0 +1,152 @@ + +# box — 仿真与强化学习实验箱 + +## 概述 + +`src/box` 包含若干用于仿真、感知与强化学习实验的环境与辅助脚本。主要面向基于 Gymnasium/MuJoCo 的任务、轻量级可视化示例以及调试/测试工具。 + +Gymnasium：一个操作台，是网上的一个强化学习的开源项目，给你提供一套通用的指令，可以完成一些基础操作。并提供给你进一步写指令代码的框架。 + +MuJoCo：一个物理仿真平台，计算你的指令在现实中执行后可能产生什么结果。 + +在这个项目中是一起用，如果只是写一些小例子，可以只用Gymnasium,它的内部也包含了一些现成的环境。 + +本目录旨在提供： + +- 可复现的仿真实验环境模板； +- 启动/调试脚本示例； +- 与主仓库其余模块交互的接口说明（若存在）。 + +## 推荐目录结构（示例） + +- `simulator.py`：仿真环境实现（通常继承自 `gym.Env` 或 `gymnasium.Env`）； +- `envs/`：若干子环境模块或封装； +- `agents/`：示例智能体或策略实现（可选）； +- `scripts/`：运行/训练/评估脚本（如 `run.py`, `train.py`, `eval.py`）； +- `tests/`：小型示例与单元/集成测试脚本（如 `test_simulator.py`）； +- `configs/`：默认配置或参数文件（YAML/JSON）； +- `README.md`：本文件。 + +（实际文件以仓库中为准，此处为常见约定。） + +## 快速上手 + +以下示例以 Windows + PowerShell 为例： + +1) 进入项目根目录并创建虚拟环境： + +```powershell +cd D:\\nn +python -m venv .venv +.\\.venv\\Scripts\\Activate.ps1 +``` + +2) 安装依赖： + +```powershell +pip install -r requirements.txt -i https://pypi.tuna.tsinghua.edu.cn/simple +``` + +如果仓库未包含完整的 `requirements.txt`，可参考核心依赖： + +``` +gymnasium +mujoco +stable-baselines3 +pygame +opencv-python +numpy +scipy +matplotlib +ruamel.yaml +``` + +3) 运行示例仿真（示例脚本名以仓库实际文件为准）： + +```powershell +python scripts/run.py +``` + +或： + +```powershell +python tests/test_simulator.py +``` + +运行后通常会在本地弹出可视化窗口或在终端打印日志，具体行为视环境实现而定。 + +## 开发与调试提示 + +- MuJoCo：请确保已正确安装 MuJoCo 及其许可证（若使用 mujoco 依赖）。 +- 显示/渲染：在无显示的服务器上运行时，可能需要配置虚拟帧缓冲（Linux 下使用 xvfb）。 +- 依赖冲突：若出现版本不兼容，建议使用虚拟环境并锁定依赖版本。 + +## 如何贡献 + +- 修改或补充说明、示例脚本或配置后提交 Pull Request； +- 提交 Issue 时请包含：操作系统、Python 版本、依赖版本与复现步骤/错误日志。 + +## 参考与更多信息 + +- 查看目录下的具体脚本与模块顶部注释，通常包含使用示例与参数说明； +- 若需要，我可以为 `src/box` 中的主要文件生成更详细的文档或示例运行脚本。 + +**box — 仿真与强化学习实验箱** + +简介 +- `src/box` 目录包含基于 Gymnasium 和 MuJoCo 的仿真环境与相关辅助脚本，用于开发和测试生物力学/机器人仿真、感知模块与强化学习任务。 + +目录结构（示例） +- `simulator.py`：仿真环境核心（通常继承 `gym.Env`）。 +- `test_simulator.py`：示例运行脚本，用于启动仿真并可视化。 +- `main.py`：辅助脚本（例如证书或配置检查）。 +- `README.md`：本文件，说明目录用途与快速上手指南。 + +快速上手 +1. 创建并激活虚拟环境（以 Windows 为例）： + +```powershell +cd <项目根目录> +python -m venv venv --python=3.9 +.\\venv\\Scripts\\Activate.ps1 +``` + +2. 安装依赖（建议使用清华镜像加速）： + +```powershell +pip install -r requirements.txt -i https://pypi.tuna.tsinghua.edu.cn/simple +``` + +如果仓库没有完整的 `requirements.txt`，可参考下列核心库： + +```text +gymnasium +mujoco +stable-baselines3 +pygame +opencv-python +numpy +scipy +matplotlib +ruamel.yaml +certifi +``` + +运行示例 +- 启动仿真： + +```powershell +python test_simulator.py +``` + +运行后应弹出可视化窗口（若使用 Pygame/SDL），并在终端输出仿真日志。 + +贡献与问题反馈 +- 若需添加说明或示例，请提交 Pull Request。 +- 遇到环境或依赖问题，请在 Issue 中描述操作系统、Python 版本与错误日志。 + +更多信息 +- 若目录中包含更详细的子模块文档，请参阅相应文件（如 `simulator.py` 顶部注释或同目录下的文档）。 + +--- + From 410797486aa758f9052d7f2cf15114c477685693 Mon Sep 17 00:00:00 2001 From: lyy-1236 <3584247842@qq.com> Date: Sun, 21 Jun 2026 23:35:06 +0800 Subject: [PATCH 9/9] Update index.md --- docs/index.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/index.md b/docs/index.md index ef2c2beef4..1eb998f78f 100644 --- a/docs/index.md +++ b/docs/index.md @@ -184,7 +184,7 @@ title: 主页 # 其他 - [__YOLO数据生成器__](./carla-yolo-dataset-generator/README.md) - - [__双机械臂协同仿真系统__](./docs/index.md) - 基于Gymnasium/MuJoCo的仿真与强化学习实验环境 + - [__双机械臂协同仿真系统__](./dua_arm/index.md) - 基于Gymnasium/MuJoCo的仿真与强化学习实验环境 - [__驾驶事故视频识别__](./carla_temporal_collage/index.md) - 基于 Temporal Collage Prompting 的 CARLA 驾驶事故视频识别系统 - [__CARLA IMU 数据采集平台__](./carla_imu/carla_imu.md) — CARLA惯性测量单元数据采集与可视化驾驶平台开发汇报文档