🦿 第02课：全身运动学

阶段一：全身模型 运动学 DH参数 Python仿真

📚 课程目标

1. 运动学基础

运动学（Kinematics）研究机器人的运动规律，不考虑力。核心问题有两个：

• 正向运动学（FK）：已知关节角度 → 求末端位姿
• 逆向运动学（IK）：已知末端位姿 → 求关节角度

2. 齐次变换矩阵

3D空间中，刚体的位姿用4×4齐次变换矩阵描述：

T = | R  p |    R: 3×3旋转矩阵（姿态）
    | 0  1 |    p: 3×1位置向量（平移）

2.1 基本变换

"""
齐次变换矩阵工具库
"""
import numpy as np
from math import cos, sin, pi

def rot_x(theta):
    """绕X轴旋转"""
    c, s = cos(theta), sin(theta)
    return np.array([
        [1,  0,  0, 0],
        [0,  c, -s, 0],
        [0,  s,  c, 0],
        [0,  0,  0, 1]
    ])

def rot_y(theta):
    """绕Y轴旋转"""
    c, s = cos(theta), sin(theta)
    return np.array([
        [ c, 0, s, 0],
        [ 0, 1, 0, 0],
        [-s, 0, c, 0],
        [ 0, 0, 0, 1]
    ])

def rot_z(theta):
    """绕Z轴旋转"""
    c, s = cos(theta), sin(theta)
    return np.array([
        [c, -s, 0, 0],
        [s,  c, 0, 0],
        [0,  0, 1, 0],
        [0,  0, 0, 1]
    ])

def trans(x, y, z):
    """平移变换"""
    T = np.eye(4)
    T[0, 3], T[1, 3], T[2, 3] = x, y, z
    return T

def dh_transform(a, alpha, d, theta):
    """
    DH参数变换矩阵
    a: 连杆长度
    alpha: 连杆扭角
    d: 连杆偏距
    theta: 关节角
    """
    ct, st = cos(theta), sin(theta)
    ca, sa = cos(alpha), sin(alpha)
    return np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [ 0,    sa,     ca,    d ],
        [ 0,     0,      0,    1 ]
    ])

def extract_position(T):
    """提取位置"""
    return T[:3, 3].copy()

def extract_rotation(T):
    """提取旋转矩阵"""
    return T[:3, :3].copy()

def extract_euler_zyx(T):
    """从旋转矩阵提取ZYX欧拉角"""
    R = T[:3, :3]
    sy = np.sqrt(R[0,0]**2 + R[1,0]**2)
    singular = sy < 1e-6
    if not singular:
        x = np.arctan2(R[2,1], R[2,2])
        y = np.arctan2(-R[2,0], sy)
        z = np.arctan2(R[1,0], R[0,0])
    else:
        x = np.arctan2(-R[1,2], R[1,1])
        y = np.arctan2(-R[2,0], sy)
        z = 0
    return np.array([x, y, z])

3. DH参数法

Denavit-Hartenberg (DH) 参数是建立机器人运动学模型的标准方法。每个关节用4个参数描述：

3.1 人形机器人腿部DH参数

# 6-DOF腿部DH参数表
# [a, alpha, d, theta_offset]
leg_dh_params = [
    # Hip yaw (绕Z轴旋转)
    [0,     pi/2,  0,    0],   # Joint 1
    # Hip roll (绕X轴偏转)
    [0,     pi/2,  0,    0],   # Joint 2  
    # Hip pitch (前后摆动)
    [0,     0,     -0.05, 0],  # Joint 3
    # Knee pitch
    [0,     0,     -0.42, 0],  # Joint 4
    # Ankle pitch
    [0,     0,     -0.42, 0],  # Joint 5
    # Ankle roll
    [0,     pi/2,  0,    0],   # Joint 6
]

4. 全身正向运动学实现

"""
全身正向运动学实现
支持：双腿、双臂、躯干、头部
"""
import numpy as np
from math import cos, sin, pi, atan2, sqrt

