diff --git a/wasm/package-lock.json b/wasm/package-lock.json index 3f858f82..f9ec0335 100644 --- a/wasm/package-lock.json +++ b/wasm/package-lock.json @@ -14,6 +14,7 @@ "fflate": "^0.8.3", "lucide-react": "^0.555.0", "monaco-editor": "^0.55.1", + "pyodide": "^0.29.4", "react": "^19.2.8", "react-dom": "^19.2.8", "zustand": "^5.0.15" @@ -1497,6 +1498,12 @@ "dev": true, "license": "MIT" }, + "node_modules/@types/emscripten": { + "version": "1.41.5", + "resolved": "https://registry.npmjs.org/@types/emscripten/-/emscripten-1.41.5.tgz", + "integrity": "sha512-cMQm7pxu6BxtHyqJ7mQZ2kXWV5SLmugybFdHCBbJ5eHzOo6VhBckEgAT3//rP5FwPHNPeEiq4SmQ5ucBwsOo4Q==", + "license": "MIT" + }, "node_modules/@types/esrecurse": { "version": "4.3.1", "resolved": "https://registry.npmjs.org/@types/esrecurse/-/esrecurse-4.3.1.tgz", @@ -4461,6 +4468,19 @@ "node": ">=6" } }, + "node_modules/pyodide": { + "version": "0.29.4", + "resolved": "https://registry.npmjs.org/pyodide/-/pyodide-0.29.4.tgz", + "integrity": "sha512-tCseTsqU3kSxZIjkue5zXxTMNEwrKZwOIIEQRBA/VzHxFN1hoCxe4w41phfCdHd9it9RcCNQb5K/Re0InqMgvA==", + "license": "MPL-2.0", + "dependencies": { + "@types/emscripten": "^1.41.4", + "ws": "^8.5.0" + }, + "engines": { + "node": ">=18.0.0" + } + }, "node_modules/queue-microtask": { "version": "1.2.3", "resolved": "https://registry.npmjs.org/queue-microtask/-/queue-microtask-1.2.3.tgz", @@ -5627,6 +5647,27 @@ "node": ">=8" } }, + "node_modules/ws": { + "version": "8.21.3", + "resolved": "https://registry.npmjs.org/ws/-/ws-8.21.3.tgz", + "integrity": "sha512-201TZ/kPWxoPr/OKWjquZR1SWKXcvxdH+e1xrx89b3YbmzLMFCLfnaG1HFIgWzJOEWZ7MvpK++odZufgYR50Rw==", + "license": "MIT", + "engines": { + "node": ">=10.0.0" + }, + "peerDependencies": { + "bufferutil": "^4.0.1", + "utf-8-validate": ">=5.0.2" + }, + "peerDependenciesMeta": { + "bufferutil": { + "optional": true + }, + "utf-8-validate": { + "optional": true + } + } + }, "node_modules/xml-name-validator": { "version": "5.0.0", "resolved": "https://registry.npmjs.org/xml-name-validator/-/xml-name-validator-5.0.0.tgz", diff --git a/wasm/package.json b/wasm/package.json index dd773403..6222b96a 100644 --- a/wasm/package.json +++ b/wasm/package.json @@ -61,6 +61,7 @@ "fflate": "^0.8.3", "lucide-react": "^0.555.0", "monaco-editor": "^0.55.1", + "pyodide": "^0.29.4", "react": "^19.2.8", "react-dom": "^19.2.8", "zustand": "^5.0.15" diff --git a/wasm/src/README.md b/wasm/src/README.md new file mode 100644 index 00000000..3ae44eb6 --- /dev/null +++ b/wasm/src/README.md @@ -0,0 +1,25 @@ +# Web 仿真控制脚本 + +该目录存放可由 MuJoCo Web 平台“控制 → Python 控制器”直接导入的可信本地脚本。 + +## Go2-W 平衡控制 + +文件:`go2_w_balance.py` + +使用条件: + +1. 可导入 `/home/cen/Embodied_Workspace/unitree_ros/robots/go2w_description/` 文件夹/ZIP,或 Unitree 官方 `/home/cen/Embodied_Workspace/unitree_mujoco/unitree_robots/go2w/` MJCF 工程; +2. 使用 URDF 时选择“转换为 MJCF”和“浮动基座”; +3. 使用 URDF 时勾选“为关节添加驱动器”和“添加传感器”,平台会在浮动基座注入6轴 IMU; +4. 模型加载后进入“控制 → Python 控制器”,导入本文件; +5. 点击“加载脚本”或直接导入后,点击“启用”,再播放仿真。 + +控制器兼容两套命名: + +- Body:官方 MJCF 的 `base_link` 或 URDF 转换模型的 `base` +- 腿关节:`FL/FR/RL/RR_{hip,thigh,calf}_joint` +- 轮关节:官方 `*_wheel_joint` 或 URDF 转换后的 `*_foot_joint` +- actuator:官方 `FL_hip`/`FL_wheel` 风格,或平台生成的 `_motor` +- 6轴 IMU:`imu_gyro`(三轴角速度)和 `imu_acc`(三轴加速度) + +脚本默认只做原地站立、基于 IMU 的 roll/pitch 调平和轮毂姿态反馈,不包含行走、转向或轨迹规划。不同接触参数、质量或初始姿态下,可在脚本顶部调整 `NOMINAL`、`KP_FINAL`、`KD`、`RAMP_SECONDS` 和 `MAX_WHEEL_TORQUE`。 diff --git a/wasm/src/go2_w_balance.py b/wasm/src/go2_w_balance.py new file mode 100644 index 00000000..ee3a6884 --- /dev/null +++ b/wasm/src/go2_w_balance.py @@ -0,0 +1,186 @@ +"""Unitree Go2-W 原地站立平衡控制器(纯 MuJoCo/Python 版本)。 + +兼容两种模型: + +* unitree_mujoco/unitree_robots/go2w/go2w.xml; +* Web 平台由 unitree_ros/go2w_description.urdf 转换出的浮动基座 MJCF。 + +控制策略参考 unitree_mujoco/example/python/stand_go2.py:站立关节角、kp=50、 +kd=3.5,并在浏览器中直接计算 motor torque。姿态由基座6轴 IMU 的角速度与 +重力加速度互补滤波获得,不包含 DDS、CRC、实机电机模式或 sim2real 逻辑。 +""" + +import math + +NAME = "Unitree Go2-W 原地平衡控制" +CONTROL_HZ = 200 + +LEG_NAMES = ("FL", "FR", "RL", "RR") + +# 来自 unitree_mujoco/example/python/stand_go2.py 的 stand_up_joint_pos。 +HIP_TARGET = {"FL": 0.00571868, "FR": -0.00571868, + "RL": 0.00571868, "RR": -0.00571868} +NOMINAL = {"thigh": 0.608813, "calf": -1.21763} +HIP_DOWN = {"FL": 0.0473455, "FR": -0.0473455, + "RL": 0.0473455, "RR": -0.0473455} +STAND_DOWN = {"thigh": 1.22187, "calf": -2.44375} + +# SDK 示例使用 kp=50、kd=3.5。力矩上限按 Go2-W URDF/MJCF 保守取值。 +KP_FINAL = 50.0 +KP_INITIAL = 20.0 +KD = 3.5 +EFFORT_LIMIT = {"hip": 23.7, "thigh": 23.7, "calf": 35.55} +RAMP_SECONDS = 1.2 +WHEEL_DAMPING = 0.12 +MAX_WHEEL_TORQUE = 6.0 + + +def _clamp(value, lower, upper): + return max(lower, min(upper, value)) + + +def _accel_attitude(acceleration): + """由 Body 局部系中的重力方向估计横滚角和俯仰角。""" + ax, ay, az = acceleration + return math.atan2(ay, az), math.atan2(-ax, math.sqrt(ay * ay + az * az)) + + +def _resolve(api, kind, names): + """依次尝试官方 MJCF 与 URDF 转换模型的命名。""" + resolver = getattr(api, kind) + last_error = None + for name in names: + try: + return resolver(name) + except Exception as error: + last_error = error + raise RuntimeError(f"无法解析 {kind},候选名称:{', '.join(names)}") from last_error + + +def init(api): + legs = [] + for prefix in LEG_NAMES: + joints = {} + actuators = {} + for part in ("hip", "thigh", "calf"): + joint_name = f"{prefix}_{part}_joint" + joints[part] = _resolve(api, "joint", (joint_name,)) + actuators[part] = _resolve( + api, "actuator", (f"{prefix}_{part}", f"{joint_name}_motor") + ) + + wheel_joint = _resolve( + api, "joint", (f"{prefix}_wheel_joint", f"{prefix}_foot_joint") + ) + wheel_actuator = _resolve( + api, + "actuator", + (f"{prefix}_wheel", f"{prefix}_wheel_joint_motor", f"{prefix}_foot_joint_motor"), + ) + legs.append({ + "name": prefix, + "joints": joints, + "actuators": actuators, + "wheel_joint": wheel_joint, + "wheel_actuator": wheel_actuator, + "side": 1.0 if prefix.endswith("L") else -1.0, + "fore": 1.0 if prefix.startswith("F") else -1.0, + }) + + return { + "base": _resolve(api, "body", ("base_link", "base")), + "imu_gyro": _resolve(api, "sensor", ("imu_gyro", "__platform_imu_gyro__")), + "imu_acc": _resolve(api, "sensor", ("imu_acc", "__platform_imu_acc__")), + "legs": legs, + "started": False, + "start_time": 0.0, + "estimated_roll": 0.0, + "estimated_pitch": 0.0, + "filtered_droll": 0.0, + "filtered_dpitch": 0.0, + "unstable_duration": 0.0, + } + + +def _initialize(ctx, state, acceleration): + state["started"] = True + state["start_time"] = ctx.time + state["estimated_roll"], state["estimated_pitch"] = _accel_attitude(acceleration) + + +def step(ctx, state): + gyro = ctx.sensor(state["imu_gyro"]) + acceleration = ctx.sensor(state["imu_acc"]) + base_position = ctx.body_position(state["base"]) + if not state["started"]: + _initialize(ctx, state, acceleration) + + dt = max(1.0e-4, ctx.dt) + rate_alpha = 0.25 + state["filtered_droll"] += rate_alpha * (gyro[0] - state["filtered_droll"]) + state["filtered_dpitch"] += rate_alpha * (gyro[1] - state["filtered_dpitch"]) + accel_roll, accel_pitch = _accel_attitude(acceleration) + fusion = 0.015 + state["estimated_roll"] = (1.0 - fusion) * ( + state["estimated_roll"] + state["filtered_droll"] * dt + ) + fusion * accel_roll + state["estimated_pitch"] = (1.0 - fusion) * ( + state["estimated_pitch"] + state["filtered_dpitch"] * dt + ) + fusion * accel_pitch + roll, pitch = state["estimated_roll"], state["estimated_pitch"] + + elapsed = max(0.0, ctx.time - state["start_time"]) + ramp = 1.0 if elapsed >= 3.0 else math.tanh(elapsed / RAMP_SECONDS) + kp = KP_INITIAL + (KP_FINAL - KP_INITIAL) * ramp + + # SDK 示例的核心:12 个腿关节平滑进入站立姿态并保持 PD 闭环。 + for leg in state["legs"]: + for part in ("hip", "thigh", "calf"): + up = HIP_TARGET[leg["name"]] if part == "hip" else NOMINAL[part] + down = HIP_DOWN[leg["name"]] if part == "hip" else STAND_DOWN[part] + desired = down + ramp * (up - down) + if part == "calf" and ramp > 0.8: + # 通过左右/前后轮腿长度差调平车身:高侧缩短,低侧伸长。 + roll_term = 0.34 * roll + 0.025 * state["filtered_droll"] + pitch_term = 0.30 * pitch + 0.022 * state["filtered_dpitch"] + desired -= leg["side"] * roll_term + desired += leg["fore"] * pitch_term + target = desired + position = ctx.qpos(leg["joints"][part]) + velocity = ctx.qvel(leg["joints"][part]) + torque = kp * (target - position) - KD * velocity + limit = EFFORT_LIMIT[part] + ctx.set_control(leg["actuators"][part], _clamp(torque, -limit, limit)) + + # Go2-W 四轮保持自由滚动,只施加温和阻尼以抑制无指令漂移。 + wheel_velocities = [ctx.qvel(leg["wheel_joint"]) for leg in state["legs"]] + average_velocity = sum(wheel_velocities) / len(wheel_velocities) + wheel_torque = 1.4 * pitch + 0.10 * state["filtered_dpitch"] + wheel_torque -= WHEEL_DAMPING * average_velocity + wheel_torque = _clamp(wheel_torque, -MAX_WHEEL_TORQUE, MAX_WHEEL_TORQUE) + for leg in state["legs"]: + ctx.set_control(leg["wheel_actuator"], wheel_torque) + + # 持续失稳时主动停止,避免倒地后控制器继续输出饱和力矩。 + unstable = ramp > 0.95 and ( + base_position[2] < 0.16 or abs(roll) > 1.0 or abs(pitch) > 1.0 + ) + state["unstable_duration"] = state["unstable_duration"] + dt if unstable else 0.0 + if state["unstable_duration"] > 0.35: + raise RuntimeError( + f"Go2-W 已失稳:z={base_position[2]:.3f} m, " + f"roll={roll:.3f} rad, pitch={pitch:.3f} rad;请重置后检查模型接触参数" + ) + + +def reset(state): + state["started"] = False + state["estimated_roll"] = 0.0 + state["estimated_pitch"] = 0.0 + state["filtered_droll"] = 0.0 + state["filtered_dpitch"] = 0.0 + state["unstable_duration"] = 0.0 + + +def dispose(state): + pass diff --git a/wasm/web_platform/README.md b/wasm/web_platform/README.md index d6b61ac9..90e76613 100644 --- a/wasm/web_platform/README.md +++ b/wasm/web_platform/README.md @@ -12,6 +12,7 @@ - Three.js primitive、mesh、材质/贴图显示与对象选择 - 播放、暂停、单步、重置、0.25×–4× 速度 - actuator 滑杆、hinge/slide 关节拖动、动态 body 外力拖拽 +- 导入单文件 `.py` 控制器,通过本地 Pyodide 在 `mj_step` 前按仿真时间同步执行 - FPS、物理耗时和主线程步进预算提示 ## 开发 @@ -53,6 +54,13 @@ python3 -m http.server 8080 --directory wasm/web-platform-dist - XML 的 `include`、mesh 和贴图路径必须相对于入口/编译器配置可解析。 - 路径穿越、绝对路径、加密 ZIP、重复路径会被拒绝。 - 默认限制:2000 个文件、单文件 128 MiB、总解压大小 512 MiB、ZIP 文件 128 MiB。 +- 文件夹或 ZIP 中的 `.py` 会显示在“控制 → Python 控制器”;也可以在加载模型后单独导入不超过 1 MiB 的 `.py`。 + +## Python 控制器 + +Python 控制器是可信的单文件脚本,必须同步定义 `step(ctx, state)`;可选定义 `NAME`、`CONTROL_HZ`(限制为 1–500 Hz)、`init(api)`、`reset(state)` 和 `dispose(state)`。`init` 可用 `api.joint(name)`、`api.actuator(name)`、`api.sensor(name)`、`api.body(name)` 预解析 ID;`step` 可用 `ctx.qpos(id)`、`ctx.qvel(id)`、`ctx.sensor(id)`、`ctx.body_quat(id)`、`ctx.body_position(id)` 读取状态,并用 `ctx.set_control(id, value)` 写入经过有限值检查和 actuator 限幅的控制量。异常会自动停止控制器、暂停仿真并清零 `ctrl`。 + +当前 Python 与 MuJoCo 都运行在主线程,以保证闭环调用严格位于 `mj_step` 前。仅运行可信脚本;死循环仍可能阻塞页面。Pyodide 及 Python 标准库由 npm 包随生产构建离线发布,不从 CDN 下载;暂不支持第三方 Python 包、`pip` 或多文件 import。 ## 示例 @@ -60,6 +68,7 @@ python3 -m http.server 8080 --directory wasm/web-platform-dist - `mjcf_include/`:MJCF include、OBJ/STL mesh 和 PNG texture; - `urdf_mesh/`:引用 OBJ 的 URDF; +- `python_controller/`:倒立摆模型及 `balance.py` PD 控制器; - `invalid.xml`:无效模型; - `missing-resource.xml`:缺失资源错误示例。 @@ -69,7 +78,7 @@ python3 -m http.server 8080 --directory wasm/web-platform-dist - 仅面向桌面版 Chrome、Edge、Firefox;未适配手机和平板。 - 物理运行在主线程、单线程 WASM。超出每帧预算时限制追帧并提示。 -- 不支持 Xacro、XML 在线编辑、热重载、导出、账号或云端保存。 +- 不支持 Xacro、账号或云端保存;Python 控制器暂不支持第三方包和不可信代码隔离。 - 关节拖动只支持 hinge/slide;ball/free joint 只读。 - MuJoCo WASM 本身不支持 DAE mesh。平台会移除 DAE visual,并以 collision 几何显示;DAE collision 会替换为半径 0.05 m 的占位球体并在界面警告。高精度仿真应先将 DAE 转为 OBJ/STL 或改为 URDF primitive。 - 导入工程只存在当前页面内存,刷新页面后需重新导入。 diff --git a/wasm/web_platform/e2e/app.spec.ts b/wasm/web_platform/e2e/app.spec.ts index 49bc1ab4..954796e8 100644 --- a/wasm/web_platform/e2e/app.spec.ts +++ b/wasm/web_platform/e2e/app.spec.ts @@ -19,6 +19,8 @@ const SIMPLE_MODEL = ` `; +const SLIDE_DIRECTION_MODEL=``; + const LARGE_MODEL = `