refactor(web-platform): release V0.6 精简代码

controllers/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` 风格，或平台生成的 `<joint-name>_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`。速度指令采用斜坡限制，避免轮毂力矩阶跃导致机身后仰；起跳会等待轮速和重心稳定，随后以小腿满力矩爆发伸展，并在腾空阶段主动收腿和持续控制俯仰。

controllers/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