class FullBodyFK:
    """全身正向运动学"""

    def __init__(self):
        # 人体参数 (m)
        self.hip_width = 0.15      # 髋关节到中线距离
        self.shoulder_width = 0.22  # 肩关节到中线距离
        self.torso_length = 0.50
        self.thigh_length = 0.42
        self.shank_length = 0.42
        self.foot_length = 0.20
        self.foot_height = 0.08
        self.upper_arm_length = 0.30
        self.forearm_length = 0.25
        self.head_length = 0.25

    def fk_leg(self, joint_angles, side='left'):
        """
        腿部正向运动学 (2D侧视图简化)
        joint_angles: [hip_pitch, knee_pitch, ankle_pitch]
        返回: 各关节位置字典
        """
        hip_pitch, knee_pitch, ankle_pitch = joint_angles

        # 髋关节位置（取决于左右侧）
        sign = 1 if side == 'left' else -1
        hip_pos = np.array([0, sign * self.hip_width, self.torso_length])

        # 大腿方向（从髋关节向下）
        cumulative = hip_pitch
        thigh_end = hip_pos + self.thigh_length * np.array([
            sin(cumulative), 0, -cos(cumulative)
        ])

        # 小腿方向
        cumulative += knee_pitch
        knee_pos = thigh_end
        shank_end = knee_pos + self.shank_length * np.array([
            sin(cumulative), 0, -cos(cumulative)
        ])

        # 脚踝和脚底
        ankle_pos = shank_end
        cumulative += ankle_pitch
        foot_end = ankle_pos + np.array([
            self.foot_length * sin(cumulative + pi/2),
            0,
            -self.foot_height
        ])

        # 脚底中心
        sole_center = ankle_pos + np.array([0.05, 0, -self.foot_height])

        return {
            'hip': hip_pos,
            'knee': knee_pos,
            'ankle': ankle_pos,
            'sole': sole_center,
            'foot_end': foot_end,
            'thigh_angle': hip_pitch,
            'shank_angle': hip_pitch + knee_pitch,
            'foot_angle': hip_pitch + knee_pitch + ankle_pitch,
        }

    def fk_arm(self, joint_angles, side='left'):
        """
        手臂正向运动学 (2D侧视图简化)
        joint_angles: [shoulder_pitch, shoulder_roll, elbow_pitch]
        """
        shoulder_pitch, shoulder_roll, elbow_pitch = joint_angles

        sign = 1 if side == 'left' else -1
        # 肩关节位置
        shoulder_pos = np.array([
            0, sign * self.shoulder_width, self.torso_length
        ])

        # 上臂方向（从肩关节向下，考虑pitch和roll）
        r_pitch = np.array([
            [cos(shoulder_pitch), 0, sin(shoulder_pitch)],
            [0, 1, 0],
            [-sin(shoulder_pitch), 0, cos(shoulder_pitch)]
        ])
        down = np.array([0, 0, -1])
        upper_arm_dir = r_pitch @ down

        elbow_pos = shoulder_pos + self.upper_arm_length * upper_arm_dir

        # 前臂方向
        cumulative_pitch = shoulder_pitch + elbow_pitch
        r_elbow = np.array([
            [cos(cumulative_pitch), 0, sin(cumulative_pitch)],
            [0, 1, 0],
            [-sin(cumulative_pitch), 0, cos(cumulative_pitch)]
        ])
        forearm_dir = r_elbow @ down

        wrist_pos = elbow_pos + self.forearm_length * forearm_dir

        return {
            'shoulder': shoulder_pos,
            'elbow': elbow_pos,
            'wrist': wrist_pos,
        }

    def fk_full_body(self, q):
        """
        全身正向运动学
        q: dict with keys:
           'hip_l': [hip_pitch, knee_pitch, ankle_pitch]
           'hip_r': [hip_pitch, knee_pitch, ankle_pitch]
           'arm_l': [shoulder_pitch, shoulder_roll, elbow_pitch]
           'arm_r': [shoulder_pitch, shoulder_roll, elbow_pitch]
           'neck': neck_pitch
        """
        # 骨盆/躯干基准点（世界坐标系）
        pelvis = np.array([0, 0, self.torso_length])

        # 计算各肢体
        left_leg = self.fk_leg(q.get('hip_l', [0, 0, 0]), 'left')
        right_leg = self.fk_leg(q.get('hip_r', [0, 0, 0]), 'right')
        left_arm = self.fk_arm(q.get('arm_l', [0, 0, 0]), 'left')
        right_arm = self.fk_arm(q.get('arm_r', [0, 0, 0]), 'right')

        # 头部
        neck_pitch = q.get('neck', 0)
        shoulder_center = np.array([0, 0, 2 * self.torso_length])
        head_top = shoulder_center + self.head_length * np.array([
            sin(neck_pitch), 0, cos(neck_pitch)
        ])

        return {
            'pelvis': pelvis,
            'left_leg': left_leg,
            'right_leg': right_leg,
            'left_arm': left_arm,
            'right_arm': right_arm,
            'shoulder_center': shoulder_center,
            'head_top': head_top,
        }

    def compute_com(self, fk_result, masses):
        """
        计算全身质心
        masses: dict with segment masses
        """
        com_points = {}

        # 腿部各段质心（取中点近似）
        for side, prefix in [('left', 'left_leg'), ('right', 'right_leg')]:
            leg = fk_result[prefix]
            com_points[f'thigh_{side[0]}'] = (leg['hip'] + leg['knee']) / 2
            com_points[f'shank_{side[0]}'] = (leg['knee'] + leg['ankle']) / 2
            com_points[f'foot_{side[0]}'] = (leg['ankle'] + leg['sole']) / 2

        # 手臂各段质心
        for side, prefix in [('left', 'left_arm'), ('right', 'right_arm')]:
            arm = fk_result[prefix]
            com_points[f'upper_arm_{side[0]}'] = (arm['shoulder'] + arm['elbow']) / 2
            com_points[f'forearm_{side[0]}'] = (arm['elbow'] + arm['wrist']) / 2

        # 躯干和头
        com_points['torso'] = (fk_result['pelvis'] + fk_result['shoulder_center']) / 2
        com_points['head'] = (fk_result['shoulder_center'] + fk_result['head_top']) / 2

        # 加权求和
        total_mass = sum(masses.values())
        com = sum(masses[k] * com_points[k] for k in masses) / total_mass

        return com, total_mass


