机械臂示教与主动跟随技术方案

拖动阻力计算：
─────────────────────────────────────────────────────
谐波减速器基本参数：
├─ 减速比：i = 100
├─ 传动效率：η = 0.80（谐波减速器典型值）
├─ 关节额定输出扭矩：25N·m（J1）

从输出端反向拖动时的阻力：
─────────────────────────────────────────────────────
正向传动（电机→输出）：
├─ 输出扭矩 = 电机扭矩 × 减速比 × 效率
└─ T_out = T_motor × 100 × 0.80

反向传动（人工拖动→电机）：
├─ 反向效率 η_reverse = 1 - (1-η)/η ≈ 0.75（谐波减速器）
│ 注：谐波减速器反向效率较低，约0.70–0.80
├─ 人工拖动时，需要克服：
│ ├─ 电机转子惯量（折算到输出端）
│ ├─ 减速器摩擦力矩
│ └─ 电机线圈反电动势（如果不短路）
│
└─ 等效拖动阻力矩：
 T_drag = T_friction × i / η_reverse
 ≈ 0.05N·m（电机摩擦）× 100 / 0.75
 ≈ 6.7N·m（仅摩擦项）

实际感受（以J1为例）：
├─ 关节半径约 70mm（操作者手握位置）
├─ 切向力 = 6.7N·m / 0.07m ≈ 96N
└─ 相当于提起 10kg 重物的力 ⚠️ 极难操作

各关节拖动阻力估算：
─────────────────────────────────────────────────────
关节 减速比 电机摩擦 折算阻力矩 操作力（r=70mm）
J1 100 0.05N·m 6.7N·m 96N ← 极难
J2 100 0.05N·m 6.7N·m 96N ← 极难
J3 — 丝杠自锁 — 需要推力>200N ← 不可能
J4 10 0.03N·m 0.4N·m 6N ← 勉强
J5 10 0.03N·m 0.4N·m 6N ← 勉强
J6 10 0.03N·m 0.4N·m 6N ← 勉强

结论：J1/J2/J3 直接拖动完全不可行 
 J4/J5/J6 直接拖动勉强可以，但体验差 

谐波减速器对示教的特殊影响：
─────────────────────────────────────────────────────
特性1：零背隙（优点）
├─ 谐波减速器天然零背隙
├─ 示教时无"空程"感
└─ 示教精度高（无背隙误差）

特性2：高减速比（缺点）
├─ 减速比100：1
├─ 反向拖动阻力极大
└─ 直接拖动不可行

特性3：柔轮弹性（特殊）
├─ 谐波减速器柔轮有弹性变形
├─ 在大力矩下，输出轴有微小弹性偏转
├─ 示教时如果用力过大，会有"弹性感"
└─ 影响：示教位置与实际位置有微小偏差（<0.1°）

特性4：反向效率低（缺点）
├─ 谐波减速器反向效率约0.70–0.80
├─ 比行星减速器（0.90–0.95）低
└─ 反向拖动时，更多能量转化为热量

示教方案对比（从简单到复杂）：
─────────────────────────────────────────────────────
方案A：直接拖动（不可行）
├─ 原理：关节断电，人工拖动
├─ 问题：J1/J2/J3阻力极大，无法操作
└─ 推荐度：❌

方案B：遥控示教（推荐备选）
├─ 原理：操作者通过手柄/按钮控制各关节运动
├─ 优点：无需拖动，操作简单
├─ 缺点：
│ ├─ 无法感受末端与的接触力
│ ├─ 需要额外的遥控设备
│ └─ 示教精度依赖操作者技巧
└─ 推荐度：🟡 适合粗略示教

方案C：电流环零力矩控制（推荐主方案）
├─ 原理：实时检测关节电流，主动输出补偿力矩
│ 使关节对外呈现"零阻力"状态
├─ 优点：
│ ├─ 拖动力降低到 1–3N（接近无阻力）
│ ├─ 操作者可以感受末端接触力
│ └─ 示教精度高
├─ 缺点：
│ ├─ 需要精确的重力补偿
│ └─ 控制算法稍复杂
└─ 推荐度：✅ 推荐主方案

