diff --git a/wasm/package-lock.json b/wasm/package-lock.json index 7edd2e56..f9ec0335 100644 --- a/wasm/package-lock.json +++ b/wasm/package-lock.json @@ -9,9 +9,12 @@ "version": "1.0.0-alpha.1", "license": "Apache-2.0", "dependencies": { + "@monaco-editor/react": "^4.7.0", "@mujoco/mujoco": "^3.11.0", "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" @@ -931,6 +934,29 @@ "@jridgewell/sourcemap-codec": "^1.4.10" } }, + "node_modules/@monaco-editor/loader": { + "version": "1.7.0", + "resolved": "https://registry.npmjs.org/@monaco-editor/loader/-/loader-1.7.0.tgz", + "integrity": "sha512-gIwR1HrJrrx+vfyOhYmCZ0/JcWqG5kbfG7+d3f/C1LXk2EvzAbHSg3MQ5lO2sMlo9izoAZ04shohfKLVT6crVA==", + "license": "MIT", + "dependencies": { + "state-local": "^1.0.6" + } + }, + "node_modules/@monaco-editor/react": { + "version": "4.7.0", + "resolved": "https://registry.npmjs.org/@monaco-editor/react/-/react-4.7.0.tgz", + "integrity": "sha512-cyzXQCtO47ydzxpQtCGSQGOC8Gk3ZUeBXFAxD+CWXYFo5OqZyZUonFl0DwUlTyAfRHntBfw2p3w4s9R6oe1eCA==", + "license": "MIT", + "dependencies": { + "@monaco-editor/loader": "^1.5.0" + }, + "peerDependencies": { + "monaco-editor": ">= 0.25.0 < 1", + "react": "^16.8.0 || ^17.0.0 || ^18.0.0 || ^19.0.0", + "react-dom": "^16.8.0 || ^17.0.0 || ^18.0.0 || ^19.0.0" + } + }, "node_modules/@mujoco/mujoco": { "version": "3.11.0", "resolved": "https://registry.npmjs.org/@mujoco/mujoco/-/mujoco-3.11.0.tgz", @@ -1472,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", @@ -1550,6 +1582,13 @@ "meshoptimizer": "~1.1.1" } }, + "node_modules/@types/trusted-types": { + "version": "2.0.7", + "resolved": "https://registry.npmjs.org/@types/trusted-types/-/trusted-types-2.0.7.tgz", + "integrity": "sha512-ScaPdn1dQczgbl0QFTeTOmVHFULt394XJgOQNoyVhZ6r2vLnMLJfBPd53SB52T/3G36VI1/g2MZaX0cwDuXsfw==", + "license": "MIT", + "optional": true + }, "node_modules/@types/webxr": { "version": "0.5.24", "resolved": "https://registry.npmjs.org/@types/webxr/-/webxr-0.5.24.tgz", @@ -2542,6 +2581,15 @@ "license": "MIT", "peer": true }, + "node_modules/dompurify": { + "version": "3.2.7", + "resolved": "https://registry.npmjs.org/dompurify/-/dompurify-3.2.7.tgz", + "integrity": "sha512-WhL/YuveyGXJaerVlMYGWhvQswa7myDG17P7Vu65EWC05o8vfeNbvNf4d/BOvH99+ZW+LlQsc1GDKMa1vNK6dw==", + "license": "(MPL-2.0 OR Apache-2.0)", + "optionalDependencies": { + "@types/trusted-types": "^2.0.7" + } + }, "node_modules/eastasianwidth": { "version": "0.2.0", "resolved": "https://registry.npmjs.org/eastasianwidth/-/eastasianwidth-0.2.0.tgz", @@ -3801,6 +3849,18 @@ "integrity": "sha512-s8UhlNe7vPKomQhC1qFelMokr/Sc3AgNbso3n74mVPA5LTZwkB9NlXf4XPamLxJE8h0gh73rM94xvwRT2CVInw==", "dev": true }, + "node_modules/marked": { + "version": "14.0.0", + "resolved": "https://registry.npmjs.org/marked/-/marked-14.0.0.tgz", + "integrity": "sha512-uIj4+faQ+MgHgwUW1l2PsPglZLOLOT1uErt06dAPtx2kjteLAkbsd/0FiYg/MGS+i7ZKLb7w2WClxHkzOOuryQ==", + "license": "MIT", + "bin": { + "marked": "bin/marked.js" + }, + "engines": { + "node": ">= 18" + } + }, "node_modules/mdn-data": { "version": "2.27.1", "resolved": "https://registry.npmjs.org/mdn-data/-/mdn-data-2.27.1.tgz", @@ -3887,6 +3947,16 @@ "node": ">=16 || 14 >=14.17" } }, + "node_modules/monaco-editor": { + "version": "0.55.1", + "resolved": "https://registry.npmjs.org/monaco-editor/-/monaco-editor-0.55.1.tgz", + "integrity": "sha512-jz4x+TJNFHwHtwuV9vA9rMujcZRb0CEilTEwG2rRSpe/A7Jdkuj8xPKttCgOh+v/lkHy7HsZ64oj+q3xoAFl9A==", + "license": "MIT", + "dependencies": { + "dompurify": "3.2.7", + "marked": "14.0.0" + } + }, "node_modules/ms": { "version": "2.1.3", "resolved": "https://registry.npmjs.org/ms/-/ms-2.1.3.tgz", @@ -4398,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", @@ -4682,6 +4765,12 @@ "dev": true, "license": "MIT" }, + "node_modules/state-local": { + "version": "1.0.7", + "resolved": "https://registry.npmjs.org/state-local/-/state-local-1.0.7.tgz", + "integrity": "sha512-HTEHMNieakEnoe33shBYcZ7NX83ACUjCu8c40iOGEZsngj9zRnkqS9j1pqQPXwobB0ZcVTk27REb7COQ0UR59w==", + "license": "MIT" + }, "node_modules/std-env": { "version": "4.2.0", "resolved": "https://registry.npmjs.org/std-env/-/std-env-4.2.0.tgz", @@ -5558,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 44f8981d..6222b96a 100644 --- a/wasm/package.json +++ b/wasm/package.json @@ -56,9 +56,12 @@ }, "type": "module", "dependencies": { + "@monaco-editor/react": "^4.7.0", "@mujoco/mujoco": "^3.11.0", "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..8f6038b7 --- /dev/null +++ b/wasm/src/README.md @@ -0,0 +1,36 @@ +# 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 调平基础上，实现平台 Python SDK 的标准基本移动指令：停止、前进、后退、左转、右转和原地起跳。加载并启用脚本后，可在“基本移动指令”面板直接操作。前后移动采用四轮速度闭环；转向采用前轮主导、后轮限幅的左右差速，减小后腿 hip 关节摆幅；起跳采用“下蹲—爆发伸腿—腾空收腿—落地准备—恢复”的一次性轨迹。 + +控制脚本可选定义同步函数 `command(name, state)` 接收指令。当前标准指令名为： + +- `stop` +- `forward` +- `backward` +- `turn_left` +- `turn_right` +- `jump` + +其中移动/转向指令会持续生效，直到收到另一条移动指令或 `stop`。`stop` 会在轮速降为零后继续执行带积分补偿的站立姿态恢复，使机身自动回正。`jump` 是一次性原地起跳指令，会先切换到 `stop`。控制器在下蹲阶段保持完整重心补偿，爆发阶段以小腿竖直推力和轮毂反作用力矩抑制后仰，并记录起跳点；落地接触后会沿机身前向轴自动收回水平漂移。控制器必须确认位置、机身高度、姿态和角速度重新稳定，才会执行下一次已排队的起跳，避免连续起跳逐步后仰。不同接触参数、质量或初始姿态下，可调整 `NOMINAL`、`KP_FINAL`、`KD`、`DRIVE_SPEED`、`DRIVE_ACCELERATION`、`TURN_RATE`、`TURN_ACCELERATION`、`WHEEL_VELOCITY_KP` 和 `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..a3208e9c --- /dev/null +++ b/wasm/src/go2_w_balance.py @@ -0,0 +1,481 @@ +"""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 +WHEEL_RADIUS = 0.065 +TRACK_HALF_WIDTH = 0.23 +DRIVE_SPEED = 0.35 +DRIVE_ACCELERATION = 0.25 +TURN_RATE = 1.4 +TURN_ACCELERATION = 0.7 +WHEEL_VELOCITY_KP = 0.45 +TURN_VELOCITY_KP = 0.90 +# 后轮仅承担部分差速转向，降低侧向轮胎力造成的后腿 hip 大幅摆动。 +REAR_TURN_SCALE = 0.40 +REAR_TURN_MAX_TORQUE = 3.0 +WHEEL_FEEDFORWARD = 0.25 +JUMP_CROUCH_SECONDS = 0.28 +JUMP_EXTEND_SECONDS = 0.08 +JUMP_TUCK_SECONDS = 0.18 +JUMP_LANDING_SECONDS = 0.22 +JUMP_RECOVER_SECONDS = 0.35 +# 轮毂反作用力矩用于抵消伸腿时的后仰角动量，不用于驱动水平位移。 +JUMP_PITCH_WHEEL_TORQUE = -6.0 + + +def _clamp(value, lower, upper): + return max(lower, min(upper, value)) + + +def _move_toward(value, target, max_delta): + return value + _clamp(target - value, -max_delta, max_delta) + + +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, + "motion": "stop", + "linear_speed": 0.0, + "yaw_rate": 0.0, + "jump_requested": False, + "jump_started": None, + "posture_recovering": False, + "stable_duration": 0.0, + "recovery_roll_integral": 0.0, + "recovery_pitch_integral": 0.0, + "last_base_position": None, + "base_velocity_x": 0.0, + "base_velocity_y": 0.0, + "jump_anchor": None, + "jump_forward": None, + } + + +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) + previous_position = state["last_base_position"] + if previous_position is not None: + velocity_alpha = 0.25 + state["base_velocity_x"] += velocity_alpha * ( + (base_position[0] - previous_position[0]) / dt + - state["base_velocity_x"] + ) + state["base_velocity_y"] += velocity_alpha * ( + (base_position[1] - previous_position[1]) / dt + - state["base_velocity_y"] + ) + state["last_base_position"] = list(base_position) + 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"] + + forward_error = 0.0 + forward_velocity = 0.0 + if state["jump_anchor"] is not None: + forward_x, forward_y = state["jump_forward"] + forward_error = ( + (base_position[0] - state["jump_anchor"][0]) * forward_x + + (base_position[1] - state["jump_anchor"][1]) * forward_y + ) + forward_velocity = ( + state["base_velocity_x"] * forward_x + + state["base_velocity_y"] * forward_y + ) + + if state["posture_recovering"]: + state["recovery_roll_integral"] = _clamp( + state["recovery_roll_integral"] + roll * dt, -0.6, 0.6 + ) + state["recovery_pitch_integral"] = _clamp( + state["recovery_pitch_integral"] + pitch * dt, -0.6, 0.6 + ) + posture_stable = ( + abs(roll) < 0.06 + and abs(pitch) < 0.06 + and abs(state["filtered_droll"]) < 0.15 + and abs(state["filtered_dpitch"]) < 0.15 + and abs(forward_error) < 0.015 + and abs(forward_velocity) < 0.05 + and base_position[2] > 0.38 + ) + state["stable_duration"] = ( + state["stable_duration"] + dt if posture_stable else 0.0 + ) + if state["stable_duration"] > 0.5: + # 保留积分得到的静态姿态补偿；清零会让机身再次回到带偏差的平衡点。 + state["posture_recovering"] = False + state["stable_duration"] = 0.0 + state["jump_anchor"] = None + state["jump_forward"] = None + + 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 + + motion = state["motion"] + target_linear_speed = ( + DRIVE_SPEED + if motion == "forward" + else -DRIVE_SPEED if motion == "backward" else 0.0 + ) + target_yaw_rate = ( + TURN_RATE + if motion == "turn_left" + else -TURN_RATE if motion == "turn_right" else 0.0 + ) + state["linear_speed"] = _move_toward( + state["linear_speed"], target_linear_speed, DRIVE_ACCELERATION * ctx.dt + ) + state["yaw_rate"] = _move_toward( + state["yaw_rate"], target_yaw_rate, TURN_ACCELERATION * ctx.dt + ) + + jump_offset_thigh = 0.0 + jump_offset_calf = 0.0 + jump_launching = False + jump_balance_scale = 1.6 + jump_centering = state["jump_anchor"] is not None and state["jump_started"] is None + ready_to_jump = ( + abs(state["linear_speed"]) < 0.05 and abs(state["yaw_rate"]) < 0.1 + ) + if ( + state["jump_requested"] + and ramp > 0.98 + and ready_to_jump + and not state["posture_recovering"] + and state["jump_started"] is None + ): + state["jump_started"] = ctx.time + state["jump_requested"] = False + quat = ctx.body_quat(state["base"]) + qw, qx, qy, qz = quat + forward_x = 1.0 - 2.0 * (qy * qy + qz * qz) + forward_y = 2.0 * (qx * qy + qw * qz) + forward_norm = max(1.0e-6, math.hypot(forward_x, forward_y)) + state["jump_anchor"] = [base_position[0], base_position[1]] + state["jump_forward"] = [ + forward_x / forward_norm, + forward_y / forward_norm, + ] + if state["jump_started"] is not None: + jump_elapsed = ctx.time - state["jump_started"] + launch_end = JUMP_CROUCH_SECONDS + JUMP_EXTEND_SECONDS + tuck_end = launch_end + JUMP_TUCK_SECONDS + landing_end = tuck_end + JUMP_LANDING_SECONDS + recover_end = landing_end + JUMP_RECOVER_SECONDS + if jump_elapsed < JUMP_CROUCH_SECONDS: + phase = jump_elapsed / JUMP_CROUCH_SECONDS + jump_offset_thigh = 0.34 * phase + jump_offset_calf = -0.64 * phase + elif jump_elapsed < launch_end: + # 以小腿为主提供竖直爆发力，避免大腿推力把机身向后推出。 + jump_offset_thigh = -0.22 + jump_offset_calf = 0.30 + jump_launching = True + jump_balance_scale = 0.28 + elif jump_elapsed < tuck_end: + # 离地后主动收腿，提高轮端离地高度并减小腿部转动惯量。 + jump_offset_thigh = 0.22 + jump_offset_calf = -0.40 + jump_balance_scale = 0.4 + elif jump_elapsed < landing_end: + # 触地前逐渐伸腿，避免保持收腿姿态直接撞击地面。 + phase = (jump_elapsed - tuck_end) / JUMP_LANDING_SECONDS + jump_offset_thigh = 0.22 * (1.0 - phase) - 0.05 * phase + jump_offset_calf = -0.40 * (1.0 - phase) + 0.08 * phase + jump_balance_scale = 0.4 + 0.8 * phase + elif jump_elapsed < recover_end: + jump_centering = True + phase = (jump_elapsed - landing_end) / JUMP_RECOVER_SECONDS + jump_offset_thigh = -0.05 * (1.0 - phase) + jump_offset_calf = 0.08 * (1.0 - phase) + jump_balance_scale = 1.2 + 0.4 * phase + else: + state["jump_started"] = None + state["posture_recovering"] = True + state["stable_duration"] = 0.0 + state["recovery_roll_integral"] = 0.0 + state["recovery_pitch_integral"] = 0.0 + + # SDK 示例的核心：12 个腿关节平滑进入站立姿态并保持 PD 闭环。 + motion_level = max( + abs(state["linear_speed"]) / DRIVE_SPEED, + abs(state["yaw_rate"]) / TURN_RATE, + ) + 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: + # 通过左右/前后轮腿长度差调平车身：高侧缩短，低侧伸长。 + if state["jump_started"] is not None: + balance_scale = jump_balance_scale + elif motion == "stop": + balance_scale = 1.6 + else: + balance_scale = 1.0 + roll_term = balance_scale * ( + 0.34 * roll + + 0.025 * state["filtered_droll"] + + 0.20 * state["recovery_roll_integral"] + ) + target_pitch = 0.0 if motion == "stop" else ( + 0.12 * state["linear_speed"] / DRIVE_SPEED + + 0.08 * abs(state["yaw_rate"]) / TURN_RATE + ) + pitch_error = pitch - target_pitch + pitch_term = balance_scale * ( + 0.50 * pitch_error + + 0.035 * state["filtered_dpitch"] + + 0.28 * state["recovery_pitch_integral"] + ) + desired -= leg["side"] * roll_term + desired += leg["fore"] * pitch_term + desired -= 0.18 * motion_level + desired += jump_offset_calf + elif part == "thigh": + desired += 0.10 * motion_level + jump_offset_thigh + target = desired + position = ctx.qpos(leg["joints"][part]) + velocity = ctx.qvel(leg["joints"][part]) + damping = 1.0 if jump_launching else KD + torque = kp * (target - position) - damping * velocity + limit = EFFORT_LIMIT[part] + if jump_launching and part == "calf": + torque = limit + elif jump_launching and part == "thigh": + # 爆发阶段由小腿提供主要竖直推力；大腿推力会引入向后冲量。 + torque = 0.0 + ctx.set_control(leg["actuators"][part], _clamp(torque, -limit, limit)) + + # 四轮速度闭环：斜坡限速避免瞬时驱动力矩使机身后仰。 + if motion == "stop": + balance_torque = 3.0 * pitch + 0.25 * state["filtered_dpitch"] + else: + balance_torque = 1.4 * pitch + 0.10 * state["filtered_dpitch"] + # 记录起跳点在机身前向轴上的位置，落地接触后用轮子收回水平漂移。 + centering_speed = 0.0 + if jump_centering and base_position[2] < 0.45: + centering_speed = _clamp( + -4.0 * forward_error - 0.8 * forward_velocity, + -0.30, + 0.30, + ) + for leg in state["legs"]: + target_velocity = (state["linear_speed"] + centering_speed) / WHEEL_RADIUS + turn_scale = 1.0 if leg["fore"] > 0 else REAR_TURN_SCALE + target_velocity -= ( + leg["side"] + * state["yaw_rate"] + * TRACK_HALF_WIDTH + * turn_scale + / WHEEL_RADIUS + ) + wheel_velocity = ctx.qvel(leg["wheel_joint"]) + velocity_kp = ( + TURN_VELOCITY_KP + if abs(state["yaw_rate"]) > 0.1 or abs(centering_speed) > 0.02 + else WHEEL_VELOCITY_KP + ) + wheel_torque = balance_torque + velocity_kp * ( + target_velocity - wheel_velocity + ) + if abs(target_velocity) > 0.1: + wheel_torque += math.copysign(WHEEL_FEEDFORWARD, target_velocity) + if motion == "stop": + wheel_torque -= WHEEL_DAMPING * wheel_velocity + if jump_launching: + wheel_torque = JUMP_PITCH_WHEEL_TORQUE + elif state["jump_started"] is not None and base_position[2] > 0.47: + # 腾空期间利用四个轮子的反作用角动量持续把机身俯仰拉回零。 + # 接近地面后恢复轮速闭环，避免姿态力矩转化为水平冲量。 + wheel_torque = ( + 12.0 * pitch + 1.5 * state["filtered_dpitch"] + ) + wheel_limit = ( + REAR_TURN_MAX_TORQUE + if abs(state["yaw_rate"]) > 0.1 and leg["fore"] < 0 + else MAX_WHEEL_TORQUE + ) + ctx.set_control( + leg["wheel_actuator"], + _clamp(wheel_torque, -wheel_limit, wheel_limit), + ) + + # 持续失稳时主动停止，避免倒地后控制器继续输出饱和力矩。 + 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.65: + raise RuntimeError( + f"Go2-W 已失稳：z={base_position[2]:.3f} m, " + f"roll={roll:.3f} rad, pitch={pitch:.3f} rad；请重置后检查模型接触参数" + ) + + +def command(name, state): + """接收 Web Python SDK 的标准基本移动指令。""" + if name in ("forward", "backward", "turn_left", "turn_right"): + state["motion"] = name + state["posture_recovering"] = False + state["stable_duration"] = 0.0 + state["recovery_roll_integral"] = 0.0 + state["recovery_pitch_integral"] = 0.0 + state["jump_anchor"] = None + state["jump_forward"] = None + return + if name == "stop": + state["motion"] = "stop" + state["jump_requested"] = False + state["posture_recovering"] = True + state["stable_duration"] = 0.0 + state["recovery_roll_integral"] = 0.0 + state["recovery_pitch_integral"] = 0.0 + return + if name == "jump": + state["motion"] = "stop" + if state["jump_started"] is None: + # 每次起跳都先重新确认重心稳定；移动中触发时不会直接带着惯性伸腿。 + state["jump_requested"] = True + state["posture_recovering"] = True + state["stable_duration"] = 0.0 + return + raise ValueError(f"不支持的移动指令：{name}") + + +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 + state["motion"] = "stop" + state["linear_speed"] = 0.0 + state["yaw_rate"] = 0.0 + state["jump_requested"] = False + state["jump_started"] = None + state["posture_recovering"] = False + state["stable_duration"] = 0.0 + state["recovery_roll_integral"] = 0.0 + state["recovery_pitch_integral"] = 0.0 + state["last_base_position"] = None + state["base_velocity_x"] = 0.0 + state["base_velocity_y"] = 0.0 + state["jump_anchor"] = None + state["jump_forward"] = None + + +def dispose(state): + pass diff --git a/wasm/web_platform/README.md b/wasm/web_platform/README.md index 077f3fc4..4e3935dd 100644 --- a/wasm/web_platform/README.md +++ b/wasm/web_platform/README.md @@ -6,11 +6,13 @@ - 导入单个/多个 MJCF(XML)、URDF 和关联资源 - 兼容常见 ROS URDF：规范化重复 material、解析 `package://` 工程内资源路径 +- URDF 导入时可选为 hinge/slide 关节生成 motor 驱动器，并可用 kp/kv 调整对应 MJCF 关节刚度与阻尼，并将可调位置/朝向的摄像头固连到指定机器人 Body - 导入保留相对路径的文件夹或 ZIP 工程 - 多模型入口选择、中文加载和编译错误 - Three.js primitive、mesh、材质/贴图显示与对象选择 - 播放、暂停、单步、重置、0.25×–4× 速度 - actuator 滑杆、hinge/slide 关节拖动、动态 body 外力拖拽 +- 导入单文件 `.py` 控制器，通过本地 Pyodide 在 `mj_step` 前按仿真时间同步执行 - FPS、物理耗时和主线程步进预算提示 ## 开发 @@ -52,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)`、`command(name, state)`、`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 限幅的控制量。定义 `command` 后，界面会显示停止、前进、后退、左转、右转和起跳按钮，并分别传入 `stop`、`forward`、`backward`、`turn_left`、`turn_right`、`jump`。所有回调都必须同步；异常会自动停止控制器或显示诊断，运行期异常还会暂停仿真并清零 `ctrl`。 + +当前 Python 与 MuJoCo 都运行在主线程，以保证闭环调用严格位于 `mj_step` 前。仅运行可信脚本；死循环仍可能阻塞页面。Pyodide 及 Python 标准库由 npm 包随生产构建离线发布，不从 CDN 下载；暂不支持第三方 Python 包、`pip` 或多文件 import。 ## 示例 @@ -59,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`：缺失资源错误示例。 @@ -68,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/V0.1_CAPABILITIES.md b/wasm/web_platform/V0.1_CAPABILITIES.md new file mode 100644 index 00000000..cfc87307 --- /dev/null +++ b/wasm/web_platform/V0.1_CAPABILITIES.md @@ -0,0 +1,166 @@ +# MuJoCo Web Platform V0.1 能力总结 + +## 1. 浏览器内模型导入 + +支持以下模型和资源格式： + +- MJCF/XML +- URDF +- ZIP 工程包 +- 文件夹工程 +- OBJ、STL、DAE Mesh +- PNG、JPG 等纹理资源 +- MJCF `include` 多文件工程 +- URDF `package://` 资源路径 + +导入工程仅保存在当前浏览器会话和 WASM 文件系统中，不会修改或删除本地文件。 + +## 2. 工程资源管理 + +- 按真实目录层级展示工程文件。 +- 目录优先并按文件名排序。 +- 标记和高亮当前模型入口。 +- 导入 URDF 时，只自动展开选中 URDF 所在的目录路径。 +- 其他目录默认折叠，仍可手动展开。 +- 支持从当前会话中移除整个工程。 + +## 3. URDF 兼容处理 + +提供两种加载方式： + +- **转换为 MJCF（推荐）** +- **MuJoCo 原生 URDF** + +自动兼容处理包括： + +- 将 `package://` 地址改写为工程相对路径。 +- 在浏览器内将 DAE 转换为 OBJ。 +- 保留 DAE 单位、坐标轴和节点变换。 +- 保留 URDF visual mesh 和 Body 层级。 +- 处理 MuJoCo 不支持的重复 material。 +- DAE 转换失败时提供降级几何。 +- 自动添加 `z=0` 物理地面。 +- 计算模型最低几何点并对齐到地面。 + +## 4. 固定与浮动基座 + +URDF 转 MJCF 时可以选择： + +- **浮动基座** + - 自动添加根 `freejoint`。 + - 支持重力、碰撞和外力运动。 +- **固定基座** + - 根 Body 与世界固连。 + +切换基座类型后会自动重新转换并加载模型。 + +## 5. Three.js 模型可视化 + +- 渲染 MuJoCo Body、Geom 和 Mesh。 +- 支持 Box、Sphere、Capsule、Cylinder、Ellipsoid、Plane 和 Mesh。 +- 正确解析 MuJoCo `mjvGeom.dataid` 的 Mesh/凸包编码。 +- 支持材质颜色、透明度和基础纹理。 +- 默认显示 visual 并隐藏 collision。 +- 可切换显示碰撞几何。 +- 自动计算相机中心和观察范围。 +- 无限地面不会影响相机包围范围。 +- 支持相机旋转、缩放、平移和复位。 + +## 6. XYZ 方向指示器 + +- 右下角提供固定的 XYZ 方向指示器。 +- X、Y、Z 分别使用红、绿、蓝色。 +- 指示器随相机方向实时旋转。 +- 不参与模型拾取和交互。 + +## 7. 模型结构树 + +左侧工程文件树下方显示 MuJoCo 模型结构： + +- 按 Body 父子关系组织。 +- 显示每个 Body 所属的 Joint。 +- 支持层级展开和折叠。 +- 鼠标悬停或键盘聚焦关节时： + - 高亮关节所属 Body。 + - 在关节锚点显示黄色标记。 + - 标记随仿真运动实时更新。 + +## 8. 仿真控制 + +支持： + +- 播放和暂停。 +- 单步运行。 +- 完整重置。 +- `0.25×`～`4×` 仿真速度。 +- 实时仿真时间和物理耗时。 +- 主线程步进预算及追帧限制。 +- 播放中重置后仍可正常再次播放和暂停。 + +快捷键： + +- `Space`：播放/暂停。 +- `R`：重置。 +- `1/2/3`：切换交互模式。 + +## 9. 关节控制 + +- 使用滑块调整旋转和滑动关节。 +- 只读显示 Free、Ball 等不适合单滑块编辑的关节。 +- 调整关节时自动暂停仿真。 +- 支持重置所有可编辑关节。 +- 重置时恢复初始位置并清零对应速度。 +- 支持忽略 MuJoCo 关节限位。 +- 恢复限位时自动将超限值钳制回合法范围。 +- 高级模式显示原始上下限和“已忽略”状态。 +- 旋转关节支持弧度制和角度制切换。 +- 滑动关节保持使用米制单位。 + +## 10. Actuator 与交互工具 + +- 显示模型 Actuator。 +- 根据 `ctrlrange` 提供控制滑块。 +- 提供三种视口交互模式： + - 物体选择。 + - 关节拖动。 + - 外力施加。 +- 外力强度可调。 +- 松开鼠标后自动清除外力。 +- 显示当前选中的 Body、Geom、类型和世界位置。 + +## 11. 界面与主题 + +- 中文用户界面。 +- 左右面板支持显示、隐藏和宽度调整。 +- 支持白天与黑夜主题切换。 +- 主题选择保存到浏览器本地。 +- UI、视口、网格和 XYZ 指示器同步换色。 +- 使用约 `420 ms` 的平滑主题切换动画。 +- 支持系统“减少动态效果”设置。 + +## 12. 状态与诊断 + +实时显示： + +- 仿真时间。 +- FPS。 +- 物理步进耗时。 +- 浏览器内存使用情况。 +- WASM 加载状态。 +- Body、Joint、Geom、Actuator、qpos 和 qvel 数量。 + +错误分类包括： + +- 导入失败。 +- ZIP 解析失败。 +- 模型编译失败。 +- 渲染失败。 + +URDF 自动兼容处理会以中文列出具体修改内容。 + +## 13. 当前限制 + +- DAE 转 OBJ 主要保留几何、单位和节点变换，内嵌材质及复杂贴图不能完全保留。 +- 仿真和渲染目前运行在浏览器主线程。 +- 关节滑块主要面向 Hinge 和 Slide，Free、Ball Joint 暂不提供完整的多自由度编辑器。 +- 导入工程不会持久化，刷新页面后需要重新导入。 diff --git a/wasm/web_platform/e2e/app.spec.ts b/wasm/web_platform/e2e/app.spec.ts index a38c7753..954796e8 100644 --- a/wasm/web_platform/e2e/app.spec.ts +++ b/wasm/web_platform/e2e/app.spec.ts @@ -1,5 +1,7 @@ import {expect, test} from '@playwright/test'; +import {readFileSync} from 'node:fs'; import {fileURLToPath} from 'node:url'; +import {zipSync} from 'fflate'; const fixture = (relative: string) => fileURLToPath(new URL(`../fixtures/${relative}`, import.meta.url)); @@ -17,6 +19,8 @@ const SIMPLE_MODEL = ` `; +const SLIDE_DIRECTION_MODEL=``; + const LARGE_MODEL = `