🔄 第05课：关节空间与任务空间

阶段一：全身模型 空间映射 雅可比 冗余度

📚 课程目标

1. 两个空间的世界

机器人控制存在两个基本视角：

2. 雅可比矩阵深入

"""
雅可比矩阵的完整实现与分析
包含：解析雅可比、数值雅可比、奇异值分解、可操作性
"""
import numpy as np
from math import sin, cos, sqrt, atan2

class JacobianAnalyzer:
    """雅可比矩阵分析器"""

    def __init__(self, fk_func, n_joints):
        self.fk = fk_func
        self.n = n_joints

    def numerical_jacobian(self, q, delta=1e-7):
        """数值雅可比（有限差分法）"""
        x0 = self.fk(q)
        m = len(x0)
        J = np.zeros((m, self.n))

        for i in range(self.n):
            q_plus = q.copy()
            q_plus[i] += delta
            x_plus = self.fk(q_plus)
            J[:, i] = (x_plus - x0) / delta

        return J

    def svd_analysis(self, J):
        """奇异值分解分析"""
        U, S, Vt = np.linalg.svd(J)
        return {
            'U': U, 'S': S, 'Vt': Vt,
            'singular_values': S,
            'condition_number': S[0] / S[-1] if S[-1] > 1e-10 else float('inf'),
            'manipulability': sqrt(np.prod(S[S > 1e-10])),
            'rank': np.sum(S > 1e-10),
            'is_singular': S[-1] < 1e-6 if len(S) > 0 else True,
        }

    def manipulability_ellipsoid(self, J):
        """可操作性椭球分析"""
        # 对于位置雅可比 JJ^T 的特征值分解
        M = J @ J.T
        eigenvalues, eigenvectors = np.linalg.eigh(M)
        # 椭球半轴长度 = sqrt(eigenvalue)
        axes = np.sqrt(np.maximum(eigenvalues, 0))
        return {
            'axes': axes,
            'directions': eigenvectors,
            'volume': np.prod(axes),  # 与可操作性成正比
            'isotropy': axes.min() / axes.max() if axes.max() > 0 else 0,
        }


# === 3-DOF 腿部雅可比分析 ===
def leg_fk_3dof(q):
    """3-DOF腿部正向运动学 (2D侧视图)
    q = [hip_pitch, knee_pitch, ankle_pitch]
    返回: [x_ankle, z_ankle, foot_angle]
    """
    hip, knee, ankle = q
    L1, L2 = 0.42, 0.42  # 大腿、小腿长度

    # 关节位置
    x_hip = 0
    z_hip = L1 + L2  # 髋关节高度

    # 膝关节
    x_knee = x_hip + L1 * sin(hip)
    z_knee = z_hip - L1 * cos(hip)

    # 踝关节
    cum = hip + knee
    x_ankle = x_knee + L2 * sin(cum)
    z_ankle = z_knee - L2 * cos(cum)

    # 脚的角度
    foot_angle = hip + knee + ankle

    return np.array([x_ankle, z_ankle, foot_angle])


if __name__ == "__main__":
    analyzer = JacobianAnalyzer(leg_fk_3dof, 3)

    print("=" * 60)
    print("雅可比矩阵分析")
    print("=" * 60)

    configs = {
        "直立": np.array([0.0, 0.0, 0.0]),
        "半蹲": np.array([0.3, -0.6, 0.3]),
        "深蹲": np.array([0.8, -1.5, 0.7]),
        "前伸": np.array([0.5, -0.3, 0.0]),
    }

    for name, q in configs.items():
        J = analyzer.numerical_jacobian(q)
        svd = analyzer.svd_analysis(J)
        ellipsoid = analyzer.manipulability_ellipsoid(J)

        print(f"\n--- {name} 姿态 ---")
        print(f"  雅可比矩阵:")
        for row in J:
            print(f"    [{row[0]:8.4f}, {row[1]:8.4f}, {row[2]:8.4f}]")
        print(f"  奇异值: {svd['singular_values']}")
        print(f"  条件数: {svd['condition_number']:.2f}")
        print(f"  可操作性: {svd['manipulability']:.6f}")
        print(f"  是否奇异: {'⚠️ 是' if svd['is_singular'] else '否'}")
        print(f"  各向同性: {ellipsoid['isotropy']:.4f}")

    print("\n✅ 雅可比分析完成！")