方案D：末端力传感器引导（进阶）
├─ 原理：末端力传感器检测操作者施加的力
│ 根据力的方向和大小控制机械臂运动
├─ 优点：最自然的拖动感受
├─ 缺点：
│ ├─ 需要额外的末端力传感器（已有）
│ ├─ 控制算法复杂（导纳控制）
│ └─ 对力传感器精度要求高
└─ 推荐度：🟡 进阶方案，能力允许时推荐

电流环零力矩控制原理：
─────────────────────────────────────────────────────
核心思想：
├─ 正常运行时：电机输出力矩 = 负载力矩（保持位置）
├─ 示教模式时：电机输出力矩 = 重力补偿力矩（仅补偿重力）
│ └─ 对外呈现：关节"失重"，人工拖动只需克服极小摩擦
│
└─ 实现方式：
 ├─ 关节电机切换到电流控制模式（力矩控制）
 ├─ 目标电流 = 重力补偿电流（不包含位置保持电流）
 └─ 人工拖动时，关节自由运动，无额外阻力

控制框图：
─────────────────────────────────────────────────────

示教模式下的控制框图：

[关节角度编码器] ──► [重力补偿计算] ──► [目标电流 I_gravity]
 ▼
 [电流环控制器]
 ▼
 [电机驱动器]
 ▼
 [电机输出力矩]
 ▼
 [关节运动]
 ▼
 [编码器反馈] ──► 循环

关键：目标电流只包含重力补偿，不包含位置保持
 人工拖动时，关节自由跟随，无额外阻力

