控制模式设计

6 种模式对应 6 组 PD 增益，设计思路：

模式	设计目标	为什么这样设
idle	待机省电，允许手动推	kp 极低，不会跟人”较劲”
gravity_comp	零力示教，手拖机械臂	前馈抵消重力，kp≈0 让人能推动
moving	空载轨迹跟踪	中等 kp/kd，兼顾响应和稳定
loaded	带箱子运动	更高 kp，克服箱子惯量，减少静差
teach_pendant	实时跟随示教器	响应快，但不要振荡
teach_drag	手拖录制轨迹	kp≈0，完全靠前馈

为什么有 6 组而不是调一组？ 不同场景的物理条件差异太大——空载和带负载需要的刚性完全不同，一组参数没法同时满足。

摩擦力模型为什么用 tanh？ 库伦摩擦在零速时有符号跳变，直接用 sign(dq) 会让力矩在零速附近剧烈振荡。tanh(α·dq) 在零速附近平滑过渡，消除了这个不稳定因素。