ykcheungab
4.9 KiB
独立参考力与反馈骨架
用途：当前代码契约。状态：离线可测试；未接设备、运动循环、界面或生产记录。架构问题与实施顺序见审查。
边界与单位
reference_force.py 定义 ReferenceForceSensor：由适配器建立连接、去皮和校准，单个采集线程拥有非阻塞 read() 与 close()；无新数据返回 None。异常、断开和设备生命周期由未来适配器处理，不能转成零力。
单轴适配器输出不可变的 NormalForceSample，force_n 为 N，压缩为正。FT 适配器输出 WrenchSample，三个力分量为 N、可选三个力矩分量为 N m，明确 frame。两者携带 source_id、calibration_id、有效性和接收时的 t_host_mono_ns。设备原生时钟以后另存，不能直接冒充主机时钟。
project_wrench() 必须显式给出 transducer→tool 的旋转、预期输入坐标系和 compression_sign。它验证正交性、行列式 +1，并复用 normal_force_n()：F_normal = compression_sign * (R @ F_sensor)[TOOL_AXIS]。单轴传感器的安装方向由其适配器标定；不得输入触觉算法的 raw 值。FT 安装于旋转台时，R 必须随姿态更新；固定工具端才可采用已验证的固定 R。
力投影不需要平移；转换不同力矩原点时还需力臂，本次未实现力矩变换。保留原始 wrench，不能把只投影法向力称作完整 wrench 变换。原 multipoint 的 parse_rotation/normal_force_n 导入保持兼容，算法只有一个实现。
控制输入和输出
force_control.py 的 ForceController.update() 消费已校准法向力、同步压入量和当前主机时间，返回 ForceDecision。depth_mm 是接触零点之后、向橡胶内部为正的压入量，范围为 [0,max_depth_mm]；它既不是几何高度，也不是 Z 轴绝对坐标。运动适配层必须用 PressAxis 的方向/零点及当次接触零点建立转换。
输出 velocity_mm_s 是压入速度建议，正值增加压入。样本、压入量、目标、误差、力变化率和故障原因均保留在结果中，可由未来记录器逐样本保存；当前不会自动写文件。配置中的增益量纲为：P mm/(N s)、I mm/(N s²)、D mm/N，全部显式输入，没有真实机器默认增益。
| 模式 | 行为 |
|---|---|
record_relaxation（默认） |
输出速度为零，记录力衰减和压入量。实际位置保持由执行器完成。 |
compensate |
误差驱动 PID 速度建议；橡胶松弛造成欠力时允许继续压入，始终受限。 |
导数作用于测量值，积分在输出受限且误差继续推动饱和时冻结。速度同时受最大速度及剩余压入行程/最大样本间隔约束。每次保持有最长时限，不无限追赶松弛。模型测试覆盖标准线性固体的松弛行为，只证明算法路径可用，不代表实际橡胶模型已识别。
故障与执行责任
无效/非有限数据、缺少身份、未来/过期时间戳、重复/倒序/过大采样间隔、传感器或校准身份改变、力幅值超限、压入越界、保持超时及数值溢出都会锁存零输出，需显式 reset() 开始新保持。校准时保留有符号力；超力检查使用绝对值，不取绝对值伪造正法向力。
调用者必须在每个控制周期推进检查。没有新样本时可用最后一帧调用以触发停止；该版本也将重复样本作为故障，因此控制更新频率应跟随参考采样，而独立看门狗负责无数据时停止。接触尚未建立时不能运行该保持策略。
零速度建议不等于停止已经发出的运动。未来执行器必须保证唯一运动所有权、控制器端看门狗、限速/加速度与行程保护、命令确认及实际回读；故障或取消时发停止并核实停止。不得将每个周期的建议堆进现有 GUI 命令队列，也不能声称 MotionPoller 的 100 ms 刷新提供确定实时周期。本次骨架没有调用 DLL 或绕过 MotionService。
后续接入验收
先做硬件禁用状态的“适配器→投影→策略→记录”链路：证明掉线、饱和、过期、时钟跳变与取消保留原因，实际执行反馈可追溯到建议；然后才接受授权范围内的台架测试。保持实验分别记录恒力跟踪误差、峰值力、额外压入量和固定位置衰减曲线。PID 参数、滤波延迟、采样率、去皮、重力补偿和机械方向均须按具体设备实测。
当前测试位于 test_force_control.py，覆盖非平凡旋转/反力符号、无效性、两种松弛模式、采样时间、身份变化、故障锁存、输出限幅、抗积分饱和及松弛模型。未连接的适配器与真实执行器不属于已验证能力。