From c0081d08087dd6c739bcda2a474faf5896c11be7 Mon Sep 17 00:00:00 2001 From: cen617-code <1057290604@qq.com> Date: Mon, 24 Aug 2026 17:12:17 +0800 Subject: [PATCH] =?UTF-8?q?feat(web-platform):=20release=20V0.4.1=20?= =?UTF-8?q?=E5=AE=8C=E6=88=90=E5=9F=BA=E7=A1=80=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- wasm/src/README.md | 13 +- wasm/src/go2_w_balance.py | 317 +++++++++++++++++- wasm/web_platform/README.md | 2 +- wasm/web_platform/src/app/App.tsx | 5 +- .../components/PythonControllerPanel.test.tsx | 23 ++ .../app/components/PythonControllerPanel.tsx | 8 +- .../src/app/components/SidebarPanel.tsx | 6 +- .../src/controller/PythonControllerRuntime.ts | 28 +- wasm/web_platform/src/controller/types.ts | 4 + .../src/simulation/PhysicsAdapter.ts | 4 +- .../src/simulation/SimulationSession.ts | 4 +- 11 files changed, 387 insertions(+), 27 deletions(-) create mode 100644 wasm/web_platform/src/app/components/PythonControllerPanel.test.tsx diff --git a/wasm/src/README.md b/wasm/src/README.md index 3ae44eb6..8f6038b7 100644 --- a/wasm/src/README.md +++ b/wasm/src/README.md @@ -22,4 +22,15 @@ - actuator：官方 `FL_hip`/`FL_wheel` 风格，或平台生成的 `_motor` - 6轴 IMU：`imu_gyro`（三轴角速度）和 `imu_acc`（三轴加速度） -脚本默认只做原地站立、基于 IMU 的 roll/pitch 调平和轮毂姿态反馈，不包含行走、转向或轨迹规划。不同接触参数、质量或初始姿态下，可在脚本顶部调整 `NOMINAL`、`KP_FINAL`、`KD`、`RAMP_SECONDS` 和 `MAX_WHEEL_TORQUE`。 +脚本在原地站立和 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 index ee3a6884..a3208e9c 100644 --- a/wasm/src/go2_w_balance.py +++ b/wasm/src/go2_w_balance.py @@ -33,12 +33,35 @@ 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 @@ -99,6 +122,20 @@ def init(api): "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, } @@ -116,6 +153,18 @@ def step(ctx, state): _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"]) @@ -129,11 +178,139 @@ def step(ctx, state): ) + 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] @@ -141,38 +318,142 @@ def step(ctx, state): 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"] + 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]) - torque = kp * (target - position) - KD * velocity + 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)) - # 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) + # 四轮速度闭环：斜坡限速避免瞬时驱动力矩使机身后仰。 + 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"]: - ctx.set_control(leg["wheel_actuator"], wheel_torque) + 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.35: + 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 @@ -180,6 +461,20 @@ def reset(state): 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): diff --git a/wasm/web_platform/README.md b/wasm/web_platform/README.md index 90e76613..4e3935dd 100644 --- a/wasm/web_platform/README.md +++ b/wasm/web_platform/README.md @@ -58,7 +58,7 @@ python3 -m http.server 8080 --directory wasm/web-platform-dist ## 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 控制器是可信的单文件脚本，必须同步定义 `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。 diff --git a/wasm/web_platform/src/app/App.tsx b/wasm/web_platform/src/app/App.tsx index 3961c055..653ccb32 100644 --- a/wasm/web_platform/src/app/App.tsx +++ b/wasm/web_platform/src/app/App.tsx @@ -6,7 +6,7 @@ import type {ProjectManifest} from '../project/types'; import {filesFromDrop,importBrowserFiles,normalizeProjectPath,ProjectImportError} from '../project/importer'; import {MainThreadPhysicsAdapter,type UrdfBaseMode,type UrdfEnhancementOptions,type UrdfLoadMode} from '../simulation/PhysicsAdapter'; import type {ActuatorParameters} from '../simulation/SimulationSession'; -import type {ControllerStatus} from '../controller/types'; +import type {ControllerCommand,ControllerStatus} from '../controller/types'; import {MuJoCoViewer,type InteractionMode,type ViewerTheme} from '../viewer/MuJoCoViewer'; import {useAppStore,type AppDiagnostic} from '../stores/useAppStore'; import {WorkbenchHeader} from './components/WorkbenchHeader'; @@ -73,6 +73,7 @@ export function App(){ const loadControllerPath=(path:string)=>{const file=manifest.current?.files.find(candidate=>candidate.path===path);if(!file){state.setDiagnostic(diagnostic('仿真',new Error('工程中找不到控制脚本'),path));return;}setSelectedControllerPath(path);void loadControllerSource(new TextDecoder().decode(file.data),path);}; const importController=(file:File)=>{void (async()=>{try{if(!/\.py$/i.test(file.name))throw new Error('请选择 .py 文件');if(file.size>1024*1024)throw new Error('Python 控制脚本不能超过 1 MiB');const path=normalizeProjectPath(file.name),data=new Uint8Array(await file.arrayBuffer());if(manifest.current){const index=manifest.current.files.findIndex(candidate=>candidate.path===path),files=manifest.current.files.slice(),entry={path,data,size:data.byteLength,source:'file' as const,mimeType:file.type||'text/x-python'};if(index>=0)files[index]=entry;else files.push(entry);manifest.current={...manifest.current,files,totalBytes:files.reduce((total,item)=>total+item.size,0)};state.setProject(manifest.current.name,files.map(({path:filePath,size})=>({path:filePath,size})),manifest.current.entries,manifest.current.selectedEntry);state.setSnapshot(adapter.current.snapshot()??undefined);}setSelectedControllerPath(path);await loadControllerSource(new TextDecoder().decode(data),path);}catch(error){state.setDiagnostic(diagnostic('仿真',error,file.name));}})();}; const toggleController=(enabled:boolean)=>{adapter.current.setControllerEnabled(enabled);const snapshot=adapter.current.snapshot()??undefined;setControllerStatus(snapshot?.controller);state.setSnapshot(snapshot);}; + const sendControllerCommand=(command:ControllerCommand)=>{try{adapter.current.sendControllerCommand(command);const snapshot=adapter.current.snapshot()??undefined;setControllerStatus(snapshot?.controller);state.setSnapshot(snapshot);}catch(error){state.setDiagnostic(diagnostic('仿真',error,selectedControllerPath));}}; const removeController=()=>{adapter.current.removeController();setControllerStatus(undefined);state.setSnapshot(adapter.current.snapshot()??undefined);}; const notify=(title:string,detail:string,tone:WorkbenchNotification['tone']='success')=>{const notice:WorkbenchNotification={id:++notificationId.current,title,detail,tone,at:Date.now()};setNotifications(items=>[notice,...items].slice(0,20));setToast(notice);}; const saveCachedSource=async(path:string,text:string)=>{if(!manifest.current)return;manifest.current=upsertCachedMjcf(manifest.current,path,text);state.setProject(manifest.current.name,manifest.current.files.map(file=>({path:file.path,size:file.size})),manifest.current.entries,path);notify('转换后的 MJCF 已保存到缓存',path);await loadEntry(path);}; @@ -100,7 +101,7 @@ export function App(){ ]; return
event.preventDefault()} onDrop={drop}> setSourceOpen(true)} onTogglePause={togglePause} onStep={singleStep} onReset={reset} onSpeed={changeSpeed} onToggleLeft={()=>setLeftOpen(value=>!value)} onToggleRight={()=>setRightOpen(value=>!value)} onToggleTheme={()=>setTheme(value=>value==='dark'?'light':'dark')} onHelp={()=>setHelpOpen(true)} endActions={<>setNotifications(items=>items.filter(item=>item.id!==id))} onClear={()=>setNotifications([])} onOpenLog={()=>setDiagnosticsOpen(true)}/>setLayoutOpen(true)}>setSettingsOpen(true)}>} compactMenu={setCommandOpen(true)} onLayout={()=>setLayoutOpen(true)} onSettings={()=>setSettingsOpen(true)} onFullscreen={toggleFullscreen} onHelp={()=>setHelpOpen(true)} onTheme={()=>setTheme(value=>value==='dark'?'light':'dark')}/>} onCommands={()=>setCommandOpen(true)} onToggleFullscreen={toggleFullscreen} center={viewer.current?.resetCamera()}/>}/> -
viewer.current?.highlightJoint(jointId)}/>
setToast(undefined)}/>{Boolean(state.snapshot?.model.ncam)&&(showSensorCamera?
摄像头
:)}{state.entries.length>1&&!state.selectedEntry&&!pendingUrdfPath&&} {state.diagnostic&&state.setDiagnostic(undefined)} onRetry={state.diagnostic.category==='模型编译'&&state.diagnostic.path?()=>void loadEntry(state.diagnostic!.path!):undefined} onOpenProject={()=>{setLeftOpen(true);state.setDiagnostic(undefined);}}/>}
/\.py$/i.test(file.path)).map(file=>file.path)} selectedControllerPath={selectedControllerPath} controllerStatus={controllerStatus} onUrdfMode={changeUrdfMode} onBaseMode={changeBaseMode} onShowCollision={setShowCollision} onResetJoints={resetJoints} onToggleJointLimits={toggleJointLimits} onToggleAdvanced={()=>setJointAdvanced(value=>!value)} onToggleAngleUnit={()=>setAngleUnit(value=>value==='rad'?'deg':'rad')} onActuator={setActuator} onActuatorParameters={setActuatorParameters} onJoint={setJoint} onForceScale={setForceScale} onSelectControllerPath={setSelectedControllerPath} onLoadControllerPath={loadControllerPath} onImportController={importController} onToggleController={toggleController} onRemoveController={removeController}/>
+
viewer.current?.highlightJoint(jointId)}/>
setToast(undefined)}/>{Boolean(state.snapshot?.model.ncam)&&(showSensorCamera?
摄像头
:)}{state.entries.length>1&&!state.selectedEntry&&!pendingUrdfPath&&} {state.diagnostic&&state.setDiagnostic(undefined)} onRetry={state.diagnostic.category==='模型编译'&&state.diagnostic.path?()=>void loadEntry(state.diagnostic!.path!):undefined} onOpenProject={()=>{setLeftOpen(true);state.setDiagnostic(undefined);}}/>}
/\.py$/i.test(file.path)).map(file=>file.path)} selectedControllerPath={selectedControllerPath} controllerStatus={controllerStatus} onUrdfMode={changeUrdfMode} onBaseMode={changeBaseMode} onShowCollision={setShowCollision} onResetJoints={resetJoints} onToggleJointLimits={toggleJointLimits} onToggleAdvanced={()=>setJointAdvanced(value=>!value)} onToggleAngleUnit={()=>setAngleUnit(value=>value==='rad'?'deg':'rad')} onActuator={setActuator} onActuatorParameters={setActuatorParameters} onJoint={setJoint} onForceScale={setForceScale} onSelectControllerPath={setSelectedControllerPath} onLoadControllerPath={loadControllerPath} onImportController={importController} onToggleController={toggleController} onControllerCommand={sendControllerCommand} onRemoveController={removeController}/>
{pendingUrdfPath&&}{sourceOpen&&generatedMjcf&&generatedMjcfPath&&setSourceOpen(false)} onSave={saveCachedSource}/>}setHelpOpen(false)}/>setDiagnosticsOpen(false)} onClear={()=>setNotifications([])}/>setSettingsOpen(false)} theme={theme} angleUnit={angleUnit} showCollision={showCollision} jointAdvanced={jointAdvanced} forceScale={forceScale} onTheme={setTheme} onAngleUnit={setAngleUnit} onShowCollision={setShowCollision} onJointAdvanced={setJointAdvanced} onForceScale={setForceScale}/>setLayoutOpen(false)} leftOpen={leftOpen} rightOpen={rightOpen} onLeftOpen={setLeftOpen} onRightOpen={setRightOpen} onPreset={applyLayoutPreset} onReset={()=>applyLayoutPreset('default')}/>setCommandOpen(false)} commands={commands}/>setRemoveConfirmOpen(false)}>

确定从当前会话中移除“{state.projectName}”吗？

该操作不会删除本地文件。
; } diff --git a/wasm/web_platform/src/app/components/PythonControllerPanel.test.tsx b/wasm/web_platform/src/app/components/PythonControllerPanel.test.tsx new file mode 100644 index 00000000..dd66fd55 --- /dev/null +++ b/wasm/web_platform/src/app/components/PythonControllerPanel.test.tsx @@ -0,0 +1,23 @@ +import {fireEvent,render,screen} from '@testing-library/react'; +import {describe,expect,it,vi} from 'vitest'; +import {PythonControllerPanel} from './PythonControllerPanel'; + +const noop=()=>{}; + +describe('PythonControllerPanel',()=>{ + it('向支持 command 的已启用控制器发送基本移动指令',()=>{ + const onCommand=vi.fn(); + render(); + fireEvent.click(screen.getByRole('button',{name:'前进'})); + fireEvent.click(screen.getByRole('button',{name:'左转'})); + fireEvent.click(screen.getByRole('button',{name:'起跳'})); + expect(onCommand.mock.calls).toEqual([['forward'],['turn_left'],['jump']]); + expect(screen.getByRole('button',{name:'移动停止'})).toHaveAttribute('aria-pressed','true'); + }); + + it('控制器未启用时禁用基本移动按钮',()=>{ + render(); + expect(screen.getByRole('button',{name:'前进'})).toBeDisabled(); + expect(screen.getByRole('button',{name:'起跳'})).toBeDisabled(); + }); +}); diff --git a/wasm/web_platform/src/app/components/PythonControllerPanel.tsx b/wasm/web_platform/src/app/components/PythonControllerPanel.tsx index fad7f72c..04571f79 100644 --- a/wasm/web_platform/src/app/components/PythonControllerPanel.tsx +++ b/wasm/web_platform/src/app/components/PythonControllerPanel.tsx @@ -1,6 +1,6 @@ import {useRef,type ChangeEvent} from 'react'; -import {FileUp,Power,RotateCw,Trash2} from 'lucide-react'; -import type {ControllerStatus} from '../../controller/types'; +import {ArrowDown,ArrowLeft,ArrowRight,ArrowUp,FileUp,Octagon,Power,RotateCw,Trash2} from 'lucide-react'; +import type {ControllerCommand,ControllerStatus} from '../../controller/types'; import {Badge,Button,PropertyRow,Select} from '../../components/ui'; export interface PythonControllerPanelProps { @@ -12,10 +12,11 @@ export interface PythonControllerPanelProps { onLoadPath(path:string):void; onImport(file:File):void; onToggle(enabled:boolean):void; + onCommand(command:ControllerCommand):void; onRemove():void; } -export function PythonControllerPanel({paths,selectedPath,status,loading,onSelectPath,onLoadPath,onImport,onToggle,onRemove}:PythonControllerPanelProps){ +export function PythonControllerPanel({paths,selectedPath,status,loading,onSelectPath,onLoadPath,onImport,onToggle,onCommand,onRemove}:PythonControllerPanelProps){ const input=useRef(null); const importFile=(event:ChangeEvent)=>{const file=event.target.files?.[0];if(file)onImport(file);event.target.value='';}; return
@@ -29,6 +30,7 @@ export function PythonControllerPanel({paths,selectedPath,status,loading,onSelec
{status.name}{status.enabled?'运行中':'已停止'}
{status.error&&

{status.error}

} + {status.acceptsCommands&&

基本移动指令

}
:

加载可信的单文件 Python 控制器。脚本在每次 mj_step 前按仿真时间同步执行，默认 100 Hz。

} ; diff --git a/wasm/web_platform/src/app/components/SidebarPanel.tsx b/wasm/web_platform/src/app/components/SidebarPanel.tsx index 7d4fef37..1cc74aa0 100644 --- a/wasm/web_platform/src/app/components/SidebarPanel.tsx +++ b/wasm/web_platform/src/app/components/SidebarPanel.tsx @@ -6,7 +6,7 @@ import {countModelStructureSearchResults,ModelStructureTree} from '../../project import type {ActuatorInfo,ActuatorParameters,SimulationSnapshot} from '../../simulation/SimulationSession'; import type {UrdfBaseMode,UrdfLoadMode} from '../../simulation/PhysicsAdapter'; import type {ViewerSelection} from '../../viewer/MuJoCoViewer'; -import type {ControllerStatus} from '../../controller/types'; +import type {ControllerCommand,ControllerStatus} from '../../controller/types'; import {Badge,Button,CollapsibleSection,CopyButton,PropertyRow,ResizablePanel,Select,Tabs} from '../../components/ui'; import {TreeSearchField} from './TreeSearchField'; import {ProjectBreadcrumb} from './ProjectBreadcrumb'; @@ -23,13 +23,13 @@ interface ModelControlsProps{ onUrdfMode:(value:UrdfLoadMode)=>void;onBaseMode:(value:UrdfBaseMode)=>void;onShowCollision:(value:boolean)=>void; onResetJoints:()=>void;onToggleJointLimits:()=>void;onToggleAdvanced:()=>void;onToggleAngleUnit:()=>void; onActuator:(id:number,value:number)=>void;onActuatorParameters:(id:number,parameters:ActuatorParameters)=>void;onJoint:(id:number,value:number)=>void;onForceScale:(value:number)=>void; - onSelectControllerPath:(path:string)=>void;onLoadControllerPath:(path:string)=>void;onImportController:(file:File)=>void;onToggleController:(enabled:boolean)=>void;onRemoveController:()=>void; + onSelectControllerPath:(path:string)=>void;onLoadControllerPath:(path:string)=>void;onImportController:(file:File)=>void;onToggleController:(enabled:boolean)=>void;onControllerCommand:(command:ControllerCommand)=>void;onRemoveController:()=>void; } export function ModelControlsSidebar(props:ModelControlsProps){const [tab,setTab]=useState<'properties'|'controls'>('properties'),s=props.snapshot;if(!s)return
导入模型后显示属性
; const properties=<>{s.model.nbody} Body}>
 {props.selectedFormat==='urdf'&&

MJCF 模式保留 visual mesh、添加物理地面，并将模型最低点对齐到 z=0。

} {props.selection?
}/>}/>value.toFixed(3)).join(', ')} action={}/>
:

在视口中单击物体

}; - const controls=<>{s.controller.enabled?'运行':'停止'}:undefined}>{s.actuators.length}}>{s.actuators.length?s.actuators.map(actuator=>props.onActuator(actuator.id,value)} onParameters={parameters=>props.onActuatorParameters(actuator.id,parameters)}/>):

模型没有驱动器

} + const controls=<>{s.controller.enabled?'运行':'停止'}:undefined}>{s.actuators.length}}>{s.actuators.length?s.actuators.map(actuator=>props.onActuator(actuator.id,value)} onParameters={parameters=>props.onActuatorParameters(actuator.id,parameters)}/>):

模型没有驱动器

} {s.joints.length}}>
{s.joints.map(joint=>{const scale=joint.type===3&&props.angleUnit==='deg'?180/Math.PI:1,unit=joint.type===3?(props.angleUnit==='deg'?'°':' rad'):joint.type===2?' m':'';return props.onJoint(joint.id,value/scale)}/>;})}

选择“外力施加”，在动态物体上按住拖动，松开即清零。

; return ,content:properties},{value:'controls',label:'控制',icon:,content:controls}]}/>; diff --git a/wasm/web_platform/src/controller/PythonControllerRuntime.ts b/wasm/web_platform/src/controller/PythonControllerRuntime.ts index c5deac43..c4e39ac2 100644 --- a/wasm/web_platform/src/controller/PythonControllerRuntime.ts +++ b/wasm/web_platform/src/controller/PythonControllerRuntime.ts @@ -1,6 +1,6 @@ import type {PyodideInterface} from 'pyodide'; import type {PyCallable,PyDict} from 'pyodide/ffi'; -import type {ControllerBindings,ControllerStatus} from './types'; +import type {ControllerBindings,ControllerCommand,ControllerStatus} from './types'; const DEFAULT_CONTROL_HZ=100; const MIN_CONTROL_HZ=1; @@ -33,13 +33,14 @@ export class PythonControllerRuntime { private initFunction?:PyCallable; private stepFunction?:PyCallable; private resetFunction?:PyCallable; + private commandFunction?:PyCallable; private disposeFunction?:PyCallable; private state?:unknown; private nextControlTime=0; private statusValue:ControllerStatus; private constructor(private readonly bindings:ControllerBindings,path:string,name:string,controlHz:number){ - this.statusValue={language:'python',path,name,controlHz,loaded:true,enabled:false,lastStepMs:0}; + this.statusValue={language:'python',path,name,controlHz,loaded:true,enabled:false,acceptsCommands:false,lastStepMs:0}; } static async load(source:string,path:string,bindings:ControllerBindings):Promise{ @@ -57,6 +58,8 @@ export class PythonControllerRuntime { runtime.initFunction=globals.has('init')?globals.get('init') as PyCallable:undefined; runtime.stepFunction=globals.get('step') as PyCallable; runtime.resetFunction=globals.has('reset')?globals.get('reset') as PyCallable:undefined; + runtime.commandFunction=globals.has('command')?globals.get('command') as PyCallable:undefined; + runtime.statusValue.acceptsCommands=Boolean(runtime.commandFunction); runtime.disposeFunction=globals.has('dispose')?globals.get('dispose') as PyCallable:undefined; runtime.state=runtime.initFunction?.(bindings.model); if(runtime.state instanceof Promise)throw new Error('控制器函数必须同步执行'); @@ -74,6 +77,22 @@ export class PythonControllerRuntime { this.statusValue.enabled=enabled; this.statusValue.error=undefined; this.nextControlTime=currentTime; + if(!enabled)this.statusValue.activeCommand=undefined; + } + + command(command:ControllerCommand):void { + if(!this.statusValue.enabled)throw new Error('请先启用 Python 控制器'); + if(!this.commandFunction)throw new Error('当前 Python 控制器未定义 command(name, state)'); + try{ + const result=this.commandFunction(command,this.state); + if(result instanceof Promise)throw new Error('command() 必须是同步函数'); + destroyProxy(result); + this.statusValue.activeCommand=command==='jump'?'stop':command; + this.statusValue.error=undefined; + }catch(error){ + this.statusValue.error=errorMessage(error); + throw new Error(`Python 控制指令失败：${this.statusValue.error}`,{cause:error}); + } } stepIfDue(time:number):void { @@ -96,6 +115,7 @@ export class PythonControllerRuntime { reset(currentTime:number):void { this.nextControlTime=currentTime; + this.statusValue.activeCommand=undefined; if(!this.resetFunction)return; try{const result=this.resetFunction(this.state);destroyProxy(result);} catch(error){this.statusValue.enabled=false;this.statusValue.error=errorMessage(error);throw error;} @@ -108,8 +128,8 @@ export class PythonControllerRuntime { try{if(this.disposeFunction){const result=this.disposeFunction(this.state);destroyProxy(result);}} finally{ destroyProxy(this.state);this.state=undefined; - this.initFunction?.destroy();this.stepFunction?.destroy();this.resetFunction?.destroy();this.disposeFunction?.destroy();this.globals?.destroy(); - this.initFunction=undefined;this.stepFunction=undefined;this.resetFunction=undefined;this.disposeFunction=undefined;this.globals=undefined; + this.initFunction?.destroy();this.stepFunction?.destroy();this.resetFunction?.destroy();this.commandFunction?.destroy();this.disposeFunction?.destroy();this.globals?.destroy(); + this.initFunction=undefined;this.stepFunction=undefined;this.resetFunction=undefined;this.commandFunction=undefined;this.disposeFunction=undefined;this.globals=undefined; } } } diff --git a/wasm/web_platform/src/controller/types.ts b/wasm/web_platform/src/controller/types.ts index 957462a2..108d85e3 100644 --- a/wasm/web_platform/src/controller/types.ts +++ b/wasm/web_platform/src/controller/types.ts @@ -1,3 +1,5 @@ +export type ControllerCommand='stop'|'forward'|'backward'|'turn_left'|'turn_right'|'jump'; + export interface ControllerStatus { language:'python'; path:string; @@ -5,6 +7,8 @@ export interface ControllerStatus { controlHz:number; loaded:boolean; enabled:boolean; + acceptsCommands:boolean; + activeCommand?:ControllerCommand; lastStepMs:number; error?:string; } diff --git a/wasm/web_platform/src/simulation/PhysicsAdapter.ts b/wasm/web_platform/src/simulation/PhysicsAdapter.ts index 1df6b6f7..22426a4d 100644 --- a/wasm/web_platform/src/simulation/PhysicsAdapter.ts +++ b/wasm/web_platform/src/simulation/PhysicsAdapter.ts @@ -4,7 +4,7 @@ import {prepareProjectForMujoco} from '../project/importer'; import {enhanceConvertedMjcf,groundConvertedMjcf,type UrdfBaseMode,type UrdfEnhancementOptions} from '../project/urdfToMjcf'; import {MemfsWorkspace} from '../project/workspace'; import {SimulationSession,type ActuatorParameters,type FrameResult,type SimulationSnapshot} from './SimulationSession'; -import type {ControllerStatus} from '../controller/types'; +import type {ControllerCommand,ControllerStatus} from '../controller/types'; export type UrdfLoadMode='mjcf'|'native'; export type {UrdfBaseMode,UrdfEnhancementOptions}; @@ -26,6 +26,7 @@ export interface PhysicsAdapter { clearExternalForce(): void; loadPythonController(source:string,path:string):Promise; setControllerEnabled(enabled:boolean):void; + sendControllerCommand(command:ControllerCommand):void; removeController():void; cachedSupportFiles():ProjectFile[]; exportMjcf(): Uint8Array; @@ -91,6 +92,7 @@ export class MainThreadPhysicsAdapter implements PhysicsAdapter { clearExternalForce():void{this.session?.clearExternalForce();} async loadPythonController(source:string,path:string):Promise{if(!this.session)throw new Error('请先加载模型');return this.session.loadPythonController(source,path);} setControllerEnabled(enabled:boolean):void{this.session?.setControllerEnabled(enabled);} + sendControllerCommand(command:ControllerCommand):void{this.session?.sendControllerCommand(command);} removeController():void{this.session?.removeController();} cachedSupportFiles():ProjectFile[]{return this.supportFiles.map(file=>({...file,data:file.data.slice()}));} exportMjcf():Uint8Array{ diff --git a/wasm/web_platform/src/simulation/SimulationSession.ts b/wasm/web_platform/src/simulation/SimulationSession.ts index bf347932..cf23b0fc 100644 --- a/wasm/web_platform/src/simulation/SimulationSession.ts +++ b/wasm/web_platform/src/simulation/SimulationSession.ts @@ -1,7 +1,7 @@ import type {MainModule, MjData, MjModel, MjvPerturb, MjvScene} from '@mujoco/mujoco'; import {meshIdFromSceneDataId} from './geometry'; import {PythonControllerRuntime} from '../controller/PythonControllerRuntime'; -import type {ControllerBindings,ControllerStatus} from '../controller/types'; +import type {ControllerBindings,ControllerCommand,ControllerStatus} from '../controller/types'; export interface ActuatorParameters {gear:number;gain:number;kp:number;kv:number;ctrlLimited:boolean;ctrlMin:number;ctrlMax:number;forceLimited:boolean;forceMin:number;forceMax:number;} export interface ActuatorInfo extends ActuatorParameters {id:number;name:string;value:number;min:number;max:number;limited:boolean;jointId?:number;jointName?:string;jointType?:number;unit:string;kind:'motor'|'position'|'velocity'|'other';controlCount:number;} @@ -73,6 +73,8 @@ export class SimulationSession { if(!enabled)this.data.ctrl.fill(0); } + sendControllerCommand(command:ControllerCommand):void {this.pythonController?.command(command);} + removeController():void {this.controllerLoadGeneration+=1;this.pythonController?.dispose();this.pythonController=undefined;this.data.ctrl.fill(0);} private runController():void {