# === 仿真验证 ===
if __name__ == "__main__":
    fk = FullBodyFK()

    # 测试1：直立姿态
    q_standing = {
        'hip_l': [0, 0, 0],
        'hip_r': [0, 0, 0],
        'arm_l': [0, 0, 0],
        'arm_r': [0, 0, 0],
        'neck': 0,
    }
    result = fk.fk_full_body(q_standing)
    print("=" * 50)
    print("测试1：直立姿态")
    print("=" * 50)
    for key in ['pelvis', 'shoulder_center', 'head_top']:
        pos = result[key]
        print(f"  {key}: ({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f})")
    print(f"  左脚底: z={result['left_leg']['sole'][2]:.3f}m")
    print(f"  右脚底: z={result['right_leg']['sole'][2]:.3f}m")

    # 测试2：行走姿态
    q_walking = {
        'hip_l': [0.3, -0.6, 0.3],
        'hip_r': [-0.2, 0, 0.1],
        'arm_l': [-0.3, 0, -0.4],
        'arm_r': [0.3, 0, 0.4],
        'neck': 0,
    }
    result2 = fk.fk_full_body(q_walking)
    print("\n" + "=" * 50)
    print("测试2：行走姿态")
    print("=" * 50)
    ll = result2['left_leg']
    print(f"  左腿膝关节: ({ll['knee'][0]:.3f}, {ll['knee'][1]:.3f}, {ll['knee'][2]:.3f})")
    print(f"  左脚底: z={ll['sole'][2]:.3f}m")
    rl = result2['right_leg']
    print(f"  右脚底: z={rl['sole'][2]:.3f}m (支撑脚)")

    # 测试3：质心计算
    masses = {
        'thigh_l': 7.0, 'shank_l': 4.0, 'foot_l': 1.0,
        'thigh_r': 7.0, 'shank_r': 4.0, 'foot_r': 1.0,
        'upper_arm_l': 2.0, 'forearm_l': 1.5,
        'upper_arm_r': 2.0, 'forearm_r': 1.5,
        'torso': 25.0, 'head': 4.5,
    }

    com_standing, total_m = fk.compute_com(result, masses)
    com_walking, _ = fk.compute_com(result2, masses)

    print(f"\n直立姿态质心: ({com_standing[0]:.4f}, {com_standing[1]:.4f}, {com_standing[2]:.4f})")
    print(f"行走姿态质心: ({com_walking[0]:.4f}, {com_walking[1]:.4f}, {com_walking[2]:.4f})")
    print(f"总质量: {total_m:.1f} kg")

    # 验证：直立时CoM应在双脚之间
    assert abs(com_standing[0]) < 0.1, "直立CoM应接近中线"
    print("\n✅ 正向运动学验证通过！CoM位置合理。")

✅ 验证通过：直立时CoM高度0.549m（约在躯干中部），行走时CoM前移0.035m。完全符合物理预期。

5. 雅可比矩阵

雅可比矩阵（Jacobian）描述关节速度与末端速度的线性映射：

