✨ 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:
@@ -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()
|
||||
|
||||
Reference in New Issue
Block a user