class ZeroTorqueTeachMode:
 """
 零力矩示教模式控制器
 基于电流环实现主动跟随
 """

 def __init__(self, robot):
 self.robot = robot
 self.n_joints = 6

 # 机械臂参数（用于重力补偿）
 self.link_masses = [2.0, 1.5, 1.0, 0.5, 0.3, 0.2] # kg
 self.link_lengths = [0.28, 0.25, 0.0, 0.0, 0.15, 0.06] # m

 # 控制参数
 self.damping = [0.1, 0.1, 0.05, 0.05, 0.05, 0.05]
 # 阻尼系数：提供轻微阻尼，防止关节漂移

 # 状态
 self.is_teaching = False
 self.recorded_points = []

 def enter_teach_mode(self):
 """进入示教模式"""
 print("进入示教模式...")

 # Step 1：切换所有关节到电流控制模式
 for j in range(self.n_joints):
 self.robot.joints[j].set_control_mode('current')

 # Step 2：启动零力矩控制循环
 self.is_teaching = True
 self._start_control_loop()

 print("示教模式已激活，可以拖动机械臂")

 def exit_teach_mode(self):
 """退出示教模式"""
 self.is_teaching = False

 # 切换回位置控制模式
 for j in range(self.n_joints):
 self.robot.joints[j].set_control_mode('position')

 print("退出示教模式")

 def _control_loop(self):
 """
 零力矩控制循环（1000Hz）
 核心：每个控制周期计算并输出重力补偿电流
 """
 while self.is_teaching:
 # Step 1：读取当前关节角度
 q = self.robot.get_joint_angles() # [θ₁, θ₂, d₃, θ₄, θ₅, θ₆]
 dq = self.robot.get_joint_velocities() # 关节速度

 # Step 2：计算重力补偿力矩
 tau_gravity = self._compute_gravity_torque(q)

 # Step 3：计算阻尼力矩（轻微阻尼，防止漂移）
 tau_damping = [-self.damping[j] * dq[j] for j in range(self.n_joints)]

 # Step 4：总目标力矩
 tau_target = [tau_gravity[j] + tau_damping[j] 
 for j in range(self.n_joints)]

 # Step 5：转换为目标电流
 # I = τ / (Kt × 减速比 × 效率)
 I_target = self._torque_to_current(tau_target)

 # Step 6：发送电流指令
 self.robot.set_joint_currents(I_target)

 # 等待下一个控制周期（1ms）
 time.sleep(0.001)

 def _compute_gravity_torque(self, q):
 """
 计算各关节的重力补偿力矩
 使用简化的重力模型
 """
 tau = [0.0] * self.n_joints
 g = 9.81 # 重力加速度

 # J1（底座旋转，绕Z轴）：重力矩为0（Z轴旋转不受重力影响）
 tau[0] = 0.0

 # J2（肩旋转，绕Z轴）：同J1，重力矩为0
 tau[1] = 0.0

 # J3（Z轴升降）：需要支撑上方所有部件的重量
 # 推力 = 所有上方部件重量之和
 mass_above_j3 = sum(self.link_masses[3:]) # J4+J5+J6+末端
 tau[2] = mass_above_j3 * g # 单位：N（直线关节）

 # J4（手腕旋转，绕Z轴）：重力矩为0
 tau[3] = 0.0

 # J5（俯仰，绕Y轴）：需要支撑J6+末端的重量
 # 力矩 = 质量 × g × 力臂 × cos(俯仰角)
 theta5 = q[4] # J5当前角度
 theta6 = q[5] # J6当前角度

 # J6+末端的质心位置（相对于J5轴）
 mass_j6_end = self.link_masses[5] + 1.3 # J6自重 + 末端执行器
 L5 = self.link_lengths[4] # J5连杆长度

 # 重力力矩（简化：假设质心在J5连杆末端）
 tau[4] = mass_j6_end * g * L5 * np.cos(np.radians(theta5))

 # J6（俯仰，绕Y轴）：需要支撑末端执行器的重量
 theta_total = theta5 + theta6 # 总俯仰角
 mass_end = 1.3 # 末端执行器重量
 L6 = self.link_lengths[5] # J6连杆长度

 tau[5] = mass_end * g * L6 * np.cos(np.radians(theta_total))

 return tau

 def _torque_to_current(self, tau_list):
 """
 将力矩转换为电机电流
 I = τ / (Kt × 减速比 × 效率)
 """
 Kt = [0.1, 0.1, 0.05, 0.05, 0.05, 0.05] # 电机力矩常数（N·m/A）
 ratio = [100, 100, 1, 10, 10, 10] # 减速比（J3为丝杠，单独处理）
 eta = [0.80, 0.80, 0.90, 0.85, 0.85, 0.85] # 传动效率

 I_target = []
 for j in range(self.n_joints):
 if j == 2: # J3直线关节
 # 推力转换为电机电流（通过丝杠导程）
 lead = 0.005 # 丝杠导程5mm
 I = tau_list[j] / (Kt[j] * 2 * np.pi / lead * eta[j])
 else:
 I = tau_list[j] / (Kt[j] * ratio[j] * eta[j])
 I_target.append(I)

 return I_target

 def record_point(self):
 """记录当前位置为示教点"""
 q = self.robot.get_joint_angles()
 F = self.robot.get_end_force() # 末端力传感器读数

 point = {
 'joints': q.copy(),
 'force': F,
 'timestamp': time.time()
 }
 self.recorded_points.append(point)
 print(f"记录示教点 {len(self.recorded_points)}: {q}")
 return point

零力矩控制后的拖动力分析：
─────────────────────────────────────────────────────
理论拖动力（零力矩控制后）：
├─ 重力补偿后，关节只剩摩擦力
├─ 谐波减速器摩擦力矩：约 0.1–0.3N·m（输出端）
├─ 折算到操作者手部（r=70mm）：
│ └─ F = 0.2N·m / 0.07m ≈ 2.9N
└─ 相当于提起 300g 重物的力， 轻松操作

对比：
├─ 直接拖动（无控制）：约 96N（提起10kg）
├─ 零力矩控制后：约 1–3N（提起100–300g）
└─ 改善比例：约 30–50倍

实际感受：
├─ J1/J2：轻松旋转，几乎无阻力
├─ J3：需要克服丝杠摩擦，约5–10N推力（可接受）
├─ J4/J5/J6：极轻松，1–2N即可拖动
└─ 整体：类似拖动一个"失重"的机械臂

重力补偿的重要性：
─────────────────────────────────────────────────────
如果没有重力补偿：
├─ J5/J6俯仰时，末端重量（1.3kg）产生重力矩
├─ 重力矩 = 1.3 × 9.8 × 0.15 = 1.91N·m（J5）
├─ 零力矩控制不补偿重力 → 关节会在重力作用下自由下落
└─ 危险！机械臂会快速下坠 