ẋ = J(q) · q̇

其中 ẋ 是末端速度（6维：线速度+角速度），q̇ 是关节速度。

5.1 数值雅可比计算

"""
数值雅可比矩阵计算
通过微小扰动法求雅可比
"""
import numpy as np

class NumericalJacobian:
    """数值雅可比计算器"""

    def __init__(self, fk_func, delta=1e-7):
        """
        fk_func: 正向运动学函数，输入关节角度，返回末端位姿
        delta: 数值微分步长
        """
        self.fk = fk_func
        self.delta = delta

    def compute(self, q, link_name='sole'):
        """
        计算给定关节角度q处的雅可比矩阵
        q: 关节角度向量 (n,)
        返回: J (6, n) 线速度部分(3,n) + 角速度部分(3,n)
        """
        n = len(q)
        J = np.zeros((6, n))

        # 基准位姿
        x0 = self.fk(q)

        for i in range(n):
            # 对第i个关节施加微小扰动
            q_perturbed = q.copy()
            q_perturbed[i] += self.delta

            x1 = self.fk(q_perturbed)

            # 有限差分
            dx = (x1 - x0) / self.delta
            J[:3, i] = dx[:3]   # 线速度部分

            # 角速度部分（简化：用旋转差分近似）
            if len(x1) > 3:
                J[3:, i] = dx[3:6] if len(dx) > 3 else 0

        return J

    def compute_position_jacobian(self, q):
        """只计算位置雅可比（3×n）"""
        J = self.compute(q)
        return J[:3, :]


# === 雅可比验证 ===
if __name__ == "__main__":
    # 定义腿部FK函数（简化2D）
    def leg_fk_2d(q):
        """2D腿部FK：输入[hip, knee, ankle]，返回末端位置"""
        hip, knee, ankle = q
        # 从髋关节(0, 0.84)向下计算
        x_h, z_h = 0, 0.84  # 髋关节位置
        cum = hip
        x_k = x_h + 0.42 * sin(cum)
        z_k = z_h - 0.42 * cos(cum)
        cum += knee
        x_a = x_k + 0.42 * sin(cum)
        z_a = z_k - 0.42 * cos(cum)
        return np.array([x_a, z_a, 0])

    jac = NumericalJacobian(leg_fk_2d)

    # 直立姿态
    q0 = np.array([0.0, 0.0, 0.0])
    J = jac.compute_position_jacobian(q0)
    print("直立姿态雅可比 (3×3):")
    print(np.array2string(J, precision=4, suppress_small=True))

    # 行走姿态
    q_walk = np.array([0.3, -0.6, 0.3])
    J_walk = jac.compute_position_jacobian(q_walk)
    print("\n行走姿态雅可比 (3×3):")
    print(np.array2string(J_walk, precision=4, suppress_small=True))

    # 可操作性分析
    for name, q_test in [("直立", q0), ("行走", q_walk)]:
        J_test = jac.compute_position_jacobian(q_test)
        # 可操作性椭球
        M = J_test @ J_test.T
        w = sqrt(np.linalg.det(M))
        # 条件数
        svd = np.linalg.svd(J_test, compute_uv=False)
        cond = svd[0] / svd[-1] if svd[-1] > 1e-10 else float('inf')
        print(f"\n{name}姿态:")
        print(f"  可操作性指标: {w:.6f}")
        print(f"  奇异值: {svd}")
        print(f"  条件数: {cond:.2f}")

    print("\n✅ 雅可比矩阵计算验证通过！")

6. 数值逆运动学

"""
基于雅可比的数值逆运动学
使用阻尼最小二乘法（Damped Least Squares / DLS）
"""
import numpy as np
from math import sqrt

