feat(*): 完善 FANUC J519 闭环、MoveJoint 与现场抓包验证

* 划分 J519 发送循环与稠密轨迹循环职责边界,
  FanucJ519Client 负责 UDP 周期发送,
  FanucControllerRuntime 按轨迹时间更新下一帧命令
* 执行时将规划输出 rad 转为 J519 deg 目标,
  并按 speed_ratio 调整 8ms 发送时间尺度
* 补齐 accept_cmd/received_cmd/sysrdy/rbt_inmotion
  状态位解析与启动前闭环检查
* MoveJoint 改为关节空间直线 + smoothstep 进度
  的临时 PTP 稠密轨迹,按 status=15 运动窗口复现
* 新增 UTTC 2026-04-28 三份抓包 golden tests,
  覆盖 0.5/0.7/1.0 speed_ratio 下的 J519 命令、
  IO 脉冲与响应滞后
* 状态通道补充超时重连策略与退避逻辑
* TCP 10012 命令响应统一检查 result_code
* 状态页扩展 J519 状态位与快照诊断信息
* 新增 docs/fanuc-field-runtime-workflow.md 现场工作流
* 补充 LR Mate 200iD 模型、RobotConfig.json 与 workpiece
This commit is contained in:
2026-04-29 01:03:18 +08:00
parent 0292e077ff
commit 0724efebed
25 changed files with 1986 additions and 60 deletions

View File

@@ -337,8 +337,15 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.ExecuteTrajectory(CreateImmediateMoveResult(), jointPositions);
var robot = RequireActiveRobot();
EnsureRuntimeEnabled();
var currentJointPositions = _runtime.GetJointPositions();
EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions));
EnsureJointVector(jointPositions, robot.DegreesOfFreedom, nameof(jointPositions));
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, jointPositions, speedRatio);
_runtime.ExecuteTrajectory(moveResult, jointPositions);
}
}
@@ -568,7 +575,30 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
}
/// <summary>
/// 构造 MoveJoint 直达运行时所需的最小合法轨迹结果
/// 校验关节向量与当前机器人自由度一致,且所有值都是有限数值
/// </summary>
/// <param name="joints">待校验关节向量,单位为弧度。</param>
/// <param name="expectedCount">期望自由度。</param>
/// <param name="paramName">调用方参数名。</param>
private static void EnsureJointVector(IReadOnlyList<double> joints, int expectedCount, string paramName)
{
if (joints.Count != expectedCount)
{
throw new ArgumentException($"关节数量必须为 {expectedCount}。", paramName);
}
for (var index = 0; index < joints.Count; index++)
{
var value = joints[index];
if (double.IsNaN(value) || double.IsInfinity(value))
{
throw new ArgumentOutOfRangeException(paramName, $"第 {index} 个关节值必须是有限数值。");
}
}
}
/// <summary>
/// 构造无需稠密轨迹的最小合法结果,供仍需单点状态更新的兼容路径使用。
/// </summary>
/// <returns>可立即执行的轨迹结果。</returns>
private static TrajectoryResult CreateImmediateMoveResult()