从 LinkDog 第三代四足机器狗看桌面级 AI 硬件的开发路径

import math
import time

# ── 机器狗几何参数（按你的实际尺寸修改）──────────────────
LEG_SEGMENTS = [38, 32]          # 大腿、小腿长度 (mm)
HIP_OFFSETS = [55, 40, 35]       # 前左、前右、后左、后右 的髋关节 x 坐标

# ── 步态参数 ────────────────────────────────────────────
STRIDE   = 30      # mm
LIFT     = 20      # mm
PERIOD   = 400     # ms
STANCE_Y = -50     # 站立时足端 y 坐标（相对髋关节）

# ── 足端轨迹生成 ─────────────────────────────────────────
def foot_trajectory(phase, is_swing):
    """
    phase: 0~1, 当前步态周期进度
    is_swing: True=摆动相（抬腿），False=支撑相（触地推进）
    返回足端 (x_offset, y_offset) 相对站立位置
    """
    if is_swing:
        x = STRIDE * (1 - 2 * phase)          # 从前到后
        y = LIFT * math.sin(math.pi * phase)   # 抛起弧线
    else:
        x = STRIDE * (2 * phase - 1)           # 从后到前推进
        y = 0
    return x, y

# ── 逆运动学：足端坐标 → 三个关节角度 ────────────────────
def ik(dx, dy, segs=LEG_SEGMENTS):
    """dx, dy: 足端相对髋关节的偏移，返回 (hip, knee, ankle) 角度（弧度）"""
    target_y = STANCE_Y + dy
    target_x = dx
    L1, L2 = segs
    dist = math.hypot(target_x, target_y)
    # 距离超出可达范围时钳位
    dist = min(dist, L1 + L2 - 1)
    dist = max(dist, abs(L1 - L2) + 1)

    knee_angle = math.acos((L1**2 + L2**2 - dist**2) / (2 * L1 * L2))
    hip_angle   = math.atan2(target_x, -target_y) - \
                  math.acos((L1**2 + dist**2 - L2**2) / (2 * L1 * dist))
    ankle_angle = hip_angle + knee_angle - math.pi  # 保持足端朝下
    return hip_angle, knee_angle, ankle_angle

# ── Trot 步态：对角腿同步 ─────────────────────────────────
LEG_LABELS = ["FL", "FR", "BL", "BR"]   # 前左、前右、后左、后右
SWING_PAIRS = {                          # trot: 对角同步
    0: True,   # FL swing
    1: False,  # FR stance
    2: False,  # BL stance
    3: True,   # BR swing
}

def trot_step(phase):
    """给定 phase(0~1)，返回 12 个关节角度的字典"""
    angles = {}
    for i, label in enumerate(LEG_LABELS):
        # 摆动相和支撑相各占半个周期，摆动相在前半段
        is_swing = SWING_PAIRS[i]
        local_phase = phase if is_swing else phase
        # 支撑相相位对齐：也在 0~1 内推进
        if not is_swing:
            local_phase = phase

        dx, dy = foot_trajectory(local_phase, is_swing)
        hip, knee, ankle = ik(dx, dy)
        angles[f"{label}_hip"]    = round(math.degrees(hip), 1)
        angles[f"{label}_knee"]   = round(math.degrees(knee), 1)
        angles[f"{label}_ankle"]  = round(math.degrees(ankle), 1)
    return angles

# ── 模拟运行：打印 5 个步态帧 ──────────────────────────────
if __name__ == "__main__":
    for step in range(5):
        phase = step / 5
        cmd = trot_step(phase)
        print(f"--- phase={phase:.2f} ---")
        for leg in LEG_LABELS:
            h = cmd[f"{leg}_hip"]
            k = cmd[f"{leg}_knee"]
            a = cmd[f"{leg}_ankle"]
            print(f"  {leg}: hip={h}°  knee={k}°  ankle={a}°")

python trot_gait.py

# ── 简易指令解析器（可替换为本地 LLM 推理）───────────────
COMMAND_MAP = {
    "前进":  {"stride": 30, "lift": 20, "period": 400, "direction": 0},
    "快跑":  {"stride": 45, "lift": 15, "period": 250, "direction": 0},
    "慢走":  {"stride": 15, "lift": 25, "period": 600, "direction": 0},
    "左转":  {"stride": 20, "lift": 20, "period": 400, "direction": -30},
    "右转":  {"stride": 20, "lift": 20, "period": 400, "direction": 30},
    "停下":  {"stride": 0,  "lift": 0,  "period": 0,   "direction": 0},
}

def parse_command(text: str) -> dict:
    """从自然语言提取步态参数，未匹配则默认停下"""
    for key, params in COMMAND_MAP.items():
        if key in text:
            return params
    return COMMAND_MAP["停下"]

# 使用示例
cmd = parse_command("向前快跑")
print(cmd)  # {'stride': 45, 'lift': 15, 'period': 250, 'direction': 0}