"""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