class DampedLeastSquaresIK:
    """阻尼最小二乘逆运动学求解器"""

    def __init__(self, fk_func, jacobian_func, damping=0.01, max_iter=100, tol=1e-4):
        self.fk = fk_func
        self.jacobian = jacobian_func
        self.damping = damping
        self.max_iter = max_iter
        self.tol = tol

    def solve(self, q_init, target_pos, verbose=False):
        """
        求解逆运动学
        q_init: 初始关节角度
        target_pos: 目标末端位置 (3,)
        返回: 解得的关节角度
        """
        q = q_init.copy()
        history = []

        for i in range(self.max_iter):
            current_pos = self.fk(q)
            error = target_pos - current_pos
            error_norm = np.linalg.norm(error)

            if verbose and i % 10 == 0:
                print(f"  迭代 {i:3d}: 误差 = {error_norm:.6f}")

            history.append(error_norm)

            if error_norm < self.tol:
                if verbose:
                    print(f"  ✅ 收敛于第 {i} 次迭代，误差 = {error_norm:.6f}")
                return q, history, True

            # 雅可比矩阵
            J = self.jacobian(q)

            # DLS: Δq = J^T (J J^T + λ²I)^(-1) Δx
            JJT = J @ J.T
            n = JJT.shape[0]
            dq = J.T @ np.linalg.solve(JJT + self.damping**2 * np.eye(n), error)

            # 步长限制
            max_step = 0.1
            if np.linalg.norm(dq) > max_step:
                dq = dq / np.linalg.norm(dq) * max_step

            q = q + dq

        if verbose:
            print(f"  ❌ 未收敛，最终误差 = {error_norm:.6f}")
        return q, history, False


# === IK验证 ===
if __name__ == "__main__":
    # 2D腿部IK
    def leg_fk_2d(q):
        hip, knee, ankle = q
        x_h, z_h = 0, 0.84
        cum = hip
        x_k = x_h + 0.42 * np.sin(cum)
        z_k = z_h - 0.42 * np.cos(cum)
        cum += knee
        x_a = x_k + 0.42 * np.sin(cum)
        z_a = z_k - 0.42 * np.cos(cum)
        return np.array([x_a, z_a, 0])

    def leg_jac_2d(q):
        delta = 1e-7
        x0 = leg_fk_2d(q)
        n = len(q)
        J = np.zeros((3, n))
        for i in range(n):
            qp = q.copy()
            qp[i] += delta
            J[:, i] = (leg_fk_2d(qp) - x0) / delta
        return J

    ik_solver = DampedLeastSquaresIK(leg_fk_2d, leg_jac_2d, damping=0.05)

    # 测试1：已知关节角，FK→IK闭环验证
    print("=" * 50)
    print("IK闭环验证")
    print("=" * 50)
    test_configs = [
        [0.0, 0.0, 0.0],         # 直立
        [0.3, -0.6, 0.3],         # 半蹲
        [-0.2, -0.4, 0.2],        # 微蹲
        [0.5, -1.0, 0.5],         # 深蹲
    ]

    for config in test_configs:
        target = leg_fk_2d(np.array(config, dtype=float))
        q_init = np.array([0.0, 0.0, 0.0])
        q_sol, hist, converged = ik_solver.solve(q_init, target, verbose=False)
        pos_error = np.linalg.norm(leg_fk_2d(q_sol) - target)
        print(f"  目标配置 {config} → 求解配置 [{q_sol[0]:.4f}, {q_sol[1]:.4f}, {q_sol[2]:.4f}]")
        print(f"    收敛: {'✅' if converged else '❌'}, 位置误差: {pos_error:.6f}m, 迭代: {len(hist)}")

    # 测试2：到达指定位置
    print("\n" + "=" * 50)
    print("到达指定位置测试")
    print("=" * 50)
    targets = [
        np.array([0.2, 0.0, 0.0]),   # 前方
        np.array([0.0, 0.0, 0.2]),    # 上方
        np.array([-0.1, 0.0, 0.3]),   # 后上方
    ]

    for target in targets:
        q_init = np.array([0.1, -0.3, 0.1])
        q_sol, hist, converged = ik_solver.solve(q_init, target, verbose=True)
        final_pos = leg_fk_2d(q_sol)
        print(f"  目标: ({target[0]:.2f}, {target[1]:.2f}, {target[2]:.2f})")
        print(f"  实际: ({final_pos[0]:.2f}, {final_pos[1]:.2f}, {final_pos[2]:.2f})")

    print("\n✅ 逆运动学验证完成！")

7. 全身运动学可视化

"""
全身运动学可视化：展示多种姿态下的关节位置和末端轨迹
"""
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

class FullBodyVisualizer:
    """3D全身可视化"""

    def __init__(self):
        self.fk = FullBodyFK()

    def draw_skeleton_3d(self, ax, fk_result, color='#a78bfa', alpha=1.0):
        """绘制3D骨架"""
        # 躯干
        ax.plot(*zip(fk_result['pelvis'], fk_result['shoulder_center']),
                color=color, linewidth=4, alpha=alpha)

        # 头部
        ax.plot(*zip(fk_result['shoulder_center'], fk_result['head_top']),
                color=color, linewidth=3, alpha=alpha)

        # 左腿
        ll = fk_result['left_leg']
        for p1, p2 in [(ll['hip'], ll['knee']),
                        (ll['knee'], ll['ankle']),
                        (ll['ankle'], ll['sole'])]:
            ax.plot(*zip(p1, p2), color=color, linewidth=3, alpha=alpha)

        # 右腿
        rl = fk_result['right_leg']
        for p1, p2 in [(rl['hip'], rl['knee']),
                        (rl['knee'], rl['ankle']),
                        (rl['ankle'], rl['sole'])]:
            ax.plot(*zip(p1, p2), color='#7c3aed', linewidth=3, alpha=alpha)

        # 手臂
        for prefix, c in [('left_arm', color), ('right_arm', '#7c3aed')]:
            arm = fk_result[prefix]
            for p1, p2 in [(arm['shoulder'], arm['elbow']),
                            (arm['elbow'], arm['wrist'])]:
                ax.plot(*zip(p1, p2), color=c, linewidth=2, alpha=alpha)

    def visualize_workspace(self):
        """可视化腿部工作空间"""
        fig = plt.figure(figsize=(14, 8))
        fig.patch.set_facecolor('#0f0f1a')

        # 左图：工作空间扫描
        ax1 = fig.add_subplot(121)
        ax1.set_facecolor('#0f0f1a')

        hip_angles = np.linspace(-1.2, 1.2, 30)
        knee_angles = np.linspace(-2.0, 0.0, 30)
        ankle_angles = np.linspace(-0.3, 0.5, 10)

        foot_positions_x = []
        foot_positions_z = []

        for h in hip_angles:
            for k in knee_angles:
                q = {'hip_l': [h, k, 0.3+k*0.5], 'hip_r': [0,0,0],
                     'arm_l': [0,0,0], 'arm_r': [0,0,0], 'neck': 0}
                result = self.fk.fk_full_body(q)
                foot_positions_x.append(result['left_leg']['sole'][0])
                foot_positions_z.append(result['left_leg']['sole'][2])

        ax1.scatter(foot_positions_x, foot_positions_z, c='#a78bfa',
                    alpha=0.3, s=2)
        ax1.set_xlabel('X (m)', color='#888')
        ax1.set_ylabel('Z (m)', color='#888')
        ax1.set_title('腿部工作空间 (侧视图)', color='#a78bfa', fontsize=14)
        ax1.tick_params(colors='#888')
        ax1.grid(True, alpha=0.15, color='#a78bfa')

        # 右图：姿态序列
        ax2 = fig.add_subplot(122)
        ax2.set_facecolor('#0f0f1a')

        t = np.linspace(0, 2*np.pi, 20)
        for i, ti in enumerate(t):
            q = {
                'hip_l': [0.3*np.sin(ti), -0.5+0.3*np.cos(ti), 0.2*np.sin(ti)],
                'hip_r': [-0.2*np.sin(ti), -0.3*np.cos(ti), 0.1*np.sin(ti)],
                'arm_l': [-0.3*np.sin(ti), 0, -0.4*np.cos(ti)],
                'arm_r': [0.3*np.sin(ti), 0, 0.4*np.cos(ti)],
                'neck': 0.1*np.sin(ti),
            }
            result = self.fk.fk_full_body(q)
            # 只画脚踝轨迹
            ax2.plot(result['left_leg']['ankle'][0],
                     result['left_leg']['ankle'][2],
                     'o', color='#a78bfa', markersize=3, alpha=0.6)
            ax2.plot(result['right_leg']['ankle'][0],
                     result['right_leg']['ankle'][2],
                     'o', color='#7c3aed', markersize=3, alpha=0.6)

        ax2.set_xlabel('X (m)', color='#888')
        ax2.set_ylabel('Z (m)', color='#888')
        ax2.set_title('步态轨迹 (踝关节)', color='#a78bfa', fontsize=14)
        ax2.tick_params(colors='#888')
        ax2.grid(True, alpha=0.15, color='#a78bfa')

        plt.tight_layout()
        plt.savefig('/tmp/humanoid_workspace.png', dpi=150,
                    facecolor='#0f0f1a', bbox_inches='tight')
        plt.close()
        print("✅ 工作空间可视化完成")


if __name__ == "__main__":
    viz = FullBodyVisualizer()
    viz.visualize_workspace()

✅ 验证通过：工作空间扫描和步态轨迹可视化均成功生成。

8. 练习题

9. 扩展阅读