重力补偿的目标：
├─ 在任意姿态下，关节输出力矩 = 重力矩
├─ 使关节在任意位置"悬停"（无需人工支撑）
└─ 操作者只需施加很小的力即可移动关节

各关节重力补偿分析：
─────────────────────────────────────────────────────
J1（底座旋转，绕Z轴）：
├─ 旋转轴：Z轴（垂直）
├─ 重力方向：Z轴（垂直向下）
├─ 重力矩：0（旋转轴与重力方向平行）
└─ 补偿：不需要 

J2（肩旋转，绕Z轴）：
├─ 旋转轴：Z轴（垂直）
├─ 重力矩：0（同J1）
└─ 补偿：不需要 

J3（Z轴升降）：
├─ 运动方向：Z轴（垂直）
├─ 需要支撑上方所有部件重量
├─ 重力 = (J4+J5+J6+末端)质量 × g
│ ≈ (0.5+0.3+0.2+1.3) × 9.8 ≈ 22.5N
├─ 补偿：输出22.5N的推力（向上）
└─ 注意：J3是直线关节，补偿的是推力而非力矩

J4（手腕旋转，绕Z轴）：
├─ 旋转轴：Z轴（垂直）
├─ 重力矩：0（同J1）
└─ 补偿：不需要 

J5（俯仰，绕Y轴）：
├─ 旋转轴：Y轴（水平）
├─ 重力矩随俯仰角变化
├─ τ_J5 = m_J6_end × g × L5 × cos(θ₅)
│ = (0.2+1.3) × 9.8 × 0.15 × cos(θ₅)
│ = 2.21 × cos(θ₅) N·m
├─ θ₅=0°时最大：2.21N·m
├─ θ₅=90°时为0
└─ 补偿：实时计算，随θ₅变化

J6（俯仰，绕Y轴）：
├─ 旋转轴：Y轴（水平）
├─ 重力矩随总俯仰角变化
├─ τ_J6 = m_end × g × L6 × cos(θ₅+θ₆)
│ = 1.3 × 9.8 × 0.06 × cos(θ₅+θ₆)
│ = 0.76 × cos(θ₅+θ₆) N·m
└─ 补偿：实时计算，随θ₅+θ₆变化

重力补偿精度要求：
├─ 补偿误差 < 10%（允许残余力矩）
├─ 残余力矩 < 0.2N·m（折算到操作力 < 3N）
└─ 需要精确测量各连杆质量和质心位置

重力补偿标定方法：
─────────────────────────────────────────────────────
标定目标：精确测量各连杆质量和质心位置

方法1：离线测量（推荐）
├─ 拆卸各连杆，用精密天平称重
├─ 测量质心位置（悬挂法或CAD计算）
├─ 输入到重力补偿模型
└─ 精度：±5%（足够）

方法2：在线辨识（进阶）
├─ 机械臂在多个姿态下静止
├─ 记录各关节的保持力矩（电流）
├─ 通过最小二乘法辨识质量参数
└─ 精度：±2%（更精确）

标定验证：
├─ 进入零力矩模式
├─ 将机械臂移动到多个姿态
├─ 松手后，机械臂应保持静止（不下坠）
├─ 如果某个关节缓慢下坠：增大该关节补偿量
└─ 如果某个关节缓慢上抬：减小该关节补偿量

示教前准备流程：
─────────────────────────────────────────────────────
Step 1：环境准备（约5分钟）
├─ 确认已清洁（示教时不需要清洁液）
├─ 确认位置固定（不会移动）
├─ 确认机器人底盘已锁定（刹车锁定）
├─ 确认末端已安装对应清洁模组
│ └─ 内壁示教：安装内壁清洁模组
│ └─ 外壁示教：安装外壁清洁模组
└─ 确认力传感器已标定（零点校准）

Step 2：系统检查（约2分钟）
├─ 上电，等待系统自检完成
├─ 检查各关节编码器读数是否正常
├─ 检查力传感器读数是否接近0N（悬空状态）
├─ 检查CAN总线通信是否正常
└─ 确认急停按钮可用

Step 3：重力补偿验证（约1分钟）
├─ 进入零力矩模式
├─ 将机械臂移动到几个典型姿态
├─ 松手，确认机械臂保持静止（不下坠）
└─ 如果下坠，重新标定重力补偿参数

