diff --git a/src/box/BasicWithEndEffectorPosition.py b/src/box/BasicWithEndEffectorPosition.py new file mode 100644 index 0000000000..63b3a839c2 --- /dev/null +++ b/src/box/BasicWithEndEffectorPosition.py @@ -0,0 +1,139 @@ +# 从上级包的base模块导入基础模块类(UITB框架的核心基类) +from ...base import BaseModule +# 导入数值计算库,用于数组操作 +import numpy as np + + +class BasicWithEndEffectorPosition(BaseModule): + """ + 本体感知模块:整合关节状态、肌肉激活度和末端执行器位置的观测生成器 + 核心功能: + 1. 标准化关节角度、速度、加速度、肌肉激活值 + 2. 获取末端执行器(如手部、机械爪)的全局位置 + 3. 拼接所有感知信息,输出一维观测向量(适配强化学习输入) + 继承自:BaseModule(UITB框架的基础模块类,提供模拟器交互接口) + """ + + def __init__(self, model, data, bm_model, end_effector, **kwargs): + """ + 初始化感知模块实例 + Args: + model: Mujoco模拟器的模型实例(存储物理模型结构,如关节、刚体、执行器) + data: Mujoco模拟器的数据实例(存储实时物理数据,如关节角度、速度、力) + bm_model: 生物力学模型实例(继承自uitb.bm_models.base.BaseBMModel,提供关节/执行器映射) + end_effector (list of lists): 末端执行器定义,格式示例: + - 单末端执行器:[["site", "right_hand_site"]] + - 多末端执行器:[["body", "left_hand"], ["geom", "right_foot"]] + 每个子列表第一个元素是Mujoco元素类型(geom/body/site),第二个是元素名称 + **kwargs: 可选参数(如"rng"随机数种子) + """ + # 调用父类初始化方法,传入模拟器实例和生物力学模型 + super().__init__(model, data, bm_model,** kwargs) + + # 校验末端执行器参数格式:必须是列表类型 + if not isinstance(end_effector, list): + raise RuntimeError( + "end_effector必须是长度为2的列表,或嵌套列表(每个子列表长度为2)" + ) + + # 检查是否为单层列表(如["site", "hand"]),若是则转为嵌套列表(统一格式) + if isinstance(end_effector[0], str): + end_effector = [end_effector] + + # 校验所有嵌套子列表的长度必须为2(类型+名称) + if any(len(pair) != 2 for pair in end_effector): + raise RuntimeError("end_effector的每个子列表必须包含2个元素:类型+名称") + + # 保存标准化后的末端执行器配置(实例变量,供后续方法调用) + self._end_effector = end_effector + + @staticmethod + def insert(task, **kwargs): + """ + 静态方法:用于将模块插入到任务配置中(UITB框架预留接口,暂无实现) + Args: + task: 任务实例(包含模拟器、评估指标等) + **kwargs: 扩展参数 + """ + pass + + @property + def _default_encoder(self): + """ + 属性方法:返回默认的编码器配置(适配强化学习的特征编码) + 此处配置:单层全连接编码器,输出特征维度为128 + Returns: + dict: 编码器的模块路径、类名、参数 + """ + return { + "module": "rl.encoders", # 编码器模块路径 + "cls": "OneLayer", # 编码器类名(单层全连接) + "kwargs": {"out_features": 128} # 编码器参数:输出特征维度 + } + + def get_observation(self, model, data, info=None): + """ + 核心方法:生成标准化的感知观测向量 + Args: + model: Mujoco模型实例(同__init__) + data: Mujoco数据实例(同__init__,存储实时数据) + info: 额外信息字典(可选,如环境状态) + Returns: + np.ndarray: 一维数组,包含所有标准化的感知特征 + """ + # -------------------------- 1. 标准化关节角度(qpos) -------------------------- + # 获取独立关节的角度范围(model.jnt_range:[min, max]) + jnt_range = model.jnt_range[self._bm_model.independent_joints] + # 复制独立关节的当前角度(避免修改原数据) + qpos = data.qpos[self._bm_model.independent_qpos].copy() + # 第一步归一化:将角度映射到[0, 1]区间 + qpos = (qpos - jnt_range[:, 0]) / (jnt_range[:, 1] - jnt_range[:, 0]) + # 第二步归一化:将角度映射到[-1, 1]区间(适配神经网络输入) + qpos = (qpos - 0.5) * 2 + + # -------------------------- 2. 获取关节速度/加速度 -------------------------- + # 复制独立自由度的关节速度(原始值,未标准化) + qvel = data.qvel[self._bm_model.independent_dofs].copy() + # 复制独立自由度的关节加速度(原始值,未标准化) + qacc = data.qacc[self._bm_model.independent_dofs].copy() + + # -------------------------- 3. 获取末端执行器全局位置 -------------------------- + ee_position = [] # 存储所有末端执行器的位置 + for pair in self._end_effector: + # pair[0]:Mujoco元素类型(geom/body/site),pair[1]:元素名称 + # getattr(data, pair[0])(pair[1]):获取对应元素的实例 + # .xpos:获取元素的全局坐标(3维:x,y,z) + ee_pos = getattr(data, pair[0])(pair[1]).xpos.copy() + ee_position.append(ee_pos) + # 将多个末端执行器的位置拼接为一维数组(如2个末端执行器则为6维) + ee_position = np.hstack(ee_position) + + # -------------------------- 4. 标准化肌肉/执行器激活值 -------------------------- + # data.act:所有执行器的当前激活值(范围通常[0,1]),映射到[-1,1] + act = (data.act.copy() - 0.5) * 2 + # 生物力学模型中平滑后的电机激活值,同样映射到[-1,1] + motor_act = (self._bm_model.motor_act.copy() - 0.5) * 2 + + # -------------------------- 5. 拼接所有感知特征 -------------------------- + # 本体感知特征:关节角度+速度+加速度+末端位置+执行器激活+电机激活 + proprioception = np.concatenate([qpos, qvel, qacc, ee_position, act, motor_act]) + + # 返回最终的观测向量(一维数组,可直接输入强化学习网络) + return proprioception + + def _get_state(self, model, data): + """ + 辅助方法:获取末端执行器的详细状态(用于日志/评估,非训练观测) + Args: + model: Mujoco模型实例 + data: Mujoco数据实例 + Returns: + dict: 键为"元素名称_xpos/xmat",值为对应的位置/旋转矩阵 + """ + state = {} + for pair in self._end_effector: + # 存储末端执行器的全局位置(xpos:3维坐标) + state[f"{pair[1]}_xpos"] = getattr(data, pair[0])(pair[1]).xpos.copy() + # 存储末端执行器的旋转矩阵(xmat:9维,描述空间姿态) + state[f"{pair[1]}_xmat"] = getattr(data, pair[0])(pair[1]).xmat.copy() + return state \ No newline at end of file diff --git a/src/box/README.md b/src/box/README.md index 728c18e479..81ecd8e5a8 100644 --- a/src/box/README.md +++ b/src/box/README.md @@ -1,32 +1,47 @@ -**box — 仿真与强化学习实验箱** +# box — 仿真与强化学习实验箱 -简介 -- `src/box` 目录包含基于 Gymnasium 和 MuJoCo 的仿真环境与相关辅助脚本,用于开发和测试生物力学/机器人仿真、感知模块与强化学习任务。 +## 概述 -目录结构(示例) -- `simulator.py`:仿真环境核心(通常继承 `gym.Env`)。 -- `test_simulator.py`:示例运行脚本,用于启动仿真并可视化。 -- `main.py`:辅助脚本(例如证书或配置检查)。 -- `README.md`:本文件,说明目录用途与快速上手指南。 +`src/box` 包含若干用于仿真、感知与强化学习实验的环境与辅助脚本。主要面向基于 Gymnasium/MuJoCo 的任务、轻量级可视化示例以及调试/测试工具。 -快速上手 -1. 创建并激活虚拟环境(以 Windows 为例): +本目录旨在提供: +- 可复现的仿真实验环境模板; +- 启动/调试脚本示例; +- 与主仓库其余模块交互的接口说明(若存在)。 + +## 推荐目录结构(示例) + +- `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 <项目根目录> -python -m venv venv --python=3.9 -.\\venv\\Scripts\\Activate.ps1 +cd D:\\nn +python -m venv .venv +.\\.venv\\Scripts\\Activate.ps1 ``` -2. 安装依赖(建议使用清华镜像加速): +2) 安装依赖: ```powershell pip install -r requirements.txt -i https://pypi.tuna.tsinghua.edu.cn/simple ``` -如果仓库没有完整的 `requirements.txt`,可参考下列核心库: +如果仓库未包含完整的 `requirements.txt`,可参考核心依赖: -```text +``` gymnasium mujoco stable-baselines3 @@ -36,21 +51,34 @@ numpy scipy matplotlib ruamel.yaml -certifi ``` -运行示例 -- 启动仿真: +3) 运行示例仿真(示例脚本名以仓库实际文件为准): ```powershell -python test_simulator.py +python scripts/run.py ``` -运行后应弹出可视化窗口(若使用 Pygame/SDL),并在终端输出仿真日志。 +或: + +```powershell +python tests/test_simulator.py +``` + +运行后通常会在本地弹出可视化窗口或在终端打印日志,具体行为视环境实现而定。 + +## 开发与调试提示 + +- MuJoCo:请确保已正确安装 MuJoCo 及其许可证(若使用 mujoco 依赖)。 +- 显示/渲染:在无显示的服务器上运行时,可能需要配置虚拟帧缓冲(Linux 下使用 xvfb)。 +- 依赖冲突:若出现版本不兼容,建议使用虚拟环境并锁定依赖版本。 + +## 如何贡献 + +- 修改或补充说明、示例脚本或配置后提交 Pull Request; +- 提交 Issue 时请包含:操作系统、Python 版本、依赖版本与复现步骤/错误日志。 -贡献与问题反馈 -- 若需添加说明或示例,请提交 Pull Request。 -- 遇到环境或依赖问题,请在 Issue 中描述操作系统、Python 版本与错误日志。 +## 参考与更多信息 -更多信息 -- 若目录中包含更详细的子模块文档,请参阅相应文件(如 `simulator.py` 顶部注释或同目录下的文档)。 +- 查看目录下的具体脚本与模块顶部注释,通常包含使用示例与参数说明; +- 若需要,我可以为 `src/box` 中的主要文件生成更详细的文档或示例运行脚本。 diff --git a/src/box/base.py b/src/box/base.py new file mode 100644 index 0000000000..8b374acd08 --- /dev/null +++ b/src/box/base.py @@ -0,0 +1,364 @@ +import numpy as np +import xml.etree.ElementTree as ET +import pathlib +import os +import shutil +import inspect +import mujoco +from abc import ABC, abstractmethod +import importlib +from typing import final + +from ..utils.functions import parent_path +from ..utils import element_tree as ETutils + + +class BaseBMModel(ABC): + + def __init__(self, model, data, **kwargs): + """Initializes a new `BaseBMModel`. + + Args: + model: Mujoco model instance of the simulator. + data: Mujoco data instance of the simulator. + **kwargs: Many keywords that should be documented somewhere + """ + + # Initialise mujoco model of the biomechanical model, easier to manipulate things + bm_model = mujoco.MjModel.from_xml_path(self.get_xml_file()) + + # Get an rng + self._rng = np.random.default_rng(kwargs.get("random_seed", None)) + + # Total number of actuators + self._nu = bm_model.nu + + # Number of muscle actuators + self._na = bm_model.na + + # Number of motor actuators + self._nm = self._nu - self._na + self._motor_act = np.zeros((self._nm,)) + self._motor_alpha = 0.9 + + # Get actuator names (muscle and motor) + self._actuator_names = [mujoco.mj_id2name(bm_model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) for i in range(bm_model.nu)] + self._muscle_actuator_names = set(np.array(self._actuator_names)[bm_model.actuator_trntype==3]) + self._motor_actuator_names = set(self._actuator_names) - self._muscle_actuator_names + + # Sort the names to preserve original ordering (not really necessary but looks nicer) + self._muscle_actuator_names = sorted(self._muscle_actuator_names, key=self._actuator_names.index) + self._motor_actuator_names = sorted(self._motor_actuator_names, key=self._actuator_names.index) + + # Find actuator indices in the simulation + self._muscle_actuators = [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, actuator_name) + for actuator_name in self._muscle_actuator_names] + self._motor_actuators = [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, actuator_name) + for actuator_name in self._motor_actuator_names] + + # Get joint names (dependent and independent) + self._joint_names = [mujoco.mj_id2name(bm_model, mujoco.mjtObj.mjOBJ_JOINT, i) for i in range(bm_model.njnt)] + self._dependent_joint_names = {self._joint_names[idx] for idx in + np.unique(bm_model.eq_obj1id[bm_model.eq_active.astype(bool)])} \ + if bm_model.eq_obj1id is not None else set() + self._independent_joint_names = set(self._joint_names) - self._dependent_joint_names + + # Sort the names to preserve original ordering (not really necessary but looks nicer) + self._dependent_joint_names = sorted(self._dependent_joint_names, key=self._joint_names.index) + self._independent_joint_names = sorted(self._independent_joint_names, key=self._joint_names.index) + + # Find dependent and independent joint indices in the simulation + self._dependent_joints = [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, joint_name) + for joint_name in self._dependent_joint_names] + self._independent_joints = [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, joint_name) + for joint_name in self._independent_joint_names] + + # If there are 'free' type of joints, we'll need to be more careful with which dof corresponds to + # which joint, for both qpos and qvel/qacc. There should be exactly one dof per independent/dependent joint. + def get_dofs(joint_indices): + qpos = [] + dofs = [] + for joint_idx in joint_indices: + if model.jnt_type[joint_idx] not in [mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE]: + raise NotImplementedError(f"Only 'hinge' and 'slide' joints are supported, joint " + f"{self._joint_names[joint_idx]} is of type {mujoco.mjtJoint(model.jnt_type[joint_idx]).name}") + qpos.append(model.jnt_qposadr[joint_idx]) + dofs.append(model.jnt_dofadr[joint_idx]) + return qpos, dofs + self._dependent_qpos, self._dependent_dofs = get_dofs(self._dependent_joints) + self._independent_qpos, self._independent_dofs = get_dofs(self._independent_joints) + + # Get the effort model; some models might need to know dt + self._effort_model = self.get_effort_model(kwargs.get("effort_model", {"cls": "Zero"}), dt=kwargs["dt"]) + + # Define signal-dependent noise + self._sigdepnoise_type = kwargs.get("sigdepnoise_type", None) #"white") + self._sigdepnoise_level = kwargs.get("sigdepnoise_level", 0.103) + self._sigdepnoise_rng = np.random.default_rng(kwargs.get("random_seed", None)) + self._sigdepnoise_acc = 0 #only used for red/Brownian noise + + # Define constant (i.e., signal-independent) noise + self._constantnoise_type = kwargs.get("constantnoise_type", None) #"white") + self._constantnoise_level = kwargs.get("constantnoise_level", 0.185) + self._constantnoise_rng = np.random.default_rng(kwargs.get("random_seed", None)) + self._constantnoise_acc = 0 #only used for red/Brownian noise + + ############ The methods below you should definitely overwrite as they are important ############ + + @classmethod + @abstractmethod + def _get_floor(cls): + """ If there's a floor in the bm_model.xml file it should be defined here. + + Returns: + * None if there is no floor in the file + * A dict like {"tag": "geom", "name": "name-of-the-geom"}, where "tag" indicates what kind of element the floor + is, and "name" is the name of the element. + """ + pass + + + ############ The methods below are overwritable but often don't need to be overwritten ############ + + def _reset(self, model, data): + """ Resets the biomechanical model. """ + + # Randomly sample qpos, qvel, act around zero values + nq = len(self._independent_qpos) + qpos = self._rng.uniform(low=np.ones((nq,))*-0.05, high=np.ones((nq,))*0.05) + qvel = self._rng.uniform(low=np.ones((nq,))*-0.05, high=np.ones((nq,))*0.05) + act = self._rng.uniform(low=np.zeros((self._na,)), high=np.ones((self._na,))) + + # Set qpos and qvel + data.qpos[self._dependent_qpos] = 0 + data.qpos[self._independent_qpos] = qpos + data.qvel[self._dependent_dofs] = 0 + data.qvel[self._independent_dofs] = qvel + data.act[self._muscle_actuators] = act + + # Sample random initial values for motor activation + self._motor_act = self._rng.uniform(low=np.zeros((self._nm,)), high=np.ones((self._nm,))) + # Reset smoothed average of motor actuator activation + self._motor_smooth_avg = np.zeros((self._nm,)) + + # Reset accumulative noise + self._sigdepnoise_acc = 0 + self._constantnoise_acc = 0 + + def _update(self, model, data): + """ Update the biomechanical model after a step has been taken in the simulator. """ + pass + + def _get_state(self, model, data): + """ Return the state of the biomechanical model. These states are used only for logging/evaluation, not for RL + training + + Args: + model: Mujoco model instance of the simulator. + data: Mujoco data instance of the simulator. + + Returns: + A dict where each key should have a float or a numpy vector as their value + """ + return dict() + + def set_ctrl(self, model, data, action): + """ Set control values for the biomechanical model. + + Args: + model: Mujoco model instance of the simulator. + data: Mujoco data instance of the simulator. + action: Action values between [-1, 1] + + """ + + _selected_motor_control = np.clip(self._motor_act + action[:self._nm], 0, 1) + _selected_muscle_control = np.clip(data.act[self._muscle_actuators] + action[self._nm:], 0, 1) + + if self._sigdepnoise_type is not None: + if self._sigdepnoise_type == "white": + _added_noise = self._sigdepnoise_level*self._sigdepnoise_rng.normal(scale=_selected_muscle_control) + _selected_muscle_control += _added_noise + elif self._sigdepnoise_type == "whiteonly": #only for debugging purposes + _selected_muscle_control = self._sigdepnoise_level*self._sigdepnoise_rng.normal(scale=_selected_muscle_control) + elif self._sigdepnoise_type == "red": + # self._sigdepnoise_acc *= 1 - 0.1 + self._sigdepnoise_acc += self._sigdepnoise_level*self._sigdepnoise_rng.normal(scale=_selected_muscle_control) + _selected_muscle_control += self._sigdepnoise_acc + else: + raise NotImplementedError(f"{self._sigdepnoise_type}") + if self._constantnoise_type is not None: + if self._constantnoise_type == "white": + _selected_muscle_control += self._constantnoise_level*self._constantnoise_rng.normal(scale=1) + elif self._constantnoise_type == "whiteonly": #only for debugging purposes + _selected_muscle_control = self._constantnoise_level*self._constantnoise_rng.normal(scale=1) + elif self._constantnoise_type == "red": + self._constantnoise_acc += self._constantnoise_level*self._constantnoise_rng.normal(scale=1) + _selected_muscle_control += self._constantnoise_acc + else: + raise NotImplementedError(f"{self._constantnoise_type}") + + # Update smoothed online estimate of motor actuation + self._motor_act = (1 - self._motor_alpha) * self._motor_act \ + + self._motor_alpha * np.clip(_selected_motor_control, 0, 1) + + data.ctrl[self._motor_actuators] = model.actuator_ctrlrange[self._motor_actuators,0] + self._motor_act*(model.actuator_ctrlrange[self._motor_actuators, 1] - model.actuator_ctrlrange[self._motor_actuators, 0]) + data.ctrl[self._muscle_actuators] = np.clip(_selected_muscle_control, 0, 1) + + + @classmethod + def get_xml_file(cls): + """ Overwrite this method if you want to call the mujoco xml file something other than 'bm_model.xml'. """ + return os.path.join(parent_path(inspect.getfile(cls)), "bm_model.xml") + + def get_effort_model(self, specs, dt): + """ Returns an initialised object of the effort model class. + + Overwrite this method if you want to define your effort models somewhere else. But note that in that case you need + to overwrite the 'clone' method as well since it assumes the effort models are defined in + uitb.bm_models.effort_models. + + Args: + specs: Specifications of the effort model, in format of + {"cls": "name-of-class", "kwargs": {"kw1": value1, "kw2": value2}}} + dt: Elapsed time between two consecutive simulation steps + + Returns: + An instance of a class that inherits from the uitb.bm_models.effort_models.BaseEffortModel class + """ + module = importlib.import_module(".".join(BaseBMModel.__module__.split(".")[:-1]) + ".effort_models") + return getattr(module, specs["cls"])(self, **{**specs.get("kwargs", {}), **{"dt": dt}}) + + @classmethod + def clone(cls, simulator_folder, package_name): + """ Clones (i.e. copies) the relevant python files into a new location. + + Args: + simulator_folder: Location of the simulator. + package_name: Name of the simulator (which is a python package) + """ + + # Create 'bm_models' folder + dst = os.path.join(simulator_folder, package_name, "bm_models") + os.makedirs(dst, exist_ok=True) + + # Copy this file + base_file = pathlib.Path(__file__) + shutil.copyfile(base_file, os.path.join(dst, base_file.name)) + + # Create an __init__.py file with the relevant import + modules = cls.__module__.split(".") + with open(os.path.join(dst, "__init__.py"), "w") as file: + file.write("from ." + ".".join(modules[2:]) + " import " + cls.__name__) + + # Copy bm-model folder + src = parent_path(inspect.getfile(cls)) + shutil.copytree(src, os.path.join(dst, src.stem), dirs_exist_ok=True) + + # Copy assets + shutil.copytree(os.path.join(src, "assets"), os.path.join(simulator_folder, package_name, "assets"), + dirs_exist_ok=True) + + # Copy effort models + shutil.copyfile(os.path.join(base_file.parent, "effort_models.py"), os.path.join(dst, "effort_models.py")) + + @classmethod + def insert(cls, simulator_tree): + """ Inserts the biomechanical model into the simulator by integrating the xml files together. + + Args: + simulator_tree: An `xml.etree.ElementTree` containing the parsed simulator xml file + """ + + # Parse xml file + bm_tree = ET.parse(cls.get_xml_file()) + bm_root = bm_tree.getroot() + + # Get simulator root + simulator_root = simulator_tree.getroot() + + # Add defaults + ETutils.copy_or_append("default", bm_root, simulator_root) + + # Add assets, except skybox + ETutils.copy_children("asset", bm_root, simulator_root, + exclude={"tag": "texture", "attrib": "type", "name": "skybox"}) + + # Add bodies, except floor/ground TODO this might not be currently working + if cls._get_floor() is not None: + floor = cls._get_floor() + ETutils.copy_children("worldbody", bm_root, simulator_root, + exclude={"tag": floor["tag"], "attrib": "name", "name": floor["name"]}) + else: + ETutils.copy_children("worldbody", bm_root, simulator_root) + + # Add tendons + ETutils.copy_children("tendon", bm_root, simulator_root) + + # Add actuators + ETutils.copy_children("actuator", bm_root, simulator_root) + + # Add equality constraints + ETutils.copy_children("equality", bm_root, simulator_root) + + def close(self, **kwargs): + """ Perform any necessary clean up. """ + pass + + ############ The methods below you should not overwrite ############ + + @final + def update(self, model, data): + """ Updates the biomechanical model and effort model. """ + self._update(model, data) + self._effort_model.update(model, data) + + @final + def reset(self, model, data): + """ Resets the biomechanical model and effort model. """ + self._reset(model, data) + self._effort_model.reset(model, data) + self.update(model, data) + mujoco.mj_forward(model, data) + + @final + def get_state(self, model, data): + """ Returns the state of the biomechanical model (as a dict). """ + state = dict() + state.update(self._get_state(model, data)) + state.update(self._effort_model._get_state(model, data)) + return state + + @final + def get_effort_cost(self, model, data): + """ Returns effort cost from the effort model. """ + return self._effort_model.cost(model, data) + + @property + @final + def independent_joints(self): + """ Returns indices of independent joints. """ + return self._independent_joints.copy() + + @property + @final + def independent_qpos(self): + """ Returns qpos indices of independent joints. """ + return self._independent_qpos.copy() + + @property + @final + def independent_dofs(self): + """ Returns qvel/qacc indices of independent joints. """ + return self._independent_dofs.copy() + + @property + @final + def nu(self): + """ Returns number of actuators (both muscle and motor). """ + return self._nu + + @property + def motor_act(self): + """ Returns (smoothed average of) motor actuation. """ + return self._motor_act \ No newline at end of file diff --git a/src/box/moblARMslndex.py b/src/box/moblARMslndex.py deleted file mode 100644 index 950f646641..0000000000 --- a/src/box/moblARMslndex.py +++ /dev/null @@ -1,270 +0,0 @@ -""" -MoblArmsIndex 生物力学模型 - 修改版 -这是基于 MoBL ARMS 模型的 MuJoCo 实现,专门用于手指指向任务 -""" - -import sys -import os - -# ================ 动态导入 BaseBMModel ================ -try: - # 尝试1: 从原始相对位置导入 - from ..base import BaseBMModel - print("✓ 使用相对导入: from ..base import BaseBMModel") -except ImportError: - try: - # 尝试2: 从绝对路径导入(假设在 uitb 包中) - from uitb.base import BaseBMModel - print("✓ 使用绝对导入: from uitb.base import BaseBMModel") - except ImportError: - try: - # 尝试3: 从常见路径导入 - import sys - import os - - # 将项目根目录添加到路径 - current_dir = os.path.dirname(os.path.abspath(__file__)) - project_root = os.path.dirname(os.path.dirname(current_dir)) - sys.path.insert(0, project_root) - - from base import BaseBMModel - print(f"✓ 使用项目根目录导入: from base import BaseBMModel (路径: {project_root})") - except ImportError as e: - # 尝试4: 搜索并动态导入 - print("⚠ 无法自动导入 BaseBMModel,尝试动态查找...") - - # 搜索 base.py 文件 - base_path = None - search_dirs = [ - os.path.dirname(os.path.dirname(os.path.abspath(__file__))), # 上一级目录 - os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))), # 上上级目录 - os.path.join(os.path.dirname(__file__), '..'), # 相对路径 - ] - - for search_dir in search_dirs: - possible_path = os.path.join(search_dir, 'base.py') - if os.path.exists(possible_path): - base_path = possible_path - break - - if base_path: - # 动态导入 - import importlib.util - spec = importlib.util.spec_from_file_location("base_module", base_path) - base_module = importlib.util.module_from_spec(spec) - sys.modules[spec.name] = base_module - spec.loader.exec_module(base_module) - BaseBMModel = base_module.BaseBMModel - print(f"✓ 动态导入成功: {base_path}") - else: - # 如果都失败,创建简化的 BaseBMModel - print("⚠ 无法找到 BaseBMModel,使用简化版本") - - class BaseBMModel: - """简化的 BaseBMModel 基类""" - def __init__(self, model, data, **kwargs): - self.model = model - self.data = data - - def _update(self, model, data): - """更新方法 - 子类应重写""" - pass - - @classmethod - def _get_floor(cls): - """获取地板 - 子类应重写""" - return None - - BaseBMModel = BaseBMModel - print("✓ 使用简化版本的 BaseBMModel") - -# ================ 继续原来的代码 ================ -import numpy as np -import mujoco - - -class MoblArmsIndex(BaseBMModel): - """ - 基于 MoBL ARMS 模型的生物力学模型 - - 来源: - - 原始 OpenSim 模型: https://simtk.org/frs/?group_id=657 - - MuJoCo 转换工具: https://github.com/aikkala/O2MConverter - - 说明: - 此模型与 uitb/bm_models/mobl_arms 中的模型相同, - 但食指处于弯曲状态并包含力传感器。 - """ - - def __init__(self, model, data, **kwargs): - """ - 初始化 MoblArmsIndex 模型 - - 参数: - model: MuJoCo 模型实例 - data: MuJoCo 数据实例 - **kwargs: 额外参数 - - shoulder_variant: 肩部变体,可选 "none" (默认) 或 "patch-v1" - """ - super().__init__(model, data, **kwargs) - - # 设置肩部变体 - # 使用 "none" 作为默认值 - # 使用 "patch-v1" 可以获得更合理的运动外观(未经全面测试) - self.shoulder_variant = kwargs.get("shoulder_variant", "none") - - print(f"✓ MoblArmsIndex 初始化完成,肩部变体: {self.shoulder_variant}") - - def _update(self, model, data): - """ - 更新模型状态 - - 此方法在每个时间步被调用,用于更新肩部约束。 - - 参数: - model: MuJoCo 模型实例 - data: MuJoCo 数据实例 - """ - # 更新肩部等式约束 - if self.shoulder_variant.startswith("patch"): - # 约束1: shoulder1_r2_con 约束的数据更新 - model.equality("shoulder1_r2_con").data[1] = \ - -((np.pi - 2 * data.joint('shoulder_elv').qpos) / np.pi) - - # patch-v2 变体有额外的约束 - if self.shoulder_variant == "patch-v2": - # 动态调整肩部旋转范围 - data.joint('shoulder_rot').range[:] = \ - np.array([-np.pi / 2, np.pi / 9]) - \ - 2 * np.min((data.joint('shoulder_elv').qpos, - np.pi - data.joint('shoulder_elv').qpos)) / np.pi \ - * data.joint('elv_angle').qpos - - # 执行前向计算,更新物理状态 - mujoco.mj_forward(model, data) - - # 打印调试信息(可选) - if hasattr(self, 'debug') and self.debug: - print(f"更新肩部约束: shoulder_elv={data.joint('shoulder_elv').qpos:.3f}, " - f"约束值={model.equality('shoulder1_r2_con').data[1]:.3f}") - - @classmethod - def _get_floor(cls): - """ - 获取地板配置 - - 此模型不包含地板,返回 None。 - - 返回: - None: 表示此模型没有地板 - """ - return None - - def get_force_sensor_data(self, data, sensor_name="index_force_sensor"): - """ - 获取力传感器数据 - - 参数: - data: MuJoCo 数据实例 - sensor_name: 传感器名称,默认为 "index_force_sensor" - - 返回: - numpy.ndarray: 传感器数据,如果没有找到传感器则返回 None - """ - try: - sensor_id = self.model.sensor(sensor_name).id - return data.sensordata[sensor_id] - except Exception as e: - if hasattr(self, 'debug') and self.debug: - print(f"⚠ 无法获取力传感器数据: {e}") - return None - - def set_debug_mode(self, debug=True): - """ - 设置调试模式 - - 参数: - debug: 是否启用调试模式 - """ - self.debug = debug - - def __str__(self): - """ - 返回模型的字符串表示 - - 返回: - str: 模型描述 - """ - return (f"MoblArmsIndex 模型 (肩部变体: {self.shoulder_variant})\n" - f"描述: 基于 MoBL ARMS 的上肢模型,食指弯曲并包含力传感器") - - -# ================ 测试代码 ================ -if __name__ == "__main__": - """ - 直接运行此文件的测试代码 - """ - print("=" * 60) - print("MoblArmsIndex 模型测试") - print("=" * 60) - - # 测试导入 - print("1. 测试导入和类定义:") - print(f" 类名: {MoblArmsIndex.__name__}") - print(f" 基类: {MoblArmsIndex.__bases__[0].__name__}") - print(f" 文档: {MoblArmsIndex.__doc__.strip().split('\n')[0]}") - - # 测试创建实例(需要 MuJoCo 模型) - try: - import mujoco - import numpy as np - - print("\n2. 测试创建实例:") - - # 创建一个简单的 MuJoCo 模型用于测试 - xml_string = """ - - - - - - - - - """ - - model = mujoco.MjModel.from_xml_string(xml_string) - data = mujoco.MjData(model) - - # 创建模型实例 - bm_model = MoblArmsIndex(model, data, shoulder_variant="none") - - print(f" ✓ 成功创建 MoblArmsIndex 实例") - print(f" 肩部变体: {bm_model.shoulder_variant}") - - # 测试更新方法 - print("\n3. 测试更新方法:") - bm_model._update(model, data) - print(" ✓ _update 方法执行成功") - - # 测试获取地板 - print("\n4. 测试获取地板:") - floor = bm_model._get_floor() - print(f" 地板配置: {floor}") - - # 测试字符串表示 - print("\n5. 测试字符串表示:") - print(f" {bm_model}") - - print("\n" + "=" * 60) - print("所有测试通过!") - print("=" * 60) - - except Exception as e: - print(f"\n✗ 测试过程中出现错误: {type(e).__name__}: {e}") - import traceback - traceback.print_exc() - - print("\n" + "=" * 60) - print("注意: 缺少 MuJoCo 模型文件,但导入测试成功") - print("=" * 60) diff --git a/src/box/simulator.py b/src/box/simulator.py index e803fe7319..afefbc1b00 100644 --- a/src/box/simulator.py +++ b/src/box/simulator.py @@ -1,15 +1,16 @@ import os - my-featurn-branch 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 @@ -17,6 +18,7 @@ mujoco = None HAS_MUJOCO = False +# 检测MuJoCo Viewer是否安装(原生可视化工具) try: import mujoco_viewer HAS_MUJOCO_VIEWER = True @@ -25,490 +27,246 @@ HAS_MUJOCO_VIEWER = False -class Simulator: # 第27行:类定义后必须有缩进的代码块 - """Mechanical hand simulator with optional MuJoCo backend. - Behavior: - - If `mujoco` is available, generate a simple hand MJCF, load model/data, - and expose `reset`/`step` using MuJoCo stepping routines. - - If `mujoco` is not available, fall back to a lightweight placeholder - implementation so the module remains importable and testable. - """ - # --------------- 核心:类内所有方法必须缩进(通常4个空格)--------------- - def __init__(self, render_mode: str = "human", simulator_folder: str = "./mujoco_models"): - """初始化模拟器(类的构造方法,必须缩进)""" - self.render_mode = render_mode -import numpy as np -import pygame -import mujoco -import yaml -from gymnasium import spaces -from typing import Dict, Any, Optional, Tuple -import mujoco.viewer - 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): - main - 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 - - # ---------------- MuJoCo后端实现(必须缩进)---------------- - def _init_mujoco(self): - """初始化MuJoCo仿真环境""" - self.config: Dict[str, Any] = { - "simulation": { - "max_steps": 1000, - "model_path": "hand_model.mjcf", - "target_joint_pos": [] - } - } - - # 5根手指的基座位置和连杆长度(单位:米) - bases = [(-0.20, 0.06), (-0.08, 0.06), (0.04, 0.06), (0.16, 0.06), (0.28, 0.06)] - lengths = [[0.06, 0.05, 0.04] for _ in bases] - self.finger_bases = bases - self.link_lengths = lengths - - # 生成MJCF模型文件路径 - model_path = os.path.join(self.simulator_folder, self.config["simulation"]["model_path"]) - os.makedirs(os.path.dirname(model_path), exist_ok=True) - - # 构建MJCF模型内容 - mjcf_lines: List[str] = [ - '', - ' ') - - # 保存MJCF文件 - with open(model_path, 'w', encoding='utf-8') as f: - f.write('\n'.join(mjcf_lines)) - - # 加载MuJoCo模型和数据 - try: - self.model = mujoco.MjModel.from_xml_path(model_path) - self.data = mujoco.MjData(self.model) - except Exception as e: - logger.exception(f'Failed to load MuJoCo model — falling back to placeholder: {e}') - self._init_placeholder() - return - - # 定义动作/观测空间 - self.action_space = spaces.Box( - low=-1.0, high=1.0, - shape=(int(self.model.nu),), dtype=np.float32 - ) - obs_dim = int(self.model.nq) + int(self.model.nv) - self.observation_space = spaces.Box( - low=-np.inf, high=np.inf, - shape=(obs_dim,), dtype=np.float32 - ) - - # 初始化MuJoCo Viewer - self.viewer = None - if self.render_mode == 'human' and HAS_MUJOCO_VIEWER: - try: - self.viewer = mujoco_viewer.MujocoViewer(self.model, self.data) - self.viewer.cam.azimuth = 45 - self.viewer.cam.elevation = -20 - self.viewer.cam.distance = 0.5 - except Exception as e: - logger.exception(f'Failed to initialize mujoco_viewer: {e}') - - # ---------------- 占位实现(必须缩进)---------------- - def _init_placeholder(self): - """轻量级占位实现""" - self.model = type('M', (), {'nq': 15, 'nu': 15, 'nv': 15})() - self.data = type('D', (), { - 'qpos': np.zeros(self.model.nq), - 'qvel': np.zeros(self.model.nv) - })() - - # 定义兼容的动作/观测空间 - self.action_space = spaces.Box( - low=-1.0, high=1.0, - shape=(int(self.model.nu),), dtype=np.float32 - ) - obs_dim = int(self.model.nq) + int(self.model.nv) - self.observation_space = spaces.Box( - low=-np.inf, high=np.inf, - shape=(obs_dim,), dtype=np.float32 - ) - - # 占位的手指参数 - self.finger_bases = [(-0.20, 0.06), (-0.08, 0.06), (0.04, 0.06), (0.16, 0.06), (0.28, 0.06)] - self.link_lengths = [[0.06, 0.05, 0.04] for _ in self.finger_bases] - - # ---------------- 通用API(必须缩进)---------------- - def reset(self, seed: Optional[int] = None) -> Tuple[np.ndarray, dict]: - """重置仿真环境""" - if seed is not None: - np.random.seed(int(seed)) - if HAS_MUJOCO and hasattr(mujoco, 'set_rng_seed'): - try: - mujoco.set_rng_seed(int(seed)) - except Exception as e: - logger.warning(f'Failed to set MuJoCo seed: {e}') - - self.step_count = 0 - self.terminated = False - self.truncated = False - self.last_reward = 0.0 - - if HAS_MUJOCO and isinstance(self.data, mujoco.MjData): - mujoco.mj_resetData(self.model, self.data) - else: - self.data.qpos = np.zeros(int(self.model.nq)) - self.data.qvel = np.zeros(int(self.model.nv)) - - obs = self._get_obs() - return obs, {'step': self.step_count} - - def step(self, action: np.ndarray) -> 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) - - # MuJoCo步进 - if HAS_MUJOCO and isinstance(self.data, mujoco.MjData): - self.data.ctrl[:] = action * 5.0 - try: - mujoco.mj_step(self.model, self.data) - except Exception: - try: - mujoco.mj_step1(self.model, self.data) - mujoco.mj_step2(self.model, self.data) - except Exception as e: - logger.exception(f'MuJoCo stepping failed: {e}') - else: - # 占位动力学 - gain = 0.01 - self.data.qvel = self.data.qvel + action[:int(self.model.nv)] * gain - self.data.qpos = self.data.qpos + self.data.qvel - - self.step_count += 1 - reward = self._compute_reward() - self.last_reward = float(reward) - - self.terminated = self._check_terminated() - max_steps = int(self.config['simulation'].get('max_steps', 1000)) if hasattr(self, 'config') else 1000 - self.truncated = self.step_count >= max_steps - - obs = self._get_obs() - info = {'step': self.step_count, 'reward': float(reward)} - return obs, float(reward), bool(self.terminated), bool(self.truncated), info - - def _get_obs(self) -> np.ndarray: - """获取当前观测""" - if HAS_MUJOCO and isinstance(self.data, mujoco.MjData): - qpos = self.data.qpos.copy() - qvel = self.data.qvel.copy() - else: - qpos = np.asarray(self.data.qpos).copy() - qvel = np.asarray(self.data.qvel).copy() - return np.concatenate([qpos, qvel]).astype(np.float32) - - def _compute_reward(self) -> float: - """计算奖励""" - if hasattr(self, 'config') and 'simulation' in self.config: - target = np.array(self.config['simulation'].get('target_joint_pos', [0.0] * int(self.model.nq))) - else: - target = np.zeros(int(self.model.nq)) - - current = np.asarray(self.data.qpos).copy() - if len(target) != len(current): - target = np.zeros_like(current) + """ + 仿真器初始化入口 + 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 # 上一步奖励值 - return float(-np.sum((current - target) ** 2)) - - def _check_terminated(self) -> bool: - """检查是否终止""" - return False - - def render(self): - """渲染仿真画面""" - if self.render_mode != 'human': - return - - # MuJoCo Viewer渲染 - if HAS_MUJOCO and self.viewer is not None: - try: - self.viewer.render() - return - except Exception as e: - logger.warning(f'MuJoCo Viewer render failed: {e}') - - # 占位渲染(Pygame) - try: - import pygame - except ImportError: - logger.warning("Pygame not installed — skip placeholder rendering") - return - - # 初始化Pygame - if not (pygame.get_init() and pygame.display.get_init()): - try: - pygame.init() - self.screen = pygame.display.set_mode((800, 600)) - pygame.display.set_caption('Mechanical Hand (Placeholder)') - except Exception as e: - logger.warning(f'Pygame init failed: {e}') - return - - # 绘制背景 - screen = self.screen - screen.fill((240, 240, 255)) - - # 绘制手掌 - center_x, center_y = 400, 420 - palm_w, palm_h = 360, 80 - pygame.draw.rect(screen, (160, 120, 90), - (center_x - palm_w // 2, center_y - palm_h // 2, palm_w, palm_h)) - - # 绘制手指连杆 - q = np.asarray(self.data.qpos).copy() - ptr = 0 - for i, base in enumerate(self.finger_bases): - bx, by = base - sx = center_x + int(bx * 1000) - sy = center_y - int(by * 1000) - angle = 0.0 - x0, y0 = sx, sy - - for j, L in enumerate(self.link_lengths[i]): - if ptr < len(q): - angle += float(q[ptr]) - ex = x0 + int(np.cos(angle) * (L * 1000)) - ey = y0 - int(np.sin(angle) * (L * 1000)) - - pygame.draw.line(screen, (10, 10, 10), (x0, y0), (ex, ey), max(1, 6 - j)) - pygame.draw.circle(screen, (200, 80, 80), (x0, y0), 6) - - x0, y0 = ex, ey - ptr += 1 - - # 绘制状态文本 - font = pygame.font.Font(None, 28) - txt = font.render(f"Step: {self.step_count} Reward: {self.last_reward:.3f}", True, (0, 0, 0)) - screen.blit(txt, (10, 10)) - - # 更新画面 - pygame.display.flip() - - # 处理Pygame事件 - for event in pygame.event.get(): - if event.type == pygame.QUIT: - self.close() - - def close(self): - """关闭模拟器""" - # 关闭MuJoCo Viewer - if HAS_MUJOCO and self.viewer is not None: - try: - self.viewer.close() - except Exception: - pass - - # 关闭Pygame - if self.render_mode == 'human': - try: - import pygame - pygame.quit() - except Exception: - pass - - -# 测试代码(类外代码无需缩进) -if __name__ == "__main__": - # 初始化模拟器 - sim = Simulator(render_mode="human", simulator_folder="./mujoco_hand") - - # 重置环境 - obs, info = sim.reset(seed=42) - print(f"Initial observation shape: {obs.shape}") - - # 运行100步仿真 - for _ in range(100): - # 随机动作 - action = sim.action_space.sample() - obs, reward, terminated, truncated, info = sim.step(action) - - # 渲染画面 - sim.render() - - # 打印状态 - if _ % 10 == 0: - print(f"Step: {info['step']}, Reward: {reward:.3f}") - - if terminated or truncated: - print(f"Simulation ended at step {info['step']}") - break - - # 关闭模拟器 - sim.close() -======= - self.model = None - self.data = None - self.viewer = None - self.screen = None - - # 1. 先加载配置 + # 核心组件初始化(先置空,后续分步加载) + 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. 再加载模型 + # 2. 加载/生成MuJoCo模型(MJCF文件) self.model, self.data = self._load_model() - # 3. 最后校验配置 + # 3. 校验并补全配置(保证关键参数存在) self._validate_config() - # 初始化动作空间、观测空间、渲染 - self._init_action_space() - self._init_observation_space() - + # 初始化RL核心空间和渲染 + self._init_action_space() # 动作空间(执行器控制信号) + self._init_observation_space() # 观测空间(关节状态) if self.render_mode: - self._init_render() + 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", - "control_frequency": 20, - "target_joint_pos": [0.0] + "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: - return yaml.safe_load(f) - - def _load_model(self): - model_path = os.path.join(self.simulator_folder, self.config["simulation"].get("model_path", "arm_model.mjcf")) + 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") + ) + + # 模型文件不存在时,生成简单单关节机械臂MJCF if not os.path.exists(model_path): - with open(model_path, "w", encoding="utf-8") as f: - f.write(""" - """ + # 保存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"] = {} - # 如果有模型,使用模型信息,否则使用默认值 - if self.model is not None: - nq = self.model.nq - else: - nq = 1 # 默认值 - + # 获取模型关节数(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 ) - print(f"动作空间初始化: 维度={n_actuators}") + 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 + # 关节位置数和速度数 + 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 ) - print(f"观测空间初始化: 维度={obs_dim} (位置={n_qpos}, 速度={n_qvel})") + 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") - print("Pygame渲染初始化成功") + logger.info("Pygame渲染窗口初始化成功") except Exception as e: - print(f"渲染初始化失败: {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仿真状态(关节位置/速度恢复初始值) mujoco.mj_resetData(self.model, self.data) - # 重置计数器 + # 重置计数器和状态标记 self.step_count = 0 self.terminated = False self.truncated = False @@ -521,123 +279,220 @@ def reset(self, seed: Optional[int] = None) -> Tuple[np.ndarray, dict]: 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: - # 处理pygame事件 + 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投影) + # 绘制机械臂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) - # 绘制机械臂 - pygame.draw.line(self.screen, (0, 0, 0), - (center_x, center_y), (end_x, end_y), 5) + # 绘制机械臂连杆(黑色线条,宽度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 + ) - # 绘制关节 - pygame.draw.circle(self.screen, (255, 0, 0), - (int(center_x), int(center_y)), 10) - 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": - pygame.quit() + 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() \ No newline at end of file