chenlin
f4b415c54f
web-platform-ci / TypeScript、Lint、Unit、Build (push) Has been cancelled
web-platform-ci / Playwright E2E (push) Has been cancelled
web-platform-ci / TypeScript、Lint、Unit、Build (pull_request) Has been cancelled
web-platform-ci / Playwright E2E (pull_request) Has been cancelled
482 lines
18 KiB
Python
482 lines
18 KiB
Python
"""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
|