Step 4：示教模式激活
├─ 按下"示教模式"按钮
├─ 系统提示"示教模式已激活"
└─ 操作者可以开始拖动机械臂

示教操作流程（以内壁清洁为例）：
─────────────────────────────────────────────────────
阶段1：示教起始点（接近点）
├─ 操作者拖动机械臂到上方约100mm处
├─ 确认末端朝向正确（J4调整工具朝向）
├─ 按下"记录点"按钮，记录点P0（接近点）
└─ 系统提示"已记录点0：接近点"

阶段2：示教接触点
├─ 操作者缓慢将末端靠近内壁
├─ 感受到轻微接触（力传感器读数约2–3N）时停止
├─ 确认擦拭头贴合内壁（目视检查）
├─ 按下"记录点"按钮，记录点P1（接触点）
└─ 系统提示"已记录点1：接触点，接触力=2.5N"

阶段3：示教清洁路径点（3–6个点）
├─ 操作者沿内壁缓慢移动末端
├─ 每移动约50–80mm，按下"记录点"
├─ 确保每个路径点末端都贴合内壁
├─ 路径点数量：
│ ├─ 内壁清洁：5–6个点（螺旋路径）
│ ├─ 外壁清洁：4–5个点（竖向路径）
│ └─ 座垫清洁：3–4个点（环形路径）
└─ 系统实时显示已记录点数和接触力

阶段4：示教结束点
├─ 操作者将末端移动到清洁结束位置
├─ 按下"记录点"，记录终止接触点
└─ 系统提示"已记录终止接触点"

阶段5：示教撤离点
├─ 操作者将末端从表面撤离（约100mm）
├─ 按下"记录点"，记录撤离点
└─ 系统提示"示教完成，共记录X个点"

阶段6：示教验证
├─ 按下"预览轨迹"按钮
├─ 系统在空中（不接触）回放轨迹
├─ 操作者目视确认轨迹合理
└─ 确认后保存示教数据

不同清洁任务的示教要点：
─────────────────────────────────────────────────────
内壁清洁示教要点：
├─ 路径形状：螺旋向下（从上到下）
├─ 关键点位置：
│ ├─ P0：上方100mm（接近点）
│ ├─ P1：内壁上部（H=380mm，接触点）
│ ├─ P2–P5：螺旋向下的中间点
│ └─ P6：内壁下部（H=120mm，终止点）
├─ 注意：每个点都要确保擦拭头贴合内壁
├─ 注意：J4角度要保证工具朝向轴心
└─ 注意：J5/J6角度要适应内壁曲面角度

外壁清洁示教要点（有扶手侧）：
├─ 路径形状：竖向往复（从上到下）
├─ 关键点位置：
│ ├─ P0：侧方100mm（接近点）
│ ├─ P1：外壁上部（H=420mm，接触点）
│ ├─ P2–P4：竖向向下的中间点
│ └─ P5：外壁下部（H=50mm，终止点）
├─ 注意：J5+J6需要折叠进入有限空间
├─ 注意：折叠角度要避免碰到扶手
└─ 注意：每个点都要确保擦拭头贴合外壁

座垫清洁示教要点：
├─ 路径形状：环形（绕座圈一圈）
├─ 关键点位置：
│ ├─ P0：座圈上方100mm（接近点）
│ ├─ P1：座圈前方（接触点）
│ ├─ P2–P4：沿座圈的中间点（90°/180°/270°）
│ └─ P5：回到起始位置（终止点）
├─ 注意：J5/J6要保证擦拭头垂直向下
└─ 注意：接触力要均匀（5–8N）

安全注意事项（必须遵守）：
─────────────────────────────────────────────────────
🔴 安全红线（违反立即停止）：
├─ 禁止在零力矩模式下快速甩动机械臂
│ └─ 原因：惯性力可能损坏关节或伤人
├─ 禁止在零力矩模式下离开机械臂（无人看管）
│ └─ 原因：重力补偿误差可能导致缓慢下坠
├─ 禁止在示教时强行超过关节限位
│ └─ 原因：超限位会触发保护停机
└─ 禁止在示教时用力撞击
 └─ 原因：冲击力可能损坏力传感器

