yunxiao.zhu
b1710e5d01
* 将预捕获 alpha 数据表替换为解析式 7 阶平滑点到点时间律 s(u)=35u⁴-84u⁵+70u⁶-20u⁷，形状系数按 1~3 阶导数最大值重算 * 新增离散限位校验：按真实 8ms 采样点反算速度/加速度/jerk， 不满足时自动拉长总时长后重采样，最多迭代 10000 次 * 实发轨迹落盘：ActualSendJointTraj.txt（角度制）、 ActualSendJerkStats.txt（点间跃度统计），按时间目录归档 * J519 AcceptsCommand 门控：只有机器人就绪时才发送下一帧， 减少无效下发；状态日志附带最近发送目标关节轴 * FanucControllerRuntime 构造函数改为必选 ILogger 注入， 确保 DI 解析时稳定拿到日志实例 * LegacyHttpApiController 移除已废弃的 ConnectServer 调用， EnableRobot 参数从 2 改为 4 * 新增跃度报警分析文档和六轴限值表，补充反馈远离拒绝测试 Co-authored-by: Copilot <copilot@github.com>
869 lines
36 KiB
C#
869 lines
36 KiB
C#
using Flyshot.Core.Domain;
|
||
using Flyshot.ControllerClientCompat;
|
||
using Flyshot.Core.Config;
|
||
using Flyshot.Runtime.Fanuc;
|
||
using Flyshot.Runtime.Fanuc.Protocol;
|
||
using System.Reflection;
|
||
|
||
namespace Flyshot.Core.Tests;
|
||
|
||
/// <summary>
|
||
/// 验证 FANUC 控制器运行在稠密轨迹流式执行与 IO 触发上的行为。
|
||
/// </summary>
|
||
public sealed class FanucControllerRuntimeDenseTests
|
||
{
|
||
private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
|
||
private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
|
||
private const double SmoothPtpJerkShapeCoefficient = 52.5;
|
||
|
||
/// <summary>
|
||
/// 验证真机 J519 发送按 8ms 实发周期、speed_ratio 轨迹时间步进，并输出角度制目标。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_ResamplesBySpeedRatioAndConvertsRadiansToDegrees()
|
||
{
|
||
using var commandClient = new FanucCommandClient();
|
||
using var stateClient = new FanucStateClient();
|
||
using var j519Client = new FanucJ519Client();
|
||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||
j519Client.EnableCommandHistoryForTests();
|
||
ForceRealModeEnabled(runtime, speedRatio: 0.5);
|
||
|
||
var denseTrajectory = new[]
|
||
{
|
||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||
new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||
};
|
||
|
||
var result = new TrajectoryResult(
|
||
programName: "demo",
|
||
method: PlanningMethod.Icsp,
|
||
isValid: true,
|
||
duration: TimeSpan.FromSeconds(0.016),
|
||
shotEvents: Array.Empty<ShotEvent>(),
|
||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||
failureReason: null,
|
||
usedCache: false,
|
||
originalWaypointCount: 4,
|
||
plannedWaypointCount: 4,
|
||
denseJointTrajectory: denseTrajectory);
|
||
|
||
runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||
WaitUntilIdle(runtime);
|
||
|
||
var commands = j519Client.GetCommandHistoryForTests();
|
||
Assert.Equal(5, commands.Count);
|
||
Assert.All(commands, static command => Assert.Equal(0u, command.Sequence));
|
||
Assert.Equal([0.0, 45.0, 90.0, 135.0, 180.0], commands.Select(static command => command.TargetJoints[0]));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证真机稠密发送会把实际下发给机器人的整条点位与点间跃度按时间目录落盘。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_WritesActualSentPointsAndJerkStats()
|
||
{
|
||
using var commandClient = new FanucCommandClient();
|
||
using var stateClient = new FanucStateClient();
|
||
using var j519Client = new FanucJ519Client();
|
||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
var programName = $"dense-send-{Guid.NewGuid():N}";
|
||
var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", programName);
|
||
|
||
try
|
||
{
|
||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||
j519Client.EnableCommandHistoryForTests();
|
||
ForceRealModeEnabled(runtime, speedRatio: 0.5);
|
||
|
||
var denseTrajectory = new[]
|
||
{
|
||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||
new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||
};
|
||
|
||
var result = new TrajectoryResult(
|
||
programName: programName,
|
||
method: PlanningMethod.Icsp,
|
||
isValid: true,
|
||
duration: TimeSpan.FromSeconds(0.016),
|
||
shotEvents: Array.Empty<ShotEvent>(),
|
||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||
failureReason: null,
|
||
usedCache: false,
|
||
originalWaypointCount: 4,
|
||
plannedWaypointCount: 4,
|
||
denseJointTrajectory: denseTrajectory);
|
||
|
||
runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||
WaitUntilIdle(runtime);
|
||
|
||
var commands = j519Client.GetCommandHistoryForTests();
|
||
var runDir = GetSingleDenseSendRunDirectory(outputRoot);
|
||
var pointsPath = Path.Combine(runDir, "ActualSendJointTraj.txt");
|
||
var jerkPath = Path.Combine(runDir, "ActualSendJerkStats.txt");
|
||
|
||
Assert.True(File.Exists(pointsPath));
|
||
Assert.True(File.Exists(jerkPath));
|
||
|
||
var pointLines = File.ReadAllLines(pointsPath);
|
||
var jerkLines = File.ReadAllLines(jerkPath);
|
||
Assert.Equal(commands.Count, pointLines.Length);
|
||
Assert.Equal(Math.Max(0, commands.Count - 1), jerkLines.Length);
|
||
|
||
var firstColumns = ParseColumns(pointLines[0]);
|
||
var lastColumns = ParseColumns(pointLines[^1]);
|
||
Assert.Equal(9, firstColumns.Length);
|
||
Assert.Equal(9, lastColumns.Length);
|
||
Assert.Equal(0.0, firstColumns[0], precision: 6);
|
||
Assert.Equal(180.0, lastColumns[1], precision: 6);
|
||
|
||
var firstJerkColumns = ParseColumns(jerkLines[0]);
|
||
Assert.Equal(10, firstJerkColumns.Length);
|
||
Assert.Equal(0.0, firstJerkColumns[0], precision: 6);
|
||
Assert.Equal(0.004, firstJerkColumns[2], precision: 6);
|
||
}
|
||
finally
|
||
{
|
||
if (Directory.Exists(outputRoot))
|
||
{
|
||
Directory.Delete(outputRoot, recursive: true);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。
|
||
/// </summary>
|
||
[Theory]
|
||
[InlineData(1.0)]
|
||
[InlineData(0.7)]
|
||
[InlineData(0.5)]
|
||
public void MoveJoint_RealMode_GeneratesTemporaryPtpTrajectoryAndResamplesBySpeedRatio(double speedRatio)
|
||
{
|
||
using var commandClient = new FanucCommandClient();
|
||
using var stateClient = new FanucStateClient();
|
||
using var j519Client = new FanucJ519Client();
|
||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||
var service = CreateCompatService(runtime);
|
||
var startJoints = new[]
|
||
{
|
||
1.056731,
|
||
0.011664811,
|
||
-0.017892333,
|
||
-0.01516874,
|
||
0.021492079,
|
||
0.009567846
|
||
};
|
||
var targetJoints = new[]
|
||
{
|
||
0.8532358,
|
||
0.03837953,
|
||
-0.19235304,
|
||
0.0071595116,
|
||
0.109054826,
|
||
0.040055145
|
||
};
|
||
|
||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||
j519Client.EnableCommandHistoryForTests();
|
||
ForceRealModeEnabled(runtime, speedRatio);
|
||
SetPrivateField(runtime, "_jointPositions", startJoints);
|
||
|
||
var options = new ControllerClientCompatOptions
|
||
{
|
||
ConfigRoot = TestRobotFactory.GetConfigRoot()
|
||
};
|
||
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
|
||
.LoadProfile("FANUC_LR_Mate_200iD", accLimitScale: 1.0, jerkLimitScale: 1.0);
|
||
var expectedResult = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio);
|
||
|
||
service.MoveJoint(targetJoints);
|
||
WaitUntilIdle(runtime);
|
||
|
||
var commands = j519Client.GetCommandHistoryForTests();
|
||
Assert.Equal(expectedResult.DenseJointTrajectory!.Count, commands.Count);
|
||
AssertJointDegreesEqual(startJoints, commands[0].TargetJoints);
|
||
AssertJointDegreesEqual(targetJoints, commands[^1].TargetJoints);
|
||
|
||
var middleAlpha = ComputeLineAlpha(commands[commands.Count / 2].TargetJoints, startJoints, targetJoints);
|
||
Assert.InRange(middleAlpha, 0.45, 0.55);
|
||
|
||
var earlyAlpha = ComputeLineAlpha(commands[Math.Min(5, commands.Count - 1)].TargetJoints, startJoints, targetJoints);
|
||
Assert.InRange(earlyAlpha, 0.0, 0.02);
|
||
}
|
||
|
||
[Fact]
|
||
public void MoveJointTrajectoryGenerator_LowerSpeedUsesMoreSamplesWithoutFixedCountContract()
|
||
{
|
||
var robot = CreateMoveJointReferenceRobotProfile();
|
||
var startJoints = new[] { 1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846 };
|
||
var targetJoints = new[] { 0.8532358, 0.03837953, -0.19235304, 0.0071595116, 0.109054826, 0.040055145 };
|
||
|
||
var fullSpeed = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||
var speed07 = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
|
||
var speed05 = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.5);
|
||
|
||
Assert.True(speed07.DenseJointTrajectory!.Count > fullSpeed.DenseJointTrajectory!.Count);
|
||
Assert.True(speed05.DenseJointTrajectory!.Count > speed07.DenseJointTrajectory!.Count);
|
||
Assert.True(fullSpeed.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
|
||
Assert.True(speed07.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
|
||
Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
|
||
}
|
||
|
||
[Fact]
|
||
public void MoveJoint_RealMode_LeavesFinalTargetForHoldStreaming()
|
||
{
|
||
using var commandClient = new FanucCommandClient();
|
||
using var stateClient = new FanucStateClient();
|
||
using var j519Client = new FanucJ519Client();
|
||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||
var service = CreateCompatService(runtime);
|
||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||
var targetJoints = new[] { 0.2, -0.1, 0.05, 0.0, 0.0, 0.1 };
|
||
|
||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||
SetPrivateField(runtime, "_jointPositions", startJoints);
|
||
|
||
service.MoveJoint(targetJoints);
|
||
WaitUntilIdle(runtime);
|
||
|
||
var currentCommand = j519Client.GetCurrentCommand();
|
||
Assert.NotNull(currentCommand);
|
||
AssertJointDegreesEqual(targetJoints, currentCommand.TargetJoints);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证 speed_ratio=0 时不会启动无法推进轨迹时间的后台发送任务。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_RejectsZeroSpeedRatio()
|
||
{
|
||
using var commandClient = new FanucCommandClient();
|
||
using var stateClient = new FanucStateClient();
|
||
using var j519Client = new FanucJ519Client();
|
||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||
ForceRealModeEnabled(runtime, speedRatio: 0.0);
|
||
|
||
var denseTrajectory = new[]
|
||
{
|
||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||
};
|
||
|
||
var result = new TrajectoryResult(
|
||
programName: "demo",
|
||
method: PlanningMethod.Icsp,
|
||
isValid: true,
|
||
duration: TimeSpan.FromSeconds(0.016),
|
||
shotEvents: Array.Empty<ShotEvent>(),
|
||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||
failureReason: null,
|
||
usedCache: false,
|
||
originalWaypointCount: 4,
|
||
plannedWaypointCount: 4,
|
||
denseJointTrajectory: denseTrajectory);
|
||
|
||
var exception = Assert.Throws<InvalidOperationException>(
|
||
() => runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||
Assert.Contains("Speed ratio", exception.Message, StringComparison.OrdinalIgnoreCase);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证真机模式下若 J519 响应明确显示伺服侧未就绪，则拒绝启动稠密轨迹发送。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_RejectsNotReadyJ519Status()
|
||
{
|
||
using var commandClient = new FanucCommandClient();
|
||
using var stateClient = new FanucStateClient();
|
||
using var j519Client = new FanucJ519Client();
|
||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||
SetLatestJ519Response(j519Client, status: 0b0011);
|
||
|
||
var result = CreateDenseResult(
|
||
[
|
||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||
],
|
||
durationSeconds: 0.008);
|
||
|
||
var exception = Assert.Throws<InvalidOperationException>(
|
||
() => runtime.ExecuteTrajectory(result, [Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||
|
||
Assert.Contains("J519 status is not ready", exception.Message);
|
||
Assert.Contains("sysrdy=False", exception.Message);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证控制器快照暴露最近一次 J519 响应中的四个状态位，便于状态页和诊断接口显示。
|
||
/// </summary>
|
||
[Fact]
|
||
public void GetSnapshot_RealMode_IncludesLatestJ519StatusBits()
|
||
{
|
||
using var commandClient = new FanucCommandClient();
|
||
using var stateClient = new FanucStateClient();
|
||
using var j519Client = new FanucJ519Client();
|
||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||
SetLatestJ519Response(j519Client, status: 0b0111);
|
||
|
||
var snapshot = runtime.GetSnapshot();
|
||
|
||
Assert.Equal((byte)0b0111, snapshot.J519Status);
|
||
Assert.Equal(10u, snapshot.J519Sequence);
|
||
Assert.True(snapshot.J519AcceptsCommand);
|
||
Assert.True(snapshot.J519ReceivedCommand);
|
||
Assert.True(snapshot.J519SystemReady);
|
||
Assert.False(snapshot.J519RobotInMotion);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证飞拍 IO 脉冲按轨迹时间轴嵌入 J519 命令，并在保持周期后用同一 mask 清零。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_EmbedsIoPulseOnTrajectoryTimeline()
|
||
{
|
||
using var commandClient = new FanucCommandClient();
|
||
using var stateClient = new FanucStateClient();
|
||
using var j519Client = new FanucJ519Client();
|
||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||
j519Client.EnableCommandHistoryForTests();
|
||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||
|
||
var denseTrajectory = new[]
|
||
{
|
||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||
new[] { 0.024, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||
};
|
||
|
||
var result = new TrajectoryResult(
|
||
programName: "demo",
|
||
method: PlanningMethod.Icsp,
|
||
isValid: true,
|
||
duration: TimeSpan.FromSeconds(0.024),
|
||
shotEvents: Array.Empty<ShotEvent>(),
|
||
triggerTimeline:
|
||
[
|
||
new TrajectoryDoEvent(
|
||
waypointIndex: 1,
|
||
triggerTime: 0.008,
|
||
offsetCycles: 0,
|
||
holdCycles: 2,
|
||
addressGroup: new IoAddressGroup([1, 3]))
|
||
],
|
||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||
failureReason: null,
|
||
usedCache: false,
|
||
originalWaypointCount: 4,
|
||
plannedWaypointCount: 4,
|
||
denseJointTrajectory: denseTrajectory);
|
||
|
||
runtime.ExecuteTrajectory(result, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||
WaitUntilIdle(runtime);
|
||
|
||
var commands = j519Client.GetCommandHistoryForTests();
|
||
Assert.Equal(4, commands.Count);
|
||
Assert.Equal([(ushort)0, (ushort)10, (ushort)10, (ushort)10], commands.Select(static command => command.WriteIoMask));
|
||
Assert.Equal([(ushort)0, (ushort)10, (ushort)10, (ushort)0], commands.Select(static command => command.WriteIoValue));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证仿真模式下即使传入稠密路点，也会回退到单点同步更新。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ExecuteTrajectory_WithDenseWaypoints_SimulationMode_FallsBackToSinglePoint()
|
||
{
|
||
var runtime = new FanucControllerRuntime();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||
runtime.SetActiveController(sim: true);
|
||
runtime.Connect("192.168.10.101");
|
||
runtime.EnableRobot(bufferSize: 2);
|
||
|
||
var denseTrajectory = new[]
|
||
{
|
||
new[] { 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 },
|
||
new[] { 0.008, 0.11, 0.21, 0.31, 0.41, 0.51, 0.61 },
|
||
new[] { 0.016, 0.12, 0.22, 0.32, 0.42, 0.52, 0.62 }
|
||
};
|
||
|
||
var result = new TrajectoryResult(
|
||
programName: "demo",
|
||
method: PlanningMethod.Icsp,
|
||
isValid: true,
|
||
duration: TimeSpan.FromSeconds(0.016),
|
||
shotEvents: Array.Empty<ShotEvent>(),
|
||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||
failureReason: null,
|
||
usedCache: false,
|
||
originalWaypointCount: 4,
|
||
plannedWaypointCount: 4,
|
||
denseJointTrajectory: denseTrajectory);
|
||
|
||
runtime.ExecuteTrajectory(result, [0.12, 0.22, 0.32, 0.42, 0.52, 0.62]);
|
||
|
||
var snapshot = runtime.GetSnapshot();
|
||
Assert.False(snapshot.IsInMotion);
|
||
Assert.Equal([0.12, 0.22, 0.32, 0.42, 0.52, 0.62], snapshot.JointPositions);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证 StopMove 在没有任何后台发送任务运行时不会抛出异常。
|
||
/// </summary>
|
||
[Fact]
|
||
public void StopMove_DoesNotThrowWhenNoSendTaskRunning()
|
||
{
|
||
var runtime = new FanucControllerRuntime();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||
runtime.SetActiveController(sim: true);
|
||
runtime.Connect("192.168.10.101");
|
||
runtime.EnableRobot(bufferSize: 2);
|
||
|
||
var exception = Record.Exception(() => runtime.StopMove());
|
||
Assert.Null(exception);
|
||
Assert.False(runtime.GetSnapshot().IsInMotion);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证 IO 地址组中的地址号被正确映射为 writeIoValue 的位掩码。
|
||
/// </summary>
|
||
[Theory]
|
||
[InlineData(new[] { 0 }, (ushort)1)]
|
||
[InlineData(new[] { 7 }, (ushort)128)]
|
||
[InlineData(new[] { 7, 8 }, (ushort)384)] // 128 + 256
|
||
[InlineData(new[] { 15 }, (ushort)32768)]
|
||
[InlineData(new int[] { }, (ushort)0)]
|
||
public void ComputeIoValue_MapsAddressesToBitMask(int[] addresses, ushort expected)
|
||
{
|
||
var group = new IoAddressGroup(addresses);
|
||
var actual = FanucControllerRuntime.ComputeIoValue(group);
|
||
Assert.Equal(expected, actual);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证超过 15 的地址号会被安全忽略，不会溢出位掩码。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ComputeIoValue_IgnoresOutOfRangeAddresses()
|
||
{
|
||
var group = new IoAddressGroup([0, 16, 7]);
|
||
var actual = FanucControllerRuntime.ComputeIoValue(group);
|
||
Assert.Equal((ushort)(1 | 128), actual);
|
||
}
|
||
|
||
[Fact]
|
||
public void MoveJointTrajectoryGenerator_GeneratesSmoothJerkLimitedPtpTrajectoryAtSpeedOne()
|
||
{
|
||
var robot = CreateMoveJointReferenceRobotProfile();
|
||
var startJoints = new[]
|
||
{
|
||
DegreesToRadians(60.546227),
|
||
DegreesToRadians(0.668344),
|
||
DegreesToRadians(-1.025155),
|
||
DegreesToRadians(-0.869105),
|
||
DegreesToRadians(1.231405),
|
||
DegreesToRadians(0.548197)
|
||
};
|
||
var targetJoints = new[]
|
||
{
|
||
DegreesToRadians(48.886810),
|
||
DegreesToRadians(2.198985),
|
||
DegreesToRadians(-11.021017),
|
||
DegreesToRadians(0.410210),
|
||
DegreesToRadians(6.248381),
|
||
DegreesToRadians(2.294991)
|
||
};
|
||
|
||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||
var rows = result.DenseJointTrajectory!;
|
||
|
||
Assert.True(rows.Count > 41, $"平滑 MoveJoint 不应再固定输出 41 点，actual={rows.Count}。");
|
||
Assert.True(result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
|
||
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
||
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
||
AssertAllJointJerksWithinLimits(rows, robot);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证报警样本中的 MoveJoint 预移动作不会因为点到点时间律重采样而产生任一轴跃度尖峰。
|
||
/// </summary>
|
||
[Fact]
|
||
public void MoveJointTrajectoryGenerator_ActualAlarmSampleKeepsJointOneJerkWithinLimit()
|
||
{
|
||
var robot = CreateMoveJointRuntimeLimitRobotProfile();
|
||
var startJoints = new[]
|
||
{
|
||
DegreesToRadians(71.454618),
|
||
DegreesToRadians(0.688433),
|
||
DegreesToRadians(-1.074197),
|
||
DegreesToRadians(-0.869001),
|
||
DegreesToRadians(1.218057),
|
||
DegreesToRadians(0.547036)
|
||
};
|
||
var targetJoints = new[]
|
||
{
|
||
DegreesToRadians(60.546227),
|
||
DegreesToRadians(0.668344),
|
||
DegreesToRadians(-1.025155),
|
||
DegreesToRadians(-0.869105),
|
||
DegreesToRadians(1.231405),
|
||
DegreesToRadians(0.548197)
|
||
};
|
||
|
||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||
var rows = result.DenseJointTrajectory!;
|
||
AssertAllJointJerksWithinLimits(rows, robot);
|
||
}
|
||
|
||
[Fact]
|
||
public void MoveJointTrajectoryGenerator_DoesNotShortenLimitDurationWhenSpeedRatioDoesNotDivideWindow()
|
||
{
|
||
var robot = CreateMoveJointReferenceRobotProfile();
|
||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||
var targetJoints = new[] { 0.05, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||
|
||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
|
||
var rows = result.DenseJointTrajectory!;
|
||
|
||
Assert.True(
|
||
result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints),
|
||
$"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
|
||
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
||
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
||
}
|
||
|
||
[Fact]
|
||
public void MoveJointTrajectoryGenerator_RejectsUnrepresentableSampleCountForTinySpeedRatio()
|
||
{
|
||
var robot = CreateMoveJointReferenceRobotProfile();
|
||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||
var targetJoints = new[] { 0.05, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||
|
||
var exception = Assert.Throws<InvalidOperationException>(
|
||
() => MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1e-12));
|
||
|
||
Assert.Contains("sample count", exception.Message, StringComparison.OrdinalIgnoreCase);
|
||
}
|
||
|
||
[Fact]
|
||
public void MoveJointTrajectoryGenerator_StretchesLongMoveFromJointLimitsInsteadOfKeepingFortyCycles()
|
||
{
|
||
var robot = CreateMoveJointReferenceRobotProfile();
|
||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||
var targetJoints = new[] { Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||
|
||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||
var rows = result.DenseJointTrajectory!;
|
||
var expectedVelocityDuration = Math.PI * SmoothPtpVelocityShapeCoefficient / robot.JointLimits[0].VelocityLimit;
|
||
var expectedAccelerationDuration = Math.Sqrt(Math.PI * SmoothPtpAccelerationShapeCoefficient / robot.JointLimits[0].AccelerationLimit);
|
||
var expectedJerkDuration = Math.Cbrt(Math.PI * SmoothPtpJerkShapeCoefficient / robot.JointLimits[0].JerkLimit);
|
||
var expectedMinimumDuration = new[]
|
||
{
|
||
expectedVelocityDuration,
|
||
expectedAccelerationDuration,
|
||
expectedJerkDuration
|
||
}.Max();
|
||
var expectedCountFromDuration = (int)Math.Floor(result.Duration.TotalSeconds / robot.ServoPeriod.TotalSeconds + 1e-9) + 1;
|
||
|
||
Assert.True(rows.Count > 41, $"Expected long MoveJoint to produce more than 41 points, got {rows.Count}.");
|
||
Assert.True(
|
||
result.Duration.TotalSeconds >= expectedMinimumDuration,
|
||
$"Expected duration >= {expectedMinimumDuration:F6}s from joint limits, got {result.Duration.TotalSeconds:F6}s.");
|
||
Assert.Equal(expectedCountFromDuration, rows.Count);
|
||
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
||
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
||
}
|
||
|
||
private static void ForceRealModeEnabled(FanucControllerRuntime runtime, double speedRatio)
|
||
{
|
||
SetPrivateField(runtime, "_activeControllerIsSimulation", false);
|
||
SetPrivateField(runtime, "_connectedRobotIp", "127.0.0.1");
|
||
SetPrivateField(runtime, "_isEnabled", true);
|
||
SetPrivateField(runtime, "_bufferSize", 2);
|
||
SetPrivateField(runtime, "_speedRatio", speedRatio);
|
||
}
|
||
|
||
private static ControllerClientCompatService CreateCompatService(FanucControllerRuntime runtime)
|
||
{
|
||
var options = new ControllerClientCompatOptions
|
||
{
|
||
ConfigRoot = TestRobotFactory.GetConfigRoot()
|
||
};
|
||
|
||
return new ControllerClientCompatService(
|
||
options,
|
||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||
runtime,
|
||
new ControllerClientTrajectoryOrchestrator(),
|
||
new RobotConfigLoader());
|
||
}
|
||
|
||
private static double ComputeLineAlpha(
|
||
IReadOnlyList<double> actualDegrees,
|
||
IReadOnlyList<double> startRadians,
|
||
IReadOnlyList<double> targetRadians)
|
||
{
|
||
var numerator = 0.0;
|
||
var denominator = 0.0;
|
||
for (var index = 0; index < startRadians.Count; index++)
|
||
{
|
||
var startDegrees = RadiansToDegrees(startRadians[index]);
|
||
var deltaDegrees = RadiansToDegrees(targetRadians[index]) - startDegrees;
|
||
numerator += (actualDegrees[index] - startDegrees) * deltaDegrees;
|
||
denominator += deltaDegrees * deltaDegrees;
|
||
}
|
||
|
||
return denominator <= 0.0 ? 0.0 : numerator / denominator;
|
||
}
|
||
|
||
private static void AssertJointDegreesEqual(IReadOnlyList<double> expectedRadians, IReadOnlyList<double> actualDegrees)
|
||
{
|
||
Assert.Equal(expectedRadians.Count, actualDegrees.Count);
|
||
for (var index = 0; index < expectedRadians.Count; index++)
|
||
{
|
||
Assert.Equal(RadiansToDegrees(expectedRadians[index]), actualDegrees[index], precision: 4);
|
||
}
|
||
}
|
||
|
||
private static RobotProfile CreateMoveJointReferenceRobotProfile()
|
||
{
|
||
return new RobotProfile(
|
||
name: "FANUC_LR_Mate_200iD",
|
||
modelPath: "Models/FANUC_LR_Mate_200iD.robot",
|
||
degreesOfFreedom: 6,
|
||
jointLimits:
|
||
[
|
||
new JointLimit("J1", 7.85, 32.72, 272.7),
|
||
new JointLimit("J2", 6.63, 27.63, 230.28),
|
||
new JointLimit("J3", 9.07, 37.81, 315.12),
|
||
new JointLimit("J4", 9.59, 39.99, 333.3),
|
||
new JointLimit("J5", 9.51, 39.63, 330.27),
|
||
new JointLimit("J6", 17.45, 72.72, 606.01)
|
||
],
|
||
jointCouplings: Array.Empty<JointCoupling>(),
|
||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建当前运行配置下的 MoveJoint 限值模型，覆盖报警样本使用的 acc_limit/jerk_limit=0.74。
|
||
/// </summary>
|
||
private static RobotProfile CreateMoveJointRuntimeLimitRobotProfile()
|
||
{
|
||
const double limitScale = 0.74;
|
||
return new RobotProfile(
|
||
name: "FANUC_LR_Mate_200iD",
|
||
modelPath: "Models/FANUC_LR_Mate_200iD.robot",
|
||
degreesOfFreedom: 6,
|
||
jointLimits:
|
||
[
|
||
new JointLimit("J1", 6.45, 26.90 * limitScale, 224.22 * limitScale),
|
||
new JointLimit("J2", 5.41, 22.54 * limitScale, 187.86 * limitScale),
|
||
new JointLimit("J3", 7.15, 29.81 * limitScale, 248.46 * limitScale),
|
||
new JointLimit("J4", 9.59, 39.99 * limitScale, 333.30 * limitScale),
|
||
new JointLimit("J5", 9.51, 39.63 * limitScale, 330.27 * limitScale),
|
||
new JointLimit("J6", 17.45, 72.72 * limitScale, 606.01 * limitScale)
|
||
],
|
||
jointCouplings: Array.Empty<JointCoupling>(),
|
||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 以发送周期上的二阶后向差分估算单轴最大绝对跃度，用来复现 ActualSendJerkStats 的判定口径。
|
||
/// </summary>
|
||
private static double MaxAbsJointJerk(IReadOnlyList<IReadOnlyList<double>> rows, int jointIndex)
|
||
{
|
||
double? previousTime = null;
|
||
double? previousPosition = null;
|
||
double? previousVelocity = null;
|
||
double? previousAcceleration = null;
|
||
var maxAbsJerk = 0.0;
|
||
|
||
foreach (var row in rows)
|
||
{
|
||
var currentTime = row[0];
|
||
var currentPosition = row[jointIndex + 1];
|
||
if (previousTime is not null && previousPosition is not null)
|
||
{
|
||
var dt = currentTime - previousTime.Value;
|
||
Assert.True(dt > 0.0, "MoveJoint 稠密轨迹时间戳必须严格递增。");
|
||
|
||
var velocity = (currentPosition - previousPosition.Value) / dt;
|
||
var acceleration = previousVelocity is null ? 0.0 : (velocity - previousVelocity.Value) / dt;
|
||
if (previousAcceleration is not null)
|
||
{
|
||
var jerk = (acceleration - previousAcceleration.Value) / dt;
|
||
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(jerk));
|
||
}
|
||
|
||
previousVelocity = velocity;
|
||
previousAcceleration = acceleration;
|
||
}
|
||
|
||
previousTime = currentTime;
|
||
previousPosition = currentPosition;
|
||
}
|
||
|
||
return maxAbsJerk;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证稠密 MoveJoint 轨迹按 8ms 差分后不会超过机器人每轴 jerk 限值。
|
||
/// </summary>
|
||
private static void AssertAllJointJerksWithinLimits(IReadOnlyList<IReadOnlyList<double>> rows, RobotProfile robot)
|
||
{
|
||
for (var jointIndex = 0; jointIndex < robot.DegreesOfFreedom; jointIndex++)
|
||
{
|
||
var peakJerk = MaxAbsJointJerk(rows, jointIndex);
|
||
var limit = robot.JointLimits[jointIndex].JerkLimit;
|
||
Assert.True(
|
||
peakJerk <= limit * 1.000001,
|
||
$"J{jointIndex + 1} jerk {peakJerk:F6} rad/s^3 exceeds limit {limit:F6} rad/s^3.");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按 7 阶平滑点到点时间律的速度、加速度、jerk 形状系数计算理论最小时长。
|
||
/// </summary>
|
||
private static double ExpectedSmoothPtpDuration(
|
||
RobotProfile robot,
|
||
IReadOnlyList<double> startJoints,
|
||
IReadOnlyList<double> targetJoints)
|
||
{
|
||
var duration = 0.0;
|
||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||
{
|
||
var distance = Math.Abs(targetJoints[index] - startJoints[index]);
|
||
if (distance <= 0.0)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
var limit = robot.JointLimits[index];
|
||
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limit.VelocityLimit;
|
||
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limit.AccelerationLimit);
|
||
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limit.JerkLimit);
|
||
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
|
||
}
|
||
|
||
return duration;
|
||
}
|
||
|
||
private static double DegreesToRadians(double degrees)
|
||
{
|
||
return degrees * Math.PI / 180.0;
|
||
}
|
||
|
||
private static double RadiansToDegrees(double radians)
|
||
{
|
||
return radians * 180.0 / Math.PI;
|
||
}
|
||
|
||
private static void WaitUntilIdle(FanucControllerRuntime runtime)
|
||
{
|
||
var deadline = DateTimeOffset.UtcNow.AddSeconds(1);
|
||
while (DateTimeOffset.UtcNow < deadline)
|
||
{
|
||
if (!runtime.GetSnapshot().IsInMotion)
|
||
{
|
||
return;
|
||
}
|
||
|
||
Thread.Sleep(1);
|
||
}
|
||
|
||
throw new TimeoutException("Timed out waiting for dense trajectory send task to finish.");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取一次测试执行生成的唯一稠密发送记录目录。
|
||
/// </summary>
|
||
private static string GetSingleDenseSendRunDirectory(string outputRoot)
|
||
{
|
||
var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
|
||
Assert.True(Directory.Exists(denseSendRoot));
|
||
|
||
var runDirectories = Directory.GetDirectories(denseSendRoot);
|
||
Assert.Single(runDirectories);
|
||
return runDirectories[0];
|
||
}
|
||
|
||
/// <summary>
|
||
/// 解析空格分隔的纯文本数值列。
|
||
/// </summary>
|
||
private static double[] ParseColumns(string line)
|
||
{
|
||
return line
|
||
.Split(' ', StringSplitOptions.RemoveEmptyEntries)
|
||
.Select(static value => double.Parse(value))
|
||
.ToArray();
|
||
}
|
||
|
||
private static void SetPrivateField<T>(FanucControllerRuntime runtime, string name, T value)
|
||
{
|
||
var field = typeof(FanucControllerRuntime).GetField(name, BindingFlags.Instance | BindingFlags.NonPublic);
|
||
Assert.NotNull(field);
|
||
field.SetValue(runtime, value);
|
||
}
|
||
|
||
private static TrajectoryResult CreateDenseResult(IReadOnlyList<IReadOnlyList<double>> denseTrajectory, double durationSeconds)
|
||
{
|
||
return new TrajectoryResult(
|
||
programName: "demo",
|
||
method: PlanningMethod.Icsp,
|
||
isValid: true,
|
||
duration: TimeSpan.FromSeconds(durationSeconds),
|
||
shotEvents: Array.Empty<ShotEvent>(),
|
||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||
failureReason: null,
|
||
usedCache: false,
|
||
originalWaypointCount: 4,
|
||
plannedWaypointCount: 4,
|
||
denseJointTrajectory: denseTrajectory);
|
||
}
|
||
|
||
private static void SetLatestJ519Response(FanucJ519Client client, byte status)
|
||
{
|
||
var response = new FanucJ519Response(
|
||
messageType: 0,
|
||
version: 1,
|
||
sequence: 10,
|
||
status: status,
|
||
readIoType: 2,
|
||
readIoIndex: 1,
|
||
readIoMask: 255,
|
||
readIoValue: 0,
|
||
timestamp: 1234,
|
||
pose: new double[6],
|
||
externalAxes: new double[3],
|
||
jointDegrees: new double[9],
|
||
motorCurrents: new double[9]);
|
||
var field = typeof(FanucJ519Client).GetField("_latestResponse", BindingFlags.Instance | BindingFlags.NonPublic);
|
||
Assert.NotNull(field);
|
||
field.SetValue(client, response);
|
||
}
|
||
}
|