diff --git a/.gitignore b/.gitignore index ea109aa350..63d50ff1bb 100644 Binary files a/.gitignore and b/.gitignore differ diff --git a/src/box/README.md b/src/box/README.md new file mode 100644 index 0000000000..a4f32c0853 --- /dev/null +++ b/src/box/README.md @@ -0,0 +1,60 @@ +user-in-the-box-simulator +基于Gymnasium和MuJoCo的仿真器,集成生物力学模型、感知模块与强化学习任务,支持分层强化学习与可视化渲染 + +一、环境准备 +1.虚拟环境创建与激活 +# 切换到项目根目录 +cd C:\Users\86186\user-in-the-box + +# 创建Python 3.9虚拟环境(兼容性最优) +python -m venv venv --python=3.9 + +# 激活虚拟环境(Windows PowerShell) +\venv\Scripts\Activate.ps1 +2.依赖安装 使用国内镜像源加速安装: +# 核心依赖 +pip install gymnasium==1.2.1 mujoco==2.3.5 stable-baselines3==2.2.1 pygame==2.5.2 opencv-python==4.9.0.80 -i https://pypi.tuna.tsinghua.edu.cn/simple + +# 辅助依赖 +pip install numpy==1.26.4 scipy==1.11.4 matplotlib==3.8.4 ruamel.yaml==0.18.6 certifi -i https://pypi.tuna.tsinghua.edu.cn/simple + +二、核心文件说明 + +1.simulator.py(仿真器核心) +功能:继承 gym.Env,实现仿真环境的初始化、步骤推进(step)、环境重置(reset)和可视化渲染(render),集成生物力学模型、感知模块和任务逻辑。 运行方式:需通过调用脚本(如 test_simulator.py)运行,示例见 “三、运行步骤”。 +2.main.py(辅助脚本) 功能:基于 certifi 查询 CA 证书信息(路径或内容),用于验证网络请求的安全性。 +运行方式: +# 查看证书路径 +python main.py + +# 查看证书内容 +python main.py -c +三、运行步骤 + +仿真器运行(simulator.py) +步骤 1:运行脚本 test_simulator.py + +步骤 2:执行脚本 + +python test_simulator.py +此时会弹出 Pygame 窗口,展示仿真过程(如机械臂运动、感知模块渲染)。 2. 辅助脚本运行(main.py) 在终端执行以下命令,查看证书信息: + +# 查看证书路径 +python main.py + +# 查看证书内容 +python main.py -c + +四、依赖清单 +|库名称 |版本 |用途| +|------|-------|----| +|gymnasium| 1.2.1 |强化学习环境接口| +|mujoco|2.3.5|物理仿真引擎| +|stable-baselines3|2.2.1|强化学习算法库| +|pygame |2.5.2|可视化渲染| +|opencv-python|4.9.0.80|图像感知处理| +|numpy| 1.26.4| 数值计算| +|scipy| 1.11.4|科学计算| +|matplotlib|3.8.4|数据可视化| +|ruamel.yaml|0.18.6 |配置文件解析| +|certifi |2025.10.10 |CA 证书管理| \ No newline at end of file diff --git a/src/box/mian.py b/src/box/_init_.py similarity index 100% rename from src/box/mian.py rename to src/box/_init_.py diff --git a/src/box/arm_grab_simulation_1.yaml b/src/box/arm_grab_simulation_1.yaml new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/box/main.py b/src/box/main.py new file mode 100644 index 0000000000..96f58810d7 --- /dev/null +++ b/src/box/main.py @@ -0,0 +1,15 @@ +import argparse +from certifi import where + +parser = argparse.ArgumentParser() +parser.add_argument("-c", "--contents", action="store_true", help="查看证书文件内容") +args = parser.parse_args() + +if args.contents: + # 读取 certifi 证书文件内容 + cert_path = where() + with open(cert_path, "r", encoding="utf-8") as f: + print(f.read()) +else: + # 打印证书文件路径 + print(where()) \ No newline at end of file diff --git a/src/box/perception/_init_.py b/src/box/perception/_init_.py new file mode 100644 index 0000000000..972cded186 --- /dev/null +++ b/src/box/perception/_init_.py @@ -0,0 +1,5 @@ +from .base import Perception +from .vision import VisionPerception +from .joint_state import JointStatePerception + +__all__ = ["Perception", "VisionPerception", "JointStatePerception"] \ No newline at end of file diff --git a/src/box/perception/base.py b/src/box/perception/base.py new file mode 100644 index 0000000000..3033ccc71b --- /dev/null +++ b/src/box/perception/base.py @@ -0,0 +1,109 @@ +import numpy as np +import mujoco +from collections import defaultdict + +class PerceptionModule: + """感知子模块基类,所有具体感知模块需继承此类""" + def __init__(self, modality, model, data, **kwargs): + self.modality = modality # 感知模态名称(如vision/joint_state) + self.model = model # MuJoCo模型 + self.data = data # MuJoCo数据 + self.kwargs = kwargs # 模块参数 + self.cameras = [] # 关联的相机(视觉类模块用) + + def reset(self, model, data): + """重置感知模块状态""" + self.model = model + self.data = data + + def update(self, model, data): + """每步更新感知数据""" + pass + + def get_observation(self, model, data, info=None): + """获取该模块的观测数据(需子类实现)""" + raise NotImplementedError + + def get_observation_space_params(self): + """获取Gymnasium观测空间参数(需子类实现)""" + raise NotImplementedError + + def get_renders(self): + """获取可视化渲染结果(视觉类模块用)""" + return [] + + def close(self): + """释放资源""" + pass + + +class Perception: + """感知总控制器,管理所有感知子模块,适配仿真器架构""" + def __init__(self, model, data, bm_model, perception_modules, common_kwargs): + self.model = model + self.data = data + self.bm_model = bm_model # 关联生物力学模型 + self.common_kwargs = common_kwargs # 通用参数(如dt、callbacks) + + # 初始化感知子模块 + self.perception_modules = [] + self.cameras_dict = defaultdict(list) # 相机映射(适配渲染) + self.nu = 0 # 感知模块动作维度(无动作则为0,保持与仿真器兼容) + + # 遍历配置的感知模块,初始化实例 + for module_cls, module_kwargs in perception_modules.items(): + module = module_cls( + model=model, + data=data, + **module_kwargs + ) + self.perception_modules.append(module) + # 关联相机(用于渲染) + if hasattr(module, 'cameras') and module.cameras: + self.cameras_dict[module] = module.cameras + + def reset(self, model, data): + """重置所有感知模块""" + self.model = model + self.data = data + for module in self.perception_modules: + module.reset(model, data) + + def update(self, model, data): + """每步更新所有感知模块""" + self.model = model + self.data = data + for module in self.perception_modules: + module.update(model, data) + + def get_observation(self, model, data, info=None): + """收集所有感知模块的观测数据,返回字典(适配仿真器观测空间)""" + observation = {} + for module in self.perception_modules: + obs = module.get_observation(model, data, info) + if obs is not None: + observation[module.modality] = obs + return observation + + def get_state(self, model, data): + """获取感知模块状态(用于仿真器状态记录)""" + state = {} + for module in self.perception_modules: + state[f"perception_{module.modality}"] = module.get_observation(model, data) + return state + + def get_renders(self): + """获取所有视觉类模块的渲染结果(适配仿真器可视化)""" + renders = [] + for module in self.perception_modules: + renders.extend(module.get_renders()) + return renders + + def set_ctrl(self, model, data, ctrl): + """感知模块动作控制(无动作则空实现,保持与仿真器接口兼容)""" + pass + + def close(self, **kwargs): + """释放所有感知模块资源""" + for module in self.perception_modules: + module.close() \ No newline at end of file diff --git a/src/box/perception/joint_state.py b/src/box/perception/joint_state.py new file mode 100644 index 0000000000..c99eea72be --- /dev/null +++ b/src/box/perception/joint_state.py @@ -0,0 +1,77 @@ +import numpy as np +import mujoco +from .base import PerceptionModule + +class JointStatePerception(PerceptionModule): + """关节状态感知模块,采集关节角度、速度、力矩""" + def __init__(self, model, data, joint_names=None, include_velocity=True, + include_torque=True, normalize=True, **kwargs): + super().__init__(modality="joint_state", model=model, data=data,** kwargs) + + # 关节配置 + self.joint_names = joint_names or self._get_all_joints() # 默认所有关节 + self.include_velocity = include_velocity + self.include_torque = include_torque + self.normalize = normalize + + # 获取关节ID和范围(用于归一化) + self.joint_ids = [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name) + for name in self.joint_names] + self.joint_ranges = model.jnt_range[self.joint_ids] # 关节角度范围(min, max) + + def _get_all_joints(self): + """获取模型中所有关节名称""" + joint_names = [] + for i in range(self.model.njnt): + name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_JOINT, i) + if name: + joint_names.append(name) + return joint_names + + def _normalize_qpos(self, qpos): + """归一化关节角度到[-1, 1]""" + norm_qpos = (qpos - self.joint_ranges[:, 0]) / (self.joint_ranges[:, 1] - self.joint_ranges[:, 0] + 1e-8) + return (norm_qpos - 0.5) * 2 # 映射到[-1,1] + + def get_observation(self, model, data, info=None): + """获取关节状态观测(角度+速度+力矩)""" + # 关节角度 + qpos = data.qpos[[model.jnt_qposadr[jid] for jid in self.joint_ids]] + if self.normalize: + qpos = self._normalize_qpos(qpos) + + # 拼接观测 + obs_list = [qpos.astype(np.float32)] + + # 关节速度 + if self.include_velocity: + qvel = data.qvel[[model.jnt_dofadr[jid] for jid in self.joint_ids]] + # 速度归一化到[-1,1](基于经验范围) + if self.normalize: + qvel = np.clip(qvel / 10.0, -1.0, 1.0) + obs_list.append(qvel.astype(np.float32)) + + # 关节力矩 + if self.include_torque: + torque = data.qfrc_actuator[[model.jnt_dofadr[jid] for jid in self.joint_ids]] + # 力矩归一化到[-1,1] + if self.normalize: + torque = np.clip(torque / 50.0, -1.0, 1.0) + obs_list.append(torque.astype(np.float32)) + + return np.concatenate(obs_list) + + def get_observation_space_params(self): + """定义关节状态观测空间参数""" + # 计算观测维度 + dim = len(self.joint_ids) + if self.include_velocity: + dim += len(self.joint_ids) + if self.include_torque: + dim += len(self.joint_ids) + + return { + "low": -1.0 if self.normalize else -np.inf, + "high": 1.0 if self.normalize else np.inf, + "shape": (dim,) + } \ No newline at end of file diff --git a/src/box/perception/vision.py b/src/box/perception/vision.py new file mode 100644 index 0000000000..db72c89b67 --- /dev/null +++ b/src/box/perception/vision.py @@ -0,0 +1,62 @@ +import numpy as np +import mujoco +from .base import PerceptionModule +from src.box.utils.rendering import Camera # 复用仿真器的Camera类 + +class VisionPerception(PerceptionModule): + """视觉感知模块,支持RGB/深度图采集""" + def __init__(self, model, data, camera_id="rgb_camera", resolution=(640, 480), + capture_depth=False, normalize=True, **kwargs): + super().__init__(modality="vision", model=model, data=data, **kwargs) + + # 相机配置 + self.camera_id = camera_id + self.resolution = resolution + self.capture_depth = capture_depth + self.normalize = normalize + + # 初始化相机(复用仿真器的Camera类) + self.camera = Camera( + context=self.kwargs.get("rendering_context"), + model=model, + data=data, + camera_id=camera_id, + dt=self.kwargs.get("dt", 0.01) + ) + self.cameras = [self.camera] # 关联到模块相机列表 + + def get_observation(self, model, data, info=None): + """获取视觉观测数据(RGB/深度图)""" + # 渲染相机画面 + rgb_img, depth_img = self.camera.render() + + # 拼接观测(RGB为主,可选深度) + if self.capture_depth: + # 归一化深度图 + depth_img = (depth_img - depth_img.min()) / (depth_img.max() - depth_img.min() + 1e-8) + obs = np.concatenate([rgb_img, depth_img[..., np.newaxis]], axis=-1) + else: + obs = rgb_img + + # 归一化到[0,1](可选) + if self.normalize: + obs = obs.astype(np.float32) / 255.0 + + # 展平为一维向量(适配RL观测空间) + return obs.flatten() + + def get_observation_space_params(self): + """定义视觉观测空间参数(适配Gymnasium)""" + # 计算观测维度:RGB(3通道) + 深度(可选1通道) + channels = 3 + (1 if self.capture_depth else 0) + shape = (self.resolution[0] * self.resolution[1] * channels,) + return { + "low": 0.0, + "high": 1.0, + "shape": shape + } + + def get_renders(self): + """返回原始RGB图像(用于仿真器渲染)""" + rgb_img, _ = self.camera.render() + return [rgb_img] \ No newline at end of file diff --git a/src/box/simulator.py b/src/box/simulator.py index 954b06b7a5..866969922e 100644 --- a/src/box/simulator.py +++ b/src/box/simulator.py @@ -1,816 +1,278 @@ -import gymnasium as gym -from gymnasium import spaces -import pygame -import mujoco import os import numpy as np -import scipy -import matplotlib -import sys -import importlib -import shutil -import inspect -import pathlib -from datetime import datetime -import copy -from collections import defaultdict -import xml.etree.ElementTree as ET - -from stable_baselines3 import PPO #required to load a trained LLC policy in HRL approach - -from .perception.base import Perception -from .utils.rendering import Camera, Context -from .utils.functions import output_path, parent_path, is_suitable_package_name, parse_yaml, write_yaml - - -class Simulator(gym.Env): - """ - The Simulator class contains functionality to build a standalone Python package from a config file. The built package - integrates a biomechanical model, a task model, and a perception model into one simulator that implements a gym.Env - interface. - """ - - # May be useful for later, the three digit number suffix is of format X.Y.Z where X is a major version. - version = "1.1.0" - - @classmethod - def get_class(cls, *args): - """ Returns a class from given strings. The last element in args should contain the class name. """ - # TODO check for incorrect module names etc - modules = ".".join(args[:-1]) - if "." in args[-1]: - splitted = args[-1].split(".") - if modules == "": - modules = ".".join(splitted[:-1]) - else: - modules += "." + ".".join(splitted[:-1]) - cls_name = splitted[-1] - else: - cls_name = args[-1] - module = cls.get_module(modules) - return getattr(module, cls_name) - - @classmethod - def get_module(cls, *args): - """ Returns a module from given strings. """ - src = __name__.split(".")[0] - modules = ".".join(args) - return importlib.import_module(src + "." + modules) - - @classmethod - def build(cls, config): - """ Builds a simulator based on a config. The input 'config' may be a dict (parsed from YAML) or path to a YAML file - - Args: - config: - - A dict containing configuration information. See example configs in folder uitb/configs/ - - A path to a config file - """ - - # If config is a path to the config file, parse it first - if isinstance(config, str): - if not os.path.isfile(config): - raise FileNotFoundError(f"Given config file {config} does not exist") - config = parse_yaml(config) - - # Make sure required things are defined in config - assert "simulation" in config, "Simulation specs (simulation) must be defined in config" - assert "bm_model" in config["simulation"], "Biomechanical model (bm_model) must be defined in config" - assert "task" in config["simulation"], "task (task) must be defined in config" - - assert "run_parameters" in config["simulation"], "Run parameters (run_parameters) must be defined in config" - run_parameters = config["simulation"]["run_parameters"].copy() - assert "action_sample_freq" in run_parameters, "Action sampling frequency (action_sample_freq) must be defined " \ - "in run parameters" - - # Set simulator version - config["version"] = cls.version - - # Save generated simulators to uitb/simulators - if "simulator_folder" in config: - simulator_folder = os.path.normpath(config["simulator_folder"]) - else: - simulator_folder = os.path.join(output_path(), config["simulator_name"]) - - # If 'package_name' is not defined use 'simulator_name' - if "package_name" not in config: - config["package_name"] = config["simulator_name"] - if not is_suitable_package_name(config["package_name"]): - raise NameError("Package name defined in the config file (either through 'package_name' or 'simulator_name') is " - "not a suitable Python package name. Use only lower-case letters and underscores instead of " - "spaces, and the name cannot start with a number.") - - # The name used in gym has a suffix -v0 - config["gym_name"] = "uitb:" + config["package_name"] + "-v0" - - # Create a simulator in the simulator folder - cls._clone(simulator_folder, config["package_name"]) - - # Load task class - task_cls = cls.get_class("tasks", config["simulation"]["task"]["cls"]) - task_cls.clone(simulator_folder, config["package_name"], app_executable=config["simulation"]["task"].get("kwargs", {}).get("unity_executable", None)) - simulation = task_cls.initialise(config["simulation"]["task"].get("kwargs", {})) - - # Set some compiler options - # TODO: would make more sense to have a separate "environment" class / xml file that defines all these defaults, - # including e.g. cameras, lighting, etc., so that they could be easily changed. Task and biomechanical model would - # be integrated into that object - compiler_defaults = {"inertiafromgeom": "auto", "balanceinertia": "true", "boundmass": "0.001", - "boundinertia": "0.001", "inertiagrouprange": "0 1"} - compiler = simulation.find("compiler") - if compiler is None: - ET.SubElement(simulation, "compiler", compiler_defaults) - else: - compiler.attrib.update(compiler_defaults) - - # Load biomechanical model class - bm_cls = cls.get_class("bm_models", config["simulation"]["bm_model"]["cls"]) - bm_cls.clone(simulator_folder, config["package_name"]) - bm_cls.insert(simulation) - - # Add perception modules - for module_cfg in config["simulation"].get("perception_modules", []): - module_cls = cls.get_class("perception", module_cfg["cls"]) - module_kwargs = module_cfg.get("kwargs", {}) - module_cls.clone(simulator_folder, config["package_name"]) - module_cls.insert(simulation, **module_kwargs) - - # Clone also RL library files so the package will be completely standalone - rl_cls = cls.get_class("rl", config["rl"]["algorithm"]) - rl_cls.clone(simulator_folder, config["package_name"]) - - # TODO read the xml file directly from task.getroot() instead of writing it to a file first; need to input a dict - # of assets to mujoco.MjModel.from_xml_path - simulation_file = os.path.join(simulator_folder, config["package_name"], "simulation") - with open(simulation_file+".xml", 'w') as file: - simulation.write(file, encoding='unicode') - - # Initialise the simulator - model, _, _, _, _, _ = \ - cls._initialise(config, simulator_folder, {**run_parameters, "build": True}) - - # Now that simulator has been initialised, everything should be set. Now we want to save the xml file again, but - # mujoco only is able to save the latest loaded xml file (which is either the task or bm model xml files which are - # are read in their __init__ functions), hence we need to read the file we've generated again before saving the - # modified model - mujoco.MjModel.from_xml_path(simulation_file+".xml") - mujoco.mj_saveLastXML(simulation_file+".xml", model) - - # Save the modified model also as binary for faster loading - mujoco.mj_saveModel(model, simulation_file+".mjcf", None) - - # Input built time into config - config["built"] = datetime.now().strftime("%Y-%m-%d %H:%M:%S") - - # Save config - write_yaml(config, os.path.join(simulator_folder, "config.yaml")) - - return simulator_folder - - @classmethod - def _clone(cls, simulator_folder, package_name): - """ Create a folder for the simulator being built, and copy or create relevant files. - - Args: - simulator_folder: Location of the simulator. - package_name: Name of the simulator (which is a python package). - """ - - # Create the folder - dst = os.path.join(simulator_folder, package_name) - os.makedirs(dst, exist_ok=True) - - # Copy simulator - src = pathlib.Path(inspect.getfile(cls)) - shutil.copyfile(src, os.path.join(dst, src.name)) - - # Create __init__.py with env registration - with open(os.path.join(dst, "__init__.py"), "w") as file: - file.write("from .simulator import Simulator\n\n") - file.write("from gymnasium.envs.registration import register\n") - file.write("import pathlib\n\n") - file.write("module_folder = pathlib.Path(__file__).parent\n") - file.write("simulator_folder = module_folder.parent\n") - file.write("kwargs = {'simulator_folder': simulator_folder}\n") - file.write("register(id=f'{module_folder.stem}-v0', entry_point=f'{module_folder.stem}.simulator:Simulator', kwargs=kwargs)\n") - - # Copy utils - shutil.copytree(os.path.join(parent_path(src), "utils"), os.path.join(simulator_folder, package_name, "utils"), - dirs_exist_ok=True) - # Copy train - shutil.copytree(os.path.join(parent_path(src), "train"), os.path.join(simulator_folder, package_name, "train"), - dirs_exist_ok=True) - # Copy test - shutil.copytree(os.path.join(parent_path(src), "test"), os.path.join(simulator_folder, package_name, "test"), - dirs_exist_ok=True) - - @classmethod - def _initialise(cls, config, simulator_folder, run_parameters): - """ Initialise a simulator -- i.e., create a MjModel, MjData, and initialise all necessary variables. - - Args: - config: A config dict. - simulator_folder: Location of the simulator. - run_parameters: Important run time variables that may also be used to override parameters. - """ - - # Get task class and kwargs - task_cls = cls.get_class("tasks", config["simulation"]["task"]["cls"]) - task_kwargs = config["simulation"]["task"].get("kwargs", {}) - - # Get bm class and kwargs - bm_cls = cls.get_class("bm_models", config["simulation"]["bm_model"]["cls"]) - bm_kwargs = config["simulation"]["bm_model"].get("kwargs", {}) - - # Initialise perception modules - perception_modules = {} - for module_cfg in config["simulation"].get("perception_modules", []): - module_cls = cls.get_class("perception", module_cfg["cls"]) - module_kwargs = module_cfg.get("kwargs", {}) - perception_modules[module_cls] = module_kwargs - - # Get simulation file - simulation_file = os.path.join(simulator_folder, config["package_name"], "simulation") - - # Load the mujoco model; try first with the binary model (faster, contains some parameters that may be lost when - # re-saving xml files like body mass). For some reason the binary model fails to load in some situations (like - # when the simulator has been built on a different computer) - # TODO loading from binary disabled, weird problems (like a body not found from model when loaded from binary, but - # found correctly when model loaded from xml) - # try: - # model = mujoco.MjModel.from_binary_path(simulation_file + ".mjcf") - # except: # TODO what was the exception type - model = mujoco.MjModel.from_xml_path(simulation_file + ".xml") - - # Initialise MjData - data = mujoco.MjData(model) - - # Add frame skip and dt to run parameters - run_parameters["frame_skip"] = int(1 / (model.opt.timestep * run_parameters["action_sample_freq"])) - run_parameters["dt"] = model.opt.timestep*run_parameters["frame_skip"] - - # Initialise a rendering context, required for e.g. some vision modules - run_parameters["rendering_context"] = Context(model, - max_resolution=run_parameters.get("max_resolution", [1280, 960])) - - # Initialise callbacks - callbacks = {} - for cb in run_parameters.get("callbacks", []): - callbacks[cb["name"]] = cls.get_class(cb["cls"])(cb["name"], **cb["kwargs"]) - - # Now initialise the actual classes; model and data are input to the inits so that stuff can be modified if needed - # (e.g. move target to a specific position wrt to a body part) - task = task_cls(model, data, **{**task_kwargs, **callbacks, **run_parameters}) - bm_model = bm_cls(model, data, **{**bm_kwargs, **callbacks, **run_parameters}) - perception = Perception(model, data, bm_model, perception_modules, {**callbacks, **run_parameters}) - - return model, data, task, bm_model, perception, callbacks - - @classmethod - def get(cls, simulator_folder, render_mode="rgb_array", render_mode_perception="embed", render_show_depths=False, run_parameters=None, use_cloned=True): - """ Returns a Simulator that is located in given folder. - - Args: - simulator_folder: Location of the simulator. - render_mode: Whether render() will return a single rgb array (render_mode="rgb_array"), - a list of rgb arrays (render_mode="rgb_array_list"; - adapted from https://github.com/openai/gym/blob/master/gym/wrappers/render_collection.py), - or None while the frames in a separate PyGame window are updated directly when calling - step() or reset() (render_mode="human"; - adapted from https://github.com/openai/gym/blob/master/gym/wrappers/human_rendering.py)). - render_mode_perception: Whether images of visual perception modules should be directly embedded into main camera view ("embed"), stored as separate videos ("separate"), or not used at all [which allows to watch vision in Unity Editor if debug mode is enabled/standalone app is disabled] (None) - render_show_depths: Whether depth images of visual perception modules should be included in rendering. - run_parameters: Can be used to override parameters during run time. - use_cloned: Can be useful for debugging. Set to False to use original files instead of the ones that have been - cloned/copied during building phase. - """ - - # Read config file - config_file = os.path.join(simulator_folder, "config.yaml") - try: - config = parse_yaml(config_file) - except: - raise FileNotFoundError(f"Could not open file {config_file}") - - # Make sure the simulator has been built - if "built" not in config: - raise RuntimeError("Simulator has not been built") - - # Make sure simulator_folder is in path (used to import gen_cls_cloned) - if simulator_folder not in sys.path: - sys.path.insert(0, simulator_folder) - - # Get Simulator class - gen_cls_cloned = getattr(importlib.import_module(config["package_name"]), "Simulator") - if hasattr(gen_cls_cloned, "version"): - _legacy_mode = False - gen_cls_cloned_version = gen_cls_cloned.version.split("-v")[-1] - else: - _legacy_mode = True - gen_cls_cloned_version = gen_cls_cloned.id.split("-v")[-1] #deprecated - if use_cloned: - gen_cls = gen_cls_cloned - else: - gen_cls = cls - gen_cls_version = gen_cls.version.split("-v")[-1] - - if gen_cls_version.split(".")[0] > gen_cls_cloned_version.split(".")[0]: - raise RuntimeError( - f"""Severe version mismatch. The simulator '{config["simulator_name"]}' has version {gen_cls_cloned_version}, while your uitb package has version {gen_cls_version}.\nTo run with version {gen_cls_cloned_version}, set 'use_cloned=True'.""") - elif gen_cls_version.split(".")[1] > gen_cls_cloned_version.split(".")[1]: - print( - f"""WARNING: Version mismatch. The simulator '{config["simulator_name"]}' has version {gen_cls_cloned_version}, while your uitb package has version {gen_cls_version}.\nTo run with version {gen_cls_version}, set 'use_cloned=True'.""") - - if _legacy_mode: - _simulator = gen_cls(simulator_folder, run_parameters=run_parameters) - else: - try: - _simulator = gen_cls(simulator_folder, render_mode=render_mode, render_mode_perception=render_mode_perception, render_show_depths=render_show_depths, - run_parameters=run_parameters) - except TypeError: - _simulator = gen_cls(simulator_folder, render_mode=render_mode, render_show_depths=render_show_depths, - run_parameters=run_parameters) - - # Return Simulator object - return _simulator - - def __init__(self, simulator_folder, render_mode="rgb_array", render_mode_perception="embed", render_show_depths=False, run_parameters=None): - """ Initialise a new `Simulator`. - - Args: - simulator_folder: Location of a simulator. - render_mode: Whether render() will return a single rgb array (render_mode="rgb_array"), - a list of rgb arrays (render_mode="rgb_array_list"; - adapted from https://github.com/openai/gym/blob/master/gym/wrappers/render_collection.py), - or None while the frames in a separate PyGame window are updated directly when calling - step() or reset() (render_mode="human"; - adapted from https://github.com/openai/gym/blob/master/gym/wrappers/human_rendering.py)). - render_mode_perception: Whether images of visual perception modules should be directly embedded into main camera view ("embed"), stored as separate videos ("separate"), or not used at all [which allows to watch vision in Unity Editor if debug mode is enabled/standalone app is disabled] (None) - render_show_depths: Whether depth images of visual perception modules should be included in rendering. - run_parameters: Can be used to override parameters during run time. - """ - - # Make sure simulator exists - if not os.path.exists(simulator_folder): - raise FileNotFoundError(f"Simulator folder {simulator_folder} does not exists") - self._simulator_folder = simulator_folder - - # Read config - self._config = parse_yaml(os.path.join(self._simulator_folder, "config.yaml")) - - # Get run parameters: these parameters can be used to override parameters used during training - self._run_parameters = self._config["simulation"]["run_parameters"].copy() - self._run_parameters.update(run_parameters or {}) - - # Initialise simulation - self._model, self._data, self.task, self.bm_model, self.perception, self.callbacks = \ - self._initialise(self._config, self._simulator_folder, self._run_parameters) - - # Set action space TODO for now we assume all actuators have control signals between [-1, 1] - self.action_space = self._initialise_action_space() - - # Set observation space - self.observation_space = self._initialise_observation_space() - - # Collect some episode statistics - self._episode_statistics = {"length (seconds)": 0, "length (steps)": 0, "reward": 0} - - # Initialise viewer - self._GUI_camera = Camera(self._run_parameters["rendering_context"], self._model, self._data, camera_id='for_testing', - dt=self._run_parameters["dt"]) - - self._render_mode = render_mode - self._render_mode_perception = render_mode_perception #whether perception camera views should be directly embedded into camera view of camera_id ("embed"), stored in self._render_stack_perception ("separate"), or not used at all "separate"), or not used at all [which allows to watch vision in Unity Editor if debug mode is enabled/standalone app is disabled] (None) - self._render_stack = [] #only used if render_mode == "rgb_array_list" - self._render_stack_perception = defaultdict(list) #only used if render_mode == "rgb_array_list" and self._render_mode_perception == "separate" - self._render_stack_pop = True #If True, clear the render stack after .render() is called. - self._render_stack_clean_at_reset = True #If True, clear the render stack when .reset() is called. - self._render_show_depths = render_show_depths #If True, depth images of visual perception modules are included in GUI rendering. - self._render_screen_size = None #only used if render_mode == "human" - self._render_window = None #only used if render_mode == "human" - self._render_clock = None #only used if render_mode == "human" - - if 'llc' in self.config: #if HRL approach is used - llc_simulator_folder = os.path.join(output_path(), self.config["llc"]["simulator_name"]) - if llc_simulator_folder not in sys.path: - sys.path.insert(0, llc_simulator_folder) - if not os.path.exists(llc_simulator_folder): - raise FileNotFoundError(f"Simulator folder {llc_simulator_folder} does not exists") - llccheckpoint_dir = os.path.join(llc_simulator_folder, 'checkpoints') - # Load policy TODO should create a load method for uitb.rl.BaseRLModel - print(f'Loading model: {os.path.join(llccheckpoint_dir, self.config["llc"]["checkpoint"])}\n') - self.llc_model = PPO.load(os.path.join(llccheckpoint_dir, self.config["llc"]["checkpoint"])) - self.action_space = self._initialise_HRL_action_space() - self._max_steps = self.config["llc"]["llc_ratio"] - self._dwell_threshold = int(0.5*self._max_steps) - self._target_radius = 0.05 - self._independent_dofs = [] - self._independent_joints = [] - joints = self.config["llc"]["joints"] - for joint in joints: - joint_id = mujoco.mj_name2id(self._model, mujoco.mjtObj.mjOBJ_JOINT, joint) - if self._model.jnt_type[joint_id] not in [mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE]: - raise NotImplementedError(f"Only 'hinge' and 'slide' joints are supported, joint " - f"{joint} is of type {mujoco.mjtJoint(self._model.jnt_type[joint_id]).name}") - self._independent_dofs.append(self._model.jnt_qposadr[joint_id]) - self._independent_joints.append(joint_id) - self._jnt_range = self._model.jnt_range[self._independent_joints] - - - #To normalize joint ranges for llc - def _normalise_qpos(self, qpos): - # Normalise to [0, 1] - qpos = (qpos - self._jnt_range[:, 0]) / (self._jnt_range[:, 1] - self._jnt_range[:, 0]) - # Normalise to [-1, 1] - qpos = (qpos - 0.5) * 2 - return qpos - - def _initialise_action_space(self): - """ Initialise action space. """ - num_actuators = self.bm_model.nu + self.perception.nu - actuator_limits = np.ones((num_actuators,2)) * np.array([-1.0, 1.0]) - return spaces.Box(low=np.float32(actuator_limits[:, 0]), high=np.float32(actuator_limits[:, 1])) - - def _initialise_HRL_action_space(self): - bm_nu = self.bm_model.nu - bm_jnt_range = np.ones((bm_nu,2)) * np.array([-1.0, 1.0]) - perception_nu = self.perception.nu - perception_jnt_range = np.ones((perception_nu,2)) * np.array([-1.0, 1.0]) - jnt_range = np.concatenate((bm_jnt_range, perception_jnt_range), axis=0) - action_space = gym.spaces.Box(low=jnt_range[:,0], high=jnt_range[:,1]) - return action_space - - def _initialise_observation_space(self): - """ Initialise observation space. """ - observation = self.get_observation() - obs_dict = dict() - for module in self.perception.perception_modules: - obs_dict[module.modality] = spaces.Box(dtype=np.float32, **module.get_observation_space_params()) - if "stateful_information" in observation: - obs_dict["stateful_information"] = spaces.Box(dtype=np.float32, - **self.task.get_stateful_information_space_params()) - return spaces.Dict(obs_dict) - - def _get_qpos(self, model, data): - qpos = data.qpos[self._independent_dofs].copy() - qpos = self._normalise_qpos(qpos) - return qpos - - def step(self, action): - """ Step simulation forward with given actions. - - Args: - action: Actions sampled from a policy. Limited to range [-1, 1]. - """ - if 'llc' in self.config: #if HRL approach is used - self.task._target_qpos = action # action to pass to LLC - self._steps = 0 # Initialise loop control to 0 - #acc_reward = 0 #To be used when rewards are being accumulated in llc steps - - while self._steps < self._max_steps: # loop for llc controls based on llc_ratio - - llc_action, _states = self.llc_model.predict(self.get_llcobservation(action), deterministic=True) # Get BM action from LLC - # Set control for the bm model - self.bm_model.set_ctrl(self._model, self._data, llc_action) - - # Set control for perception modules (e.g. eye movements) - self.perception.set_ctrl(self._model, self._data, action[self.bm_model.nu:]) - - # Advance the simulation - mujoco.mj_step(self._model, self._data, nstep=int(self._run_parameters["frame_skip"])) # Number of timesteps to skip for LLC - - # Update bm model (e.g. update constraints); updates also effort model - self.bm_model.update(self._model, self._data) - - # Update perception modules - self.perception.update(self._model, self._data) - - dist = np.abs(action - self._get_qpos(self._model, self._data)) - - # Update environment - reward, terminated, truncated, info = self.task.update(self._model, self._data) - - # Add an effort cost to reward - reward -= self.bm_model.get_effort_cost(self._model, self._data) - - #acc_reward += reward #To be used when rewards are being accumulated in llc steps - - # Get observation - obs = self.get_observation() - - # Add frame to stack - if self._render_mode == "rgb_array_list": - self._render_stack.append(self._GUI_rendering()) - elif self._render_mode == "human": - self._GUI_rendering_pygame() - - if truncated or terminated: - break - - # Pointing - if "target_spawned" in info: - if info["target_spawned"] or info["target_hit"]: - break - - # Choice Reaction - elif "new_button_generated" in info: - if info["new_button_generated"] or info["target_hit"]: - break - - self._steps += 1 - if np.all(dist < self._target_radius): - break - - return obs, reward, terminated, truncated, info - - else: - # Set control for the bm model - self.bm_model.set_ctrl(self._model, self._data, action[:self.bm_model.nu]) - - # Set control for perception modules (e.g. eye movements) - self.perception.set_ctrl(self._model, self._data, action[self.bm_model.nu:]) - - # Advance the simulation - mujoco.mj_step(self._model, self._data, nstep=self._run_parameters["frame_skip"]) - - # Update bm model (e.g. update constraints); updates also effort model - self.bm_model.update(self._model, self._data) - - # Update perception modules - self.perception.update(self._model, self._data) - - # Update environment - reward, terminated, truncated, info = self.task.update(self._model, self._data) - - # Add an effort cost to reward - effort_cost = self.bm_model.get_effort_cost(self._model, self._data) - info["EffortCost"] = effort_cost - reward -= effort_cost - - # Get observation - obs = self.get_observation(info) - - # Add frame to stack - if self._render_mode == "rgb_array_list": - self._render_stack.append(self._GUI_rendering()) - elif self._render_mode == "human": - self._GUI_rendering_pygame() - - return obs, reward, terminated, truncated, info - - - def get_observation(self, info=None): - """ Returns an observation from the perception model. - - Returns: - A dict with observations from individual perception modules. May also contain stateful information from a task. - """ - - # Get observation from perception - observation = self.perception.get_observation(self._model, self._data, info) - - # Add any stateful information that is required - stateful_information = self.task.get_stateful_information(self._model, self._data) - if stateful_information.size > 0: #TODO: define stateful_information (and encoder) that can be used as default, if no stateful information is provided (zero-size arrays do not work with sb3 currently...) - observation["stateful_information"] = stateful_information - - return observation - - def get_llcobservation(self,action): - """ Returns an observation from the perception model. - - Returns: - A dict with observations from individual perception modules. May also contain stateful information from a task. - """ - - # Get observation from perception - observation = self.perception.get_observation(self._model, self._data) - - # Remove Vision for LLC - observation.pop("vision") - qpos = self._get_qpos(self._model, self._data) - qpos_diff = action - qpos - - # Stateful Information for LLC policy - stateful_information = qpos_diff - if stateful_information is not None: - observation["stateful_information"] = stateful_information - - return observation - - - def reset(self, seed=None): - """ Reset the simulator and return an observation. """ - - super().reset(seed=seed) - - # Reset sim - mujoco.mj_resetData(self._model, self._data) - - # Reset all models - self.bm_model.reset(self._model, self._data) - self.perception.reset(self._model, self._data) - info = self.task.reset(self._model, self._data) - - # Do a forward so everything will be set - mujoco.mj_forward(self._model, self._data) - - if self._render_mode == "rgb_array_list": - if self._render_stack_clean_at_reset: - self._render_stack = [] - self._render_stack_perception = defaultdict(list) - self._render_stack.append(self._GUI_rendering()) - elif self._render_mode == "human": - self._GUI_rendering_pygame() - - return self.get_observation(), info - - def render(self): - if self._render_mode == "rgb_array_list": - render_stack = self._render_stack - if self._render_stack_pop: - self._render_stack = [] - return render_stack - elif self._render_mode == "rgb_array": - return self._GUI_rendering() - else: - return None - - def get_render_stack_perception(self): - render_stack_perception = self._render_stack_perception - # if self._render_stack_pop: - # self._render_stack_perception = defaultdict(list) - return render_stack_perception - - def _GUI_rendering(self): - # Grab an image from the 'for_testing' camera and grab all GUI-prepared images from included visual perception modules, and display them 'picture-in-picture' - - # Grab images - img, _ = self._GUI_camera.render() - - if self._render_mode_perception == "embed": - # Embed perception camera images into main camera image +import pygame +import mujoco +import yaml +from gymnasium import spaces +from typing import Dict, Any, Optional, Tuple +import mujoco.viewer + +class Simulator: + def __init__(self, simulator_folder: str, render_mode: Optional[str] = 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 + self.data = None + self.viewer = None + self.screen = None + + # 1. 先加载配置 + self.config = self._load_config() + # 2. 再加载模型 + self.model, self.data = self._load_model() + # 3. 最后校验配置 + self._validate_config() + + # 初始化动作空间、观测空间、渲染 + self._init_action_space() + self._init_observation_space() - # perception_camera_images = [rgb_or_depth_array for camera in self.perception.cameras - # for rgb_or_depth_array in camera.render() if rgb_or_depth_array is not None] - perception_camera_images = self.perception.get_renders() - - # TODO: add text annotations to perception camera images - if len(perception_camera_images) > 0: - _img_size = img.shape[:2] #(height, width) - - - # Vertical alignment of perception camera images, from bottom right to top right - ## TODO: allow for different inset locations - _desired_subwindow_height = np.round(_img_size[0] / len(perception_camera_images)).astype(int) - _maximum_subwindow_width = np.round(0.2 * _img_size[1]).astype(int) - - perception_camera_images_resampled = [] - for ocular_img in perception_camera_images: - # Convert 2D depth arrays to 3D heatmap arrays - if ocular_img.ndim == 2: - if self._render_show_depths: - ocular_img = matplotlib.pyplot.imshow(ocular_img, cmap=matplotlib.pyplot.cm.jet, interpolation='bicubic').make_image('TkAgg', unsampled=True)[0][ - ..., :3] - matplotlib.pyplot.close() #delete image - else: - continue - - resample_factor = min(_desired_subwindow_height / ocular_img.shape[0], _maximum_subwindow_width / ocular_img.shape[1]) - - resample_height = np.round(ocular_img.shape[0] * resample_factor).astype(int) - resample_width = np.round(ocular_img.shape[1] * resample_factor).astype(int) - resampled_img = np.zeros((resample_height, resample_width, ocular_img.shape[2]), dtype=np.uint8) - for channel in range(ocular_img.shape[2]): - resampled_img[:, :, channel] = scipy.ndimage.zoom(ocular_img[:, :, channel], resample_factor, order=0) - - perception_camera_images_resampled.append(resampled_img) - - ocular_img_bottom = _img_size[0] - for ocular_img_idx, ocular_img in enumerate(perception_camera_images_resampled): - #print(f"Modify ({ocular_img_bottom - ocular_img.shape[0]}, { _img_size[1] - ocular_img.shape[1]})-({ocular_img_bottom}, {img.shape[1]}).") - img[ocular_img_bottom - ocular_img.shape[0]:ocular_img_bottom, _img_size[1] - ocular_img.shape[1]:] = ocular_img - ocular_img_bottom -= ocular_img.shape[0] - # input((len(perception_camera_images_resampled), perception_camera_images_resampled[0].shape, img.shape)) - elif self._render_mode_perception == "separate": - for module, camera_list in self.perception.cameras_dict.items(): - for camera in camera_list: - for rgb_or_depth_array in camera.render(): - if rgb_or_depth_array is not None: - self._render_stack_perception[f"{module.modality}/{type(camera).__name__}"].append(rgb_or_depth_array) - - return img - - def _GUI_rendering_pygame(self): - rgb_array = np.transpose(self._GUI_rendering(), axes=(1, 0, 2)) - - if self._render_screen_size is None: - self._render_screen_size = rgb_array.shape[:2] - - assert self._render_screen_size == rgb_array.shape[ - :2], f"Expected an rgb array of shape {self._render_screen_size} from self._GUI_camera, but received an rgb array of shape {rgb_array.shape[:2]}. " - - if self._render_window is None: - pygame.init() - pygame.display.init() - self._render_window = pygame.display.set_mode(self._render_screen_size) - - if self._render_clock is None: - self._render_clock = pygame.time.Clock() - - surf = pygame.surfarray.make_surface(rgb_array) - self._render_window.blit(surf, (0, 0)) - pygame.event.pump() - self._render_clock.tick(self.fps) - pygame.display.flip() - - def close(self): - """ Close the rendering window (if self._render_mode == 'human').""" - super().close() - if self._render_window is not None: - import pygame - - pygame.display.quit() - pygame.quit() - - @property - def fps(self): - return self._GUI_camera._fps - - def callback(self, callback_name, num_timesteps): - """ Update a callback -- may be useful during training, e.g. for curriculum learning. """ - self.callbacks[callback_name].update(num_timesteps) - - def update_callbacks(self, num_timesteps): - """ Update all callbacks. """ - for callback_name in self.callbacks: - self.callback(callback_name, num_timesteps) - - # def get_logdict_keys(self): - # return list(self.task._info["log_dict"].keys()) - - # def get_logdict_value(self, key): - # return self.task._info["log_dict"].get(key) - - @property - def config(self): - """ Return config. """ - return copy.deepcopy(self._config) - - @property - def run_parameters(self): - """ Return run parameters. """ - # Context cannot be deep copied - exclude = {"rendering_context"} - run_params = {k: copy.deepcopy(self._run_parameters[k]) for k in self._run_parameters.keys() - exclude} - run_params["rendering_context"] = self._run_parameters["rendering_context"] - return run_params - - @property - def simulator_folder(self): - """ Return simulator folder. """ - return self._simulator_folder - - @property - def render_mode(self): - """ Return render mode. """ - return self._render_mode - - def get_state(self): - """ Return a state of the simulator / individual components (biomechanical model, perception model, task). - - This function is used for logging/evaluation purposes, not for RL training. - - Returns: - A dict with one float or numpy vector per keyword. - """ - - # Get time, qpos, qvel, qacc, act_force, act, ctrl of the current simulation - state = {"timestep": self._data.time, - "qpos": self._data.qpos.copy(), - "qvel": self._data.qvel.copy(), - "qacc": self._data.qacc.copy(), - "act_force": self._data.actuator_force.copy(), - "act": self._data.act.copy(), - "ctrl": self._data.ctrl.copy()} - - # Add state from the task - state.update(self.task.get_state(self._model, self._data)) - - # Add state from the biomechanical model - state.update(self.bm_model.get_state(self._model, self._data)) + if self.render_mode: + self._init_render() + + @classmethod + def get(cls, simulator_folder: str, **kwargs): + return cls(simulator_folder, **kwargs) + + def _load_config(self) -> 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] + } + } + with open(config_path, "w", encoding="utf-8") as f: + yaml.dump(default_config, f) + 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")) + if not os.path.exists(model_path): + with open(model_path, "w", encoding="utf-8") as f: + f.write(""" + """) + model = mujoco.MjModel.from_xml_path(model_path) + data = mujoco.MjData(model) + return model, data + + def _validate_config(self): + if "simulation" not in self.config: + self.config["simulation"] = {} + + # 如果有模型,使用模型信息,否则使用默认值 + if self.model is not None: + nq = self.model.nq + else: + nq = 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) + + def _init_action_space(self): + """初始化动作空间""" + # 确保模型已加载 + if self.model is None: + raise ValueError("模型未加载,无法初始化动作空间") + + n_actuators = self.model.nu + self.action_space = spaces.Box( + low=-1.0, + high=1.0, + shape=(n_actuators,), + dtype=np.float32 + ) + print(f"动作空间初始化: 维度={n_actuators}") + + def _init_observation_space(self): + """初始化观测空间""" + # 确保模型已加载 + 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 + ) + print(f"观测空间初始化: 维度={obs_dim} (位置={n_qpos}, 速度={n_qvel})") + + def _init_render(self): + """初始化渲染""" + if self.render_mode == "human": + try: + pygame.init() + self.screen = pygame.display.set_mode((800, 600)) + pygame.display.set_caption("MuJoCo Arm Simulation") + print("Pygame渲染初始化成功") + except Exception as e: + print(f"渲染初始化失败: {e}") + + def reset(self, seed: Optional[int] = None) -> Tuple[np.ndarray, dict]: + """重置仿真环境""" + if seed is not None: + np.random.seed(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() + + return obs, {} - # Add state from the perception model - state.update(self.perception.get_state(self._model, self._data)) + def step(self, action: np.ndarray) -> Tuple[np.ndarray, float, bool, bool, dict]: + """执行一步仿真""" + # 将动作应用到执行器 + self.data.ctrl[:] = action * 10.0 + + # 执行一步仿真 + 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() + + return obs, reward, self.terminated, self.truncated, {} - return state + def _get_obs(self) -> np.ndarray: + """获取当前观测""" + qpos = self.data.qpos.copy() + qvel = self.data.qvel.copy() + + obs = np.concatenate([qpos, qvel]) + return obs.astype(np.float32) - def close(self, **kwargs): - """ Perform any necessary clean up. + def _compute_reward(self) -> 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) + + pos_error = np.sum((current_pos - target_pos) ** 2) + reward = -pos_error + + return float(reward) - This function is inherited from gym.Env. It should be automatically called when this object is garbage collected - or the program exists, but that doesn't seem to be the case. This function will be called if this object has been - initialised in the context manager fashion (i.e. using the 'with' statement). """ - self.task.close(**kwargs) - self.perception.close(**kwargs) - self.bm_model.close(**kwargs) + def _check_terminated(self) -> bool: + """检查是否达到终止条件""" + joint_pos = self.data.qpos.copy() + + if np.any(np.abs(joint_pos) > np.pi): + return True + + return False + + def render(self): + """渲染当前仿真状态""" + if self.render_mode == "human" and self.screen is not None: + # 处理pygame事件 + for event in pygame.event.get(): + if event.type == pygame.QUIT: + self.close() + return + 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) + + # 绘制机械臂 + pygame.draw.line(self.screen, (0, 0, 0), + (center_x, center_y), (end_x, end_y), 5) + + # 绘制关节 + 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): + """关闭环境""" + if self.render_mode == "human": + pygame.quit() \ No newline at end of file diff --git a/src/box/simulators/2.py b/src/box/simulators/2.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/box/simulators/arm_simulation/arm_model.mjcf b/src/box/simulators/arm_simulation/arm_model.mjcf new file mode 100644 index 0000000000..dde587e969 --- /dev/null +++ b/src/box/simulators/arm_simulation/arm_model.mjcf @@ -0,0 +1,14 @@ + + \ No newline at end of file diff --git a/src/box/simulators/arm_simulation/arm_simulation.xml b/src/box/simulators/arm_simulation/arm_simulation.xml new file mode 100644 index 0000000000..dde587e969 --- /dev/null +++ b/src/box/simulators/arm_simulation/arm_simulation.xml @@ -0,0 +1,14 @@ + + \ No newline at end of file diff --git a/src/box/simulators/arm_simulation/config.yaml b/src/box/simulators/arm_simulation/config.yaml new file mode 100644 index 0000000000..49138a68d1 --- /dev/null +++ b/src/box/simulators/arm_simulation/config.yaml @@ -0,0 +1,4 @@ +# 最小化配置示例(具体字段需根据 simulator.py 中的要求调整) +simulation: + model_path: "arm_model.mjcf" # 模型文件名(若有) + render: true \ No newline at end of file diff --git a/src/box/skeleton_video.py b/src/box/skeleton_video.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/box/test.simulator.py b/src/box/test.simulator.py index 06f94e93fd..233864623b 100644 --- a/src/box/test.simulator.py +++ b/src/box/test.simulator.py @@ -1,25 +1,93 @@ -from uitb.simulator import Simulator - -# 加载仿真器(需替换为实际仿真器路径,如 "uitb/simulators/arm_simulation") -simulator_folder = "uitb/simulators/your_sim_folder" -env = Simulator.get( - simulator_folder=simulator_folder, - render_mode="human", # "human" 弹出Pygame窗口;"rgb_array" 输出图片数组 - render_mode_perception="embed" # 感知图像嵌入主窗口 -) - -# 重置环境 -obs, info = env.reset(seed=42) -print("初始观测值:", {k: v.shape for k in obs}) - -# 运行 1000 步仿真(随机动作示例) -for step in range(1000): - action = env.action_space.sample() - obs, reward, terminated, truncated, info = env.step(action) - if step % 100 == 0: - print(f"第 {step} 步 | 奖励:{reward:.2f} | 终止状态:{terminated}") - if terminated or truncated: - obs, info = env.reset() - -# 关闭仿真(释放资源) -env.close() +import sys +import os +import importlib.util +import numpy as np + +# 1. 手动指定 simulator.py 的绝对路径 +simulator_path = os.path.abspath(os.path.join(os.path.dirname(__file__), "simulator.py")) + +# 2. 检查文件是否存在 +if not os.path.exists(simulator_path): + raise FileNotFoundError(f"simulator.py 不存在:{simulator_path}") + +# 3. 动态加载 simulator.py 模块 +spec = importlib.util.spec_from_file_location("simulator", simulator_path) +simulator_module = importlib.util.module_from_spec(spec) +sys.modules["simulator"] = simulator_module +spec.loader.exec_module(simulator_module) + +# 4. 从加载的模块中导入 Simulator 类 +Simulator = simulator_module.Simulator + +# 配置仿真器路径(根据你的目录结构) +current_dir = os.path.dirname(os.path.abspath(__file__)) +simulator_folder = os.path.join(current_dir, "simulators", "arm_simulation") + +# 检查仿真器目录是否存在,如果不存在则创建 +if not os.path.exists(simulator_folder): + print(f"创建仿真器目录:{simulator_folder}") + os.makedirs(simulator_folder, exist_ok=True) + +print("=" * 50) +print("机械臂仿真环境") +print("=" * 50) +print(f"仿真器目录: {simulator_folder}") +print(f"当前工作目录: {os.getcwd()}") + +try: + # 创建仿真环境 + env = Simulator.get( + simulator_folder=simulator_folder, + render_mode="human" + ) + + print("仿真环境创建成功!") + print(f"模型关节数(nq): {env.model.nq}") + print(f"模型执行器数(nu): {env.model.nu}") + print(f"模型速度数(nv): {env.model.nv}") + print("=" * 50) + + # 重置环境 + obs, info = env.reset(seed=42) + print(f"初始观测值: {obs}") + print(f"观测值形状: {obs.shape}") + print(f"动作空间形状: {env.action_space.shape}") + print("=" * 50) + + print("\n开始仿真...") + print("按ESC键或关闭窗口可结束仿真") + print("-" * 50) + + # 运行仿真 + for step in range(1000): + # 随机采样动作 + action = env.action_space.sample() + + # 执行一步 + obs, reward, terminated, truncated, info = env.step(action) + + # 每50步打印一次信息 + if step % 50 == 0: + print(f"Step {step:4d} | 奖励: {reward:7.3f} | 终止: {terminated} | 截断: {truncated}") + + # 如果环境终止或截断,则重置 + if terminated or truncated: + print(f"\n环境终止/截断 (Step {step}),重置仿真...") + obs, info = env.reset() + + print("\n仿真完成!") + +except Exception as e: + print(f"\n发生错误: {type(e).__name__}: {e}") + import traceback + traceback.print_exc() + +finally: + # 关闭环境 + print("\n关闭仿真环境...") + try: + if 'env' in locals(): + env.close() + except: + pass + print("仿真结束") \ No newline at end of file diff --git a/src/box/utils/functions.py b/src/box/utils/functions.py new file mode 100644 index 0000000000..b30ef1a5e8 --- /dev/null +++ b/src/box/utils/functions.py @@ -0,0 +1,25 @@ +# src/box/utils/functions.py +import os +import yaml + +def output_path(): + """示例:返回输出路径""" + return os.path.abspath(os.path.join(os.path.dirname(__file__), "../../output")) + +def parent_path(path): + """示例:返回父目录路径""" + return os.path.dirname(os.path.abspath(path)) + +def is_suitable_package_name(name): + """示例:验证包名是否合法""" + return name.isidentifier() and name[0].islower() + +def parse_yaml(file_path): + """示例:解析YAML文件""" + with open(file_path, "r", encoding="utf-8") as f: + return yaml.safe_load(f) + +def write_yaml(data, file_path): + """示例:写入YAML文件""" + with open(file_path, "w", encoding="utf-8") as f: + yaml.safe_dump(data, f, default_flow_style=False) \ No newline at end of file diff --git a/src/box/utils/rendering.py b/src/box/utils/rendering.py new file mode 100644 index 0000000000..6c030ddf24 --- /dev/null +++ b/src/box/utils/rendering.py @@ -0,0 +1,26 @@ +# src/box/utils/rendering.py +import mujoco +import numpy as np + +class Context: + """渲染上下文类,用于管理渲染配置""" + def __init__(self, model, max_resolution): + self.model = model + self.max_resolution = max_resolution # 格式:[宽度, 高度] +# 必须确保此文件的路径和内容完全匹配 +class Context: + def __init__(self, model, max_resolution): + self.model = model + self.max_resolution = max_resolution + +class Camera: + def __init__(self, context, model, data, camera_id, dt): + self.context = context + self.model = model + self.data = data + self.camera_id = camera_id + self.dt = dt + self._fps = 1.0 / dt + + def render(self): + return (None, None) \ No newline at end of file