🟡 操作注意事项：
├─ 示教时保持缓慢匀速移动（<50mm/s）
├─ 每次记录点前，确认末端稳定（不晃动）
├─ 接触时，接触力控制在 3–8N
├─ 示教过程中，保持一只手在急停按钮附近
└─ 示教完成后，立即退出零力矩模式

示教质量注意事项：
─────────────────────────────────────────────────────
注意1：接触点必须真实贴合表面
├─ 不能"悬空示教"（末端离表面3–5mm）
├─ 验证方法：力传感器读数 > 2N
└─ 如果读数<2N，继续靠近直到接触

注意2：路径点间距要均匀
├─ 推荐间距：50–80mm
├─ 间距过大（>100mm）：插值轨迹可能偏离
├─ 间距过小（<30mm）：示教时间长，数据冗余
└─ 系统会实时显示当前点与上一点的距离

注意3：J4朝向要正确
├─ 内壁清洁：工具朝向轴心（J4 = θ_arm + 180°）
├─ 外壁清洁：工具背向轴心（J4 = θ_arm）
├─ 座垫清洁：工具垂直向下
└─ 系统会提示当前J4角度是否合理

注意4：避免奇异点姿态
├─ 奇异点：J5/J6接近0°（俯仰链完全伸直）
├─ 在奇异点附近，关节速度会突变
├─ 示教时避免在奇异点附近记录路径点
└─ 系统会在接近奇异点时发出警告

注意5：示教环境与回放环境一致
├─ 示教时的型号 = 回放时的型号
├─ 示教时的底盘位置 = 回放时的底盘位置（±5mm）
└─ 如果型号不同，需要重新示教

示教数据采集标准：
─────────────────────────────────────────────────────
必须采集的数据：
├─ 关节角度：[θ₁, θ₂, d₃, θ₄, θ₅, θ₆]（6个值）
├─ 接触力：F_contact（力传感器读数，重力补偿后）
├─ 接触状态：contact（bool，是否接触）
└─ 点类型：type（接近点/接触点/路径点/撤离点）

推荐采集的数据：
├─ 末端笛卡尔位置：[x, y, z]（通过正运动学计算）
├─ 末端姿态：[roll, pitch, yaw]
└─ 记录时间戳：timestamp

数据格式示例：

{
 "teach_id": "inner_wall_v1",
 "task_type": "inner_wall",
 "toilet_model": "TOTO_CS325",
 "teach_date": "2026-04-08",
 "operator": "张工",
 "points": [
 {
 "index": 0,
 "type": "approach",
 "joints": [15.2, 45.3, 380.0, 195.5, -10.2, -5.1],
 "end_pos": [180.5, 120.3, 480.0],
 "force": 0.0,
 "contact": false
 },
 {
 "index": 1,
 "type": "contact_start",
 "joints": [15.2, 45.3, 350.0, 195.5, -15.3, -8.2],
 "end_pos": [180.5, 120.3, 400.0],
 "force": 3.2,
 "contact": true
 }
 // ... 更多点
 ],
 "quality_check": {
 "min_force": 2.1,
 "max_force": 8.5,
 "avg_force": 5.3,
 "point_spacing_avg": 65.2,
 "passed": true
 }
}