3. 任务空间控制器

"""
任务空间控制器实现
基于雅可比的任务空间PD控制
"""
import numpy as np

class TaskSpaceController:
    """任务空间控制器"""

    def __init__(self, fk_func, jacobian_func, dt=0.01):
        self.fk = fk_func
        self.jacobian = jacobian_func
        self.dt = dt

    def task_pd_control(self, q, q_dot, x_desired, Kp, Kd,
                         nullspace_gain=0.0, q_ref=None):
        """
        任务空间PD控制 + 零空间优化
        τ = J^T(Kp(x_d - x) - Kd*ẋ) + (I - J^T*J^#)τ_null

        Args:
            q: 当前关节角度
            q_dot: 当前关节速度
            x_desired: 目标任务空间位置
            Kp: 位置增益矩阵
            Kd: 阻尼增益矩阵
            nullspace_gain: 零空间增益
            q_ref: 零空间参考关节角度
        """
        # 当前末端位姿
        x_current = self.fk(q)

        # 任务空间误差
        x_error = x_desired - x_current

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

        # 任务空间速度
        x_dot = J @ q_dot

        # 任务空间控制力
        F_task = Kp @ x_error - Kd @ x_dot

        # 映射到关节空间
        # 使用伪逆
        J_pinv = np.linalg.pinv(J)
        tau_task = J.T @ F_task

        # 零空间优化（保持关节角度接近参考值）
        tau_null = np.zeros_like(q)
        if nullspace_gain > 0 and q_ref is not None:
            # 零空间投影
            N = np.eye(len(q)) - J_pinv @ J
            q_error = q_ref - q
            tau_null = nullspace_gain * N @ q_error

        tau_total = tau_task + tau_null
        return tau_total, F_task, x_error

    def simulate_reaching(self, q_init, target_traj, Kp_scale=100,
                           Kd_scale=20, duration=5.0):
        """
        仿真到达运动
        target_traj: 目标轨迹函数 (t) -> x_desired
        """
        n_steps = int(duration / self.dt)
        q = q_init.copy()
        q_dot = np.zeros_like(q)

        q_history = [q.copy()]
        x_history = [self.fk(q)]
        error_history = [0.0]

        m = len(self.fk(q))
        Kp = Kp_scale * np.eye(m)
        Kd = Kd_scale * np.eye(m)

        for i in range(n_steps):
            t = (i + 1) * self.dt
            x_desired = target_traj(t)

            tau, F, x_err = self.task_pd_control(
                q, q_dot, x_desired, Kp, Kd
            )

            # 简化动力学（单位质量矩阵）
            q_ddot = tau  # 忽略完整动力学

            # 积分
            q_dot += q_ddot * self.dt
            # 速度阻尼
            q_dot *= 0.99
            q += q_dot * self.dt

            # 关节限位
            q[1] = np.clip(q[1], -2.0, 0.0)

            q_history.append(q.copy())
            x_history.append(self.fk(q))
            error_history.append(np.linalg.norm(x_err))

        return {
            'q': np.array(q_history),
            'x': np.array(x_history),
            'error': np.array(error_history),
        }


# === 仿真验证 ===
if __name__ == "__main__":
    def leg_jac_3dof(q):
        delta = 1e-7
        x0 = leg_fk_3dof(q)
        J = np.zeros((3, 3))
        for i in range(3):
            qp = q.copy()
            qp[i] += delta
            J[:, i] = (leg_fk_3dof(qp) - x0) / delta
        return J

    ctrl = TaskSpaceController(leg_fk_3dof, leg_jac_3dof, dt=0.01)

    print("=" * 60)
    print("任务空间控制器仿真")
    print("=" * 60)

    # 测试1：定点到达
    q_init = np.array([0.3, -0.6, 0.3])
    x_target = np.array([0.15, 0.2, 0.0])

    result = ctrl.simulate_reaching(
        q_init,
        lambda t: x_target,
        Kp_scale=200, Kd_scale=40, duration=3.0
    )

    print(f"\n定点到达:")
    print(f"  初始末端: ({result['x'][0][0]:.3f}, {result['x'][0][1]:.3f})")
    print(f"  目标位置: ({x_target[0]:.3f}, {x_target[1]:.3f})")
    print(f"  最终末端: ({result['x'][-1][0]:.3f}, {result['x'][-1][1]:.3f})")
    print(f"  最终误差: {result['error'][-1]:.6f}")

    # 测试2：圆形轨迹跟踪
    def circle_traj(t):
        cx, cz = 0.15, 0.3
        r = 0.05
        return np.array([cx + r * np.cos(t), cz + r * np.sin(t), 0.0])

    result_circle = ctrl.simulate_reaching(
        q_init,
        circle_traj,
        Kp_scale=300, Kd_scale=50, duration=10.0
    )

    print(f"\n圆形轨迹跟踪:")
    print(f"  跟踪误差 (平均): {np.mean(result_circle['error'][100:]):.6f}")
    print(f"  跟踪误差 (最大): {np.max(result_circle['error'][100:]):.6f}")

    # 测试3：关节空间 vs 任务空间对比
    print(f"\n--- 空间对比 ---")
    q_jnt = q_init.copy()
    q_tsk = q_init.copy()
    qd_jnt = np.zeros(3)
    qd_tsk = np.zeros(3)

    dt = 0.01
    target_q = np.array([0.5, -1.0, 0.5])
    target_x = leg_fk_3dof(target_q)

    for _ in range(300):
        # 关节空间PD
        tau_jnt = 200 * (target_q - q_jnt) - 40 * qd_jnt
        qd_jnt += tau_jnt * dt
        q_jnt += qd_jnt * dt

        # 任务空间PD
        tau_tsk, _, _ = ctrl.task_pd_control(
            q_tsk, qd_tsk, target_x,
            200 * np.eye(3), 40 * np.eye(3)
        )
        qd_tsk += tau_tsk * dt
        qd_tsk *= 0.99
        q_tsk += qd_tsk * dt

    err_jnt = np.linalg.norm(leg_fk_3dof(q_jnt)[:2] - target_x[:2])
    err_tsk = np.linalg.norm(leg_fk_3dof(q_tsk)[:2] - target_x[:2])
    print(f"  关节空间控制 - 末端误差: {err_jnt:.6f}")
    print(f"  任务空间控制 - 末端误差: {err_tsk:.6f}")

    print("\n✅ 任务空间控制器验证完成！")

4. 冗余度与零空间

"""
冗余度利用：零空间优化
当 DOF > 任务维度时，机器人是冗余的
零空间运动不改变末端位姿，但可优化关节配置
"""
import numpy as np

class NullspaceOptimizer:
    """零空间优化器"""

    def __init__(self, fk_func, jac_func, n_joints):
        self.fk = fk_func
        self.jac = jac_func
        self.n = n_joints

    def compute_nullspace_projector(self, q):
        """计算零空间投影矩阵"""
        J = self.jac(q)
        J_pinv = np.linalg.pinv(J)
        N = np.eye(self.n) - J_pinv @ J
        return N

    def gradient_joint_limits(self, q, q_min, q_max):
        """
        关节限位梯度（推关节远离限位）
        """
        q_mid = (q_min + q_max) / 2
        q_range = (q_max - q_min) / 2
        # 梯度：越接近限位，推力越大
        gradient = -(2 * (q - q_mid)) / (q_range**2)
        return gradient

    def gradient_manipulability(self, q, delta=1e-5):
        """
        可操作性梯度（推向高可操作性区域）
        """
        J = self.jac(q)
        M = J @ J.T
        w = np.sqrt(max(np.linalg.det(M), 1e-20))

        grad = np.zeros(self.n)
        for i in range(self.n):
            q_plus = q.copy()
            q_plus[i] += delta
            J_plus = self.jac(q_plus)
            M_plus = J_plus @ J_plus.T
            w_plus = np.sqrt(max(np.linalg.det(M_plus), 1e-20))
            grad[i] = (w_plus - w) / delta

        return grad

    def redundant_ik_step(self, q, x_target, alpha=0.5,
                           nullspace_weight=0.1, q_min=None, q_max=None):
        """
        冗余IK单步更新
        Δq = J^#Δx + α(I - J^#J)∇h
        """
        x_current = self.fk(q)
        x_error = x_target - x_current

        J = self.jac(q)
        J_pinv = np.linalg.pinv(J)

        # 主要任务
        dq_primary = J_pinv @ x_error

        # 零空间优化
        N = self.compute_nullspace_projector(q)

        # 综合多个零空间目标
        null_grad = np.zeros(self.n)

        # 1. 关节限位避障
        if q_min is not None and q_max is not None:
            null_grad += self.gradient_joint_limits(q, q_min, q_max)

        # 2. 可操作性最大化
        null_grad += 0.01 * self.gradient_manipulability(q)

        dq_null = nullspace_weight * N @ null_grad

        dq = alpha * dq_primary + dq_null
        return dq


# === 验证 ===
if __name__ == "__main__":
    opt = NullspaceOptimizer(leg_fk_3dof, leg_jac_3dof, 3)

    print("=" * 60)
    print("冗余度与零空间优化")
    print("=" * 60)

    # 冗余IK求解
    q = np.array([0.0, 0.0, 0.0])
    target = np.array([0.2, 0.5, 0.0])
    q_min = np.array([-1.5, -2.0, -0.5])
    q_max = np.array([1.5, 0.0, 0.8])

    print(f"\n目标位置: ({target[0]:.2f}, {target[1]:.2f})")
    print(f"关节限位: hip[{q_min[0]:.1f}, {q_max[0]:.1f}], "
          f"knee[{q_min[1]:.1f}, {q_max[1]:.1f}], "
          f"ankle[{q_min[2]:.1f}, {q_max[2]:.1f}]")

    # 不使用零空间优化
    q_no_null = q.copy()
    for _ in range(200):
        dq = opt.redundant_ik_step(q_no_null, target, alpha=0.5,
                                    nullspace_weight=0.0)
        q_no_null += dq

    # 使用零空间优化
    q_with_null = q.copy()
    for _ in range(200):
        dq = opt.redundant_ik_step(q_with_null, target, alpha=0.5,
                                    nullspace_weight=0.3,
                                    q_min=q_min, q_max=q_max)
        q_with_null += dq

    x_no = leg_fk_3dof(q_no_null)
    x_with = leg_fk_3dof(q_with_null)

    print(f"\n无零空间: q=[{q_no_null[0]:.3f}, {q_no_null[1]:.3f}, {q_no_null[2]:.3f}]")
    print(f"  末端: ({x_no[0]:.4f}, {x_no[1]:.4f}), 误差: {np.linalg.norm(x_no[:2]-target[:2]):.6f}")
    print(f"  膝关节距限位: {abs(q_no_null[1] - q_max[1]):.3f} rad")

    print(f"\n有零空间: q=[{q_with_null[0]:.3f}, {q_with_null[1]:.3f}, {q_with_null[2]:.3f}]")
    print(f"  末端: ({x_with[0]:.4f}, {x_with[1]:.4f}), 误差: {np.linalg.norm(x_with[:2]-target[:2]):.6f}")
    print(f"  膝关节距限位: {abs(q_with_null[1] - q_max[1]):.3f} rad")

    # 可操作性对比
    J_no = leg_jac_3dof(q_no_null)
    J_with = leg_jac_3dof(q_with_null)
    w_no = np.sqrt(max(np.linalg.det(J_no @ J_no.T), 1e-20))
    w_with = np.sqrt(max(np.linalg.det(J_with @ J_with.T), 1e-20))
    print(f"\n可操作性: 无零空间={w_no:.6f}, 有零空间={w_with:.6f}")

    print("\n✅ 冗余度优化验证完成！")

5. 奇异位形分析

6. 练习题