diff --git a/src/box/moblARMslndex.py b/src/box/moblARMslndex.py new file mode 100644 index 0000000000..950f646641 --- /dev/null +++ b/src/box/moblARMslndex.py @@ -0,0 +1,270 @@ +""" +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/perception/moblARMslndex.py b/src/box/perception/moblARMslndex.py new file mode 100644 index 0000000000..299bbb4d67 --- /dev/null +++ b/src/box/perception/moblARMslndex.py @@ -0,0 +1,372 @@ +""" +MoblArmsIndex 简化版本 - 只生成人体上肢模型运动视频 +完全修复版 +""" + +import numpy as np +import mujoco +import imageio +import os +import subprocess +import sys + +def install_ffmpeg(): + """安装必要的视频编码依赖""" + try: + # 尝试导入 imageio-ffmpeg + import imageio_ffmpeg + print("✓ imageio-ffmpeg 已安装") + return True + except ImportError: + print("正在安装 imageio-ffmpeg...") + try: + subprocess.check_call([sys.executable, "-m", "pip", "install", "imageio[ffmpeg]"]) + print("✓ imageio[ffmpeg] 安装成功") + return True + except Exception as e: + print(f"⚠ 安装失败: {e}") + print("将使用 GIF 格式替代 MP4") + return False + +# ================ 简化的 BaseBMModel 类 ================ +class BaseBMModel: + """简化的 BaseBMModel 基类""" + def __init__(self, model, data, **kwargs): + self.model = model + self.data = data + + def _update(self, model, data): + """更新方法 - 子类应重写""" + pass + +# ================ MoblArmsIndex 模型 ================ +class MoblArmsIndex(BaseBMModel): + """ + 基于 MoBL ARMS 模型的生物力学模型 + 只专注于生成上肢运动视频 + """ + + def __init__(self, model, data, **kwargs): + super().__init__(model, data, **kwargs) + print("✓ MoblArmsIndex 上肢模型初始化完成") + + def _update(self, model, data): + """更新模型状态 - 简化版本""" + # 这里可以添加任何必要的模型更新逻辑 + pass + +# ================ 主程序:只生成视频 ================ +def main(): + print("=" * 60) + print("人体上肢模型运动视频生成器") + print("=" * 60) + + # 检查并安装必要依赖 + has_ffmpeg = install_ffmpeg() + + # 1. 创建更真实的上肢XML模型(修复了所有问题) + xml_string = """ + + + """ + + # 2. 加载模型 + try: + model = mujoco.MjModel.from_xml_string(xml_string) + data = mujoco.MjData(model) + print("✓ MuJoCo 模型加载成功") + print(f" 关节数: {model.njnt}") + print(f" 几何体数: {model.ngeom}") + print(f" 执行器数: {model.nu}") + except Exception as e: + print(f"❌ 模型加载失败: {e}") + return + + # 3. 创建生物力学模型实例 + bm_model = MoblArmsIndex(model, data) + + # 4. 设置仿真参数 + fps = 30 + duration = 5 # 5秒仿真 + total_steps = int(duration / model.opt.timestep) + + print(f"\n仿真参数:") + print(f" 时长: {duration}秒") + print(f" 时间步长: {model.opt.timestep}") + print(f" 总步数: {total_steps}") + print(f" 视频帧率: {fps} FPS") + + # 5. 设置渲染器(使用较小的分辨率避免问题) + try: + # 创建渲染器(使用640x480避免离屏缓冲区问题) + renderer = mujoco.Renderer(model, height=480, width=640) + + # 设置相机视角 + camera = mujoco.MjvCamera() + camera.azimuth = 30 # 水平角度 + camera.elevation = -20 # 垂直角度 + camera.distance = 2.5 # 距离 + camera.lookat = np.array([0.2, 0, 1.2]) # 看向点 + + # 初始渲染 + renderer.update_scene(data, camera=camera) + print("✓ 渲染器创建成功") + except Exception as e: + print(f"❌ 渲染器创建失败: {e}") + return + + # 6. 运行仿真并捕获视频帧 + frames = [] + + print("\n开始仿真并录制视频...") + + for step in range(total_steps): + # 计算时间(用于控制运动) + t = step * model.opt.timestep + + # 协同运动控制 + # 肩部运动(缓慢的波浪) + shoulder_pitch = 0.3 * np.sin(2 * np.pi * 0.3 * t) + 0.1 + shoulder_roll = 0.2 * np.sin(2 * np.pi * 0.2 * t + 0.5) + + # 肘部运动 + elbow = 0.6 * np.sin(2 * np.pi * 0.4 * t) - 1.0 + + # 手腕运动 + wrist = 0.1 * np.sin(2 * np.pi * 0.6 * t) + + # 手指运动序列 + if t < 1: + # 初始:手指弯曲 + index_angle = -0.4 + thumb_angle = 0.1 + elif t < 3: + # 指向阶段 + phase = (t - 1) / 2 + index_angle = 0.1 * np.sin(2 * np.pi * 1.0 * t) - 0.1 + thumb_angle = 0.15 * np.sin(2 * np.pi * 0.8 * t) + else: + # 返回阶段 + phase = (t - 3) / 2 + index_angle = -0.4 * (1 - phase) + thumb_angle = 0.1 * (1 - phase) + + # 应用控制信号 + data.ctrl[0] = shoulder_pitch # 肩部俯仰 + data.ctrl[1] = shoulder_roll # 肩部滚动 + data.ctrl[2] = elbow # 肘部 + data.ctrl[3] = wrist # 手腕 + data.ctrl[4] = index_angle # 食指 + data.ctrl[5] = thumb_angle # 拇指 + + # 步进仿真 + mujoco.mj_step(model, data) + + # 定期捕获帧 + if step % 2 == 0: # 每2步捕获一帧,获得15fps(GIF比较流畅) + try: + # 动态调整相机视角 + if t < 2: + camera.azimuth = 30 + 15 * t + elif t < 4: + camera.elevation = -20 + 10 * np.sin(t * 0.5) + + # 更新场景并渲染 + renderer.update_scene(data, camera=camera) + frame = renderer.render() + frames.append(frame) + except: + # 如果失败,使用默认视角 + try: + renderer.update_scene(data) + frame = renderer.render() + frames.append(frame) + except: + pass + + # 显示进度 + if step % 100 == 0: + progress = (step / total_steps) * 100 + print(f"进度: {progress:.1f}%", end='\r') + + print(f"进度: 100.0% - 仿真完成!") + + # 7. 生成视频文件 + if frames: + try: + # 确定保存路径(桌面) + desktop = os.path.join(os.path.expanduser("~"), "Desktop") + if not os.path.exists(desktop): + desktop = os.path.dirname(os.path.abspath(__file__)) + + # 首先尝试生成MP4(如果有FFMPEG) + if has_ffmpeg: + try: + video_path = os.path.join(desktop, "human_arm_motion.mp4") + + # 保存MP4视频 + print(f"\n生成MP4视频中...") + imageio.mimsave( + video_path, + frames, + fps=15, # GIF帧率较低 + codec='libx264', + quality=8 + ) + print(f"✓ MP4视频已保存: {video_path}") + print(f" 文件大小: {os.path.getsize(video_path)/1024/1024:.1f} MB") + + except Exception as mp4_error: + print(f"⚠ MP4生成失败: {mp4_error}") + # 回退到GIF + has_ffmpeg = False + + # 如果没有FFMPEG或MP4生成失败,生成GIF + if not has_ffmpeg: + gif_path = os.path.join(desktop, "human_arm_motion.gif") + + print(f"\n生成GIF动画中...") + # 保存为GIF(质量稍低但兼容性好) + imageio.mimsave( + gif_path, + frames, + fps=15, # GIF帧率较低 + subrectangles=True # 优化GIF大小 + ) + print(f"✓ GIF动画已保存: {gif_path}") + print(f" 文件大小: {os.path.getsize(gif_path)/1024:.1f} KB") + + except Exception as e: + print(f"❌ 文件保存失败: {e}") + else: + print("⚠ 没有捕获到视频帧") + + print("\n" + "=" * 60) + print("✅ 任务完成!") + print("=" * 60) + + # 8. 打开文件所在目录(仅Windows) + if 'video_path' in locals() and os.path.exists(video_path): + print(f"📁 MP4视频文件: {video_path}") + try: + os.startfile(os.path.dirname(video_path)) + print("📂 已打开文件所在目录") + except: + pass + elif 'gif_path' in locals() and os.path.exists(gif_path): + print(f"📁 GIF动画文件: {gif_path}") + try: + os.startfile(os.path.dirname(gif_path)) + print("📂 已打开文件所在目录") + except: + pass + +# ================ 运行主程序 ================ +if __name__ == "__main__": + # 检查必要依赖 + try: + import mujoco + import numpy as np + import imageio + print("✓ 核心库已安装") + except ImportError as e: + print(f"❌ 缺少必要的库: {e}") + print("\n请安装以下依赖:") + print("pip install mujoco numpy imageio") + exit(1) + + try: + main() + except KeyboardInterrupt: + print("\n⚠ 用户中断") + except Exception as e: + print(f"\n❌ 运行出错: {type(e).__name__}: {e}") + import traceback + traceback.print_exc() + \ No newline at end of file diff --git a/src/box/trainer.py b/src/box/trainer.py new file mode 100644 index 0000000000..7170f873d6 --- /dev/null +++ b/src/box/trainer.py @@ -0,0 +1,121 @@ +import sys +import os, shutil +import logging +from datetime import datetime + +import wandb +from wandb.integration.sb3 import WandbCallback + +from uitb.simulator import Simulator +from uitb.utils.functions import output_path, timeout_input + +from stable_baselines3.common.save_util import load_from_zip_file + +import argparse + +if __name__=="__main__": + + parser = argparse.ArgumentParser(description='Train an agent.') + parser.add_argument('config_file_path', type=str, + help='path to the config file') + parser.add_argument('--checkpoint', type=str, default=None, + help='filename of a specific checkpoint to resume training at ' + '(default: None, start training from the scratch)') + parser.add_argument('--resume', action='store_true', help='resume at latest checkpoint') + parser.add_argument('--eval', type=int, default=None, const=400000, nargs='?', help='run and store evaluations at a specific frequency (every ``eval`` timestep)') + parser.add_argument('--eval_info_keywords', type=str, nargs='*', default=[], help='additional keys of ``info`` dict that should be logged at the end of each evaluation episode') + args = parser.parse_args() + + # Get config file path + config_file_path = args.config_file_path + + # Build the simulator + simulator_folder = Simulator.build(config_file_path) + + # Initialise + simulator = Simulator.get(simulator_folder) + + # Get the config + config = simulator.config + + # Get simulator name + name = config.get("simulator_name", None) + + # Get checkpoint dir + checkpoint_dir = os.path.join(simulator._simulator_folder, 'checkpoints') + + # Restore wandb run_id from checkpoint if available + checkpoint = args.checkpoint + resume_training = args.resume or (checkpoint is not None) + if resume_training: + if os.path.isdir(checkpoint_dir): + existing_checkpoints = [os.path.join(checkpoint_dir, f) for f in os.listdir(checkpoint_dir) if + os.path.isfile(os.path.join(checkpoint_dir, f))] + else: + raise FileNotFoundError(f"Checkpoint directory does not exist: {checkpoint_dir}\nTry to run without --checkpoint or --resume.") + + if checkpoint is not None: + checkpoint_path = os.path.join(checkpoint_dir, checkpoint) + if not os.path.isfile(checkpoint_path): + raise FileNotFoundError(f"Checkpoint does not exist: {checkpoint_path}") + else: + assert len(existing_checkpoints) > 0, f"There are no checkpoints found in checkpoint directory: {checkpoint_dir}\n" \ + f"Maybe existing checkpoints were moved to a backup directory, " \ + f"which can be renamed to 'checkpoints/' to resume training." + # # find largest checkpoint + # checkpoint_path = sorted(existing_checkpoints, key=lambda f: int(f.split("_steps")[0].split("_")[-1]))[-1] + # find latest checkpoint + checkpoint_path = sorted(existing_checkpoints, key=os.path.getctime)[-1] + try: + _data, _, _ = load_from_zip_file(checkpoint_path) + wandb_id = _data["policy_kwargs"]["wandb_id"] + print(f"Resume wandb run {wandb_id} starting at checkpoint {checkpoint_path}.") + except Exception: + logging.warning("Cannot reliably identify wandb run id. Will resume training, but with new wandb instance and with step counter reset to zero.") + wandb_id = None + else: + checkpoint_path = None + wandb_id = None + # Backup existing checkpoint directory + if os.path.isdir(checkpoint_dir): + existing_checkpoints = [os.path.join(checkpoint_dir, f) for f in os.listdir(checkpoint_dir) if + os.path.isfile(os.path.join(checkpoint_dir, f))] + if len(existing_checkpoints) > 0: + last_checkpoint_time = max([os.path.getctime(f) for f in existing_checkpoints]) + last_checkpoint_time = datetime.fromtimestamp(last_checkpoint_time).strftime('%Y%m%d_%H%M%S') + checkpoint_dir_backup = os.path.join(simulator._simulator_folder, f'checkpoints_{last_checkpoint_time}') + shutil.move(checkpoint_dir, checkpoint_dir_backup) + + if name is None: + # Ask user to name this run + name = timeout_input("Give a name for this run. Input empty string or wait for 30 seconds for a random name.", + timeout=30, default="") + config["simulator_name"] = name.replace("-", "_") + + # Get project name + project = config.get("project", "uitb") + + # Prepare evaluation by storing custom log tags + with_evaluation = args.eval is not None + eval_freq = args.eval + eval_info_keywords = tuple(args.eval_info_keywords) + + # Initialise wandb + if wandb_id is None: + wandb_id = wandb.util.generate_id() + run = wandb.init(id=wandb_id, resume="allow", project=project, name=name, config=config, sync_tensorboard=True, save_code=True, dir=output_path()) + + # Initialise RL model + rl_cls = simulator.get_class("rl", config["rl"]["algorithm"]) + rl_model = rl_cls(simulator, checkpoint_path=checkpoint_path, wandb_id=wandb_id) + + # Haven't figured out how to periodically save rl in wandb; this is currently done inside the rl_model class + # TODO this doesn't seem to work; do the files need to be in wandb.run.dir? + #wandb.save(os.path.join(model_folder, run.name, 'checkpoints', "model_*_steps.zip"), + # base_path=os.path.join(model_folder, run.name, 'checkpoints')) + + # Start the training + # rl_model.learn(WandbCallback(verbose=2)) + rl_model.learn(WandbCallback(verbose=2), + with_evaluation=with_evaluation, eval_freq=eval_freq, eval_info_keywords=eval_info_keywords) + run.finish()