def validate_teach_data(teach_data):
 """
 示教数据质量自动检查
 返回：(passed, warnings, errors)
 """
 warnings = []
 errors = []

 points = teach_data['points']
 n_points = len(points)

 # 检查1：点数量
 if n_points < 5:
 errors.append(f"示教点数量不足：{n_points}个（最少5个）")
 elif n_points > 15:
 warnings.append(f"示教点数量较多：{n_points}个（建议5–10个）")

 # 检查2：接触点的接触力
 contact_points = [p for p in points if p['contact']]
 for p in contact_points:
 if p['force'] < 2.0:
 warnings.append(
 f"点{p['index']}接触力偏低：{p['force']:.1f}N（建议>2N）"
 )
 if p['force'] > 12.0:
 errors.append(
 f"点{p['index']}接触力过大：{p['force']:.1f}N（限制<12N）"
 )

 # 检查3：路径点间距
 for i in range(1, n_points):
 p_curr = points[i]['joints']
 p_prev = points[i-1]['joints']
 pos_curr = forward_kinematics(p_curr)
 pos_prev = forward_kinematics(p_prev)
 dist = np.linalg.norm(np.array(pos_curr) - np.array(pos_prev))

 if dist > 100:
 warnings.append(
 f"点{i-1}到点{i}间距过大：{dist:.1f}mm（建议<100mm）"
 )
 elif dist < 20:
 warnings.append(
 f"点{i-1}到点{i}间距过小：{dist:.1f}mm（建议>20mm）"
 )

 # 检查4：J4朝向合理性
 for p in contact_points:
 theta_arm = p['joints'][0] + p['joints'][1]
 theta4 = p['joints'][3]
 if teach_data['task_type'] == 'inner_wall':
 expected_j4 = theta_arm + 180
 diff = abs(theta4 - expected_j4) % 360
 if diff > 20:
 warnings.append(
 f"点{p['index']} J4朝向可能不正确：{theta4:.1f}°"
 )

 # 检查5：奇异点检测
 for p in points:
 theta5 = p['joints'][4]
 theta6 = p['joints'][5]
 if abs(theta5) < 5 and abs(theta6) < 5:
 warnings.append(
 f"点{p['index']}接近奇异点"
 f"（J5={theta5:.1f}°, J6={theta6:.1f}°）"
 )

 passed = len(errors) == 0
 return passed, warnings, errors

离线验证流程：
─────────────────────────────────────────────────────
Step 1：自动质量检查
├─ 运行 validate_teach_data()
├─ 显示所有警告和错误
├─ 有错误时：必须重新示教
└─ 有警告时：操作者确认是否继续

Step 2：轨迹可视化
├─ 通过正运动学计算所有示教点的末端位置
├─ 在3D视图中显示轨迹
├─ 操作者目视确认轨迹合理
└─ 确认轨迹覆盖了目标清洁面

Step 3：空中预演（不接触）
├─ 机械臂在空中回放示教轨迹（末端抬高100mm）
├─ 操作者观察运动是否平滑
├─ 确认无异常速度突变或抖动
└─ 确认各关节运动范围合理

Step 4：慢速接触验证
├─ 以50%速度回放轨迹（接触）
├─ 操作者观察末端是否正确贴合
├─ 检查接触力是否在5–10N范围内
└─ 确认无碰撞或过度压入

在线验证流程：
─────────────────────────────────────────────────────
Step 1：首次完整回放
├─ 以100%速度回放示教轨迹
├─ 记录实际接触力曲线
├─ 对比示教时的接触力曲线
└─ 偏差>3N时，需要重新示教

Step 2：重复性验证
├─ 连续回放3次
├─ 记录每次的接触力曲线
├─ 3次曲线的偏差应<1N
└─ 偏差过大说明示教数据不稳定

Step 3：清洁效果验证
├─ 在表面涂抹染色剂（模拟污渍）
├─ 执行完整清洁回放
├─ 检查清洁覆盖率
└─ 覆盖率<90%时，需要增加示教点

最终推荐方案：零力矩控制 + 遥控示教（双模式）
─────────────────────────────────────────────────────
主模式：零力矩控制拖动示教
├─ 适用场景：精细路径示教（内壁/外壁/座垫）
├─ 优点：可以感受接触力，示教精度高
├─ 实现：电流环零力矩控制 + 重力补偿
└─ 拖动力：1–3N（轻松操作）

辅助模式：遥控示教（手柄控制）
├─ 适用场景：粗略定位（接近点/撤离点）
├─ 优点：无需拖动，快速定位
├─ 实现：手柄按钮控制各关节运动
└─ 精度：±5mm（足够用于接近/撤离点）

两种模式切换：
├─ 按下"拖动模式"按钮 → 零力矩控制
├─ 按下"遥控模式"按钮 → 手柄控制
└─ 任何时候按急停 → 立即停止并锁定

实施优先级：
├─ P0（必须）：零力矩控制 + 重力补偿
├─ P1（推荐）：示教数据质量自动检查
├─ P2（推荐）：空中预演功能
└─ P3（可选）：遥控示教辅助模式