xsxiaosa/FlyShotHost
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

♻️ refactor(compat): 替换 MoveJoint 时间律为解析式 7 阶平滑函数并添加离散限位校验

Browse Source 
* 将预捕获 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>
This commit is contained in:
yunxiao.zhu
2026-05-06 09:06:28 +08:00
parent af65ca03a0
commit b1710e5d01
13 changed files with 1654 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files

316
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
View File

@@ -12,9 +12,9 @@ namespace Flyshot.Core.Tests;
/// </summary>
public sealed class FanucControllerRuntimeDenseTests
{
private const double CapturedMvpointVelocityShapeCoefficient = 2.0759961613199973;
private const double CapturedMvpointAccelerationShapeCoefficient = 7.986313199999984;
private const double CapturedMvpointJerkShapeCoefficient = 36.12609273600853;
private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
private const double SmoothPtpJerkShapeCoefficient = 52.5;
/// <summary>
/// 验证真机 J519 发送按 8ms 实发周期、speed_ratio 轨迹时间步进，并输出角度制目标。
@@ -61,6 +61,84 @@ public sealed class FanucControllerRuntimeDenseTests
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>
@@ -135,9 +213,9 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.True(speed07.DenseJointTrajectory!.Count > fullSpeed.DenseJointTrajectory!.Count);
Assert.True(speed05.DenseJointTrajectory!.Count > speed07.DenseJointTrajectory!.Count);
Assert.InRange(fullSpeed.Duration.TotalSeconds, 0.318, 0.322);
Assert.True(speed07.Duration.TotalSeconds >= 0.320);
Assert.InRange(speed05.Duration.TotalSeconds, 0.318, 0.322);
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]
@@ -395,7 +473,7 @@ public sealed class FanucControllerRuntimeDenseTests
}
[Fact]
public void MoveJointTrajectoryGenerator_MatchesCapturedMvpointAlphaLawAtSpeedOne()
public void MoveJointTrajectoryGenerator_GeneratesSmoothJerkLimitedPtpTrajectoryAtSpeedOne()
{
var robot = CreateMoveJointReferenceRobotProfile();
var startJoints = new[]
@@ -420,64 +498,46 @@ public sealed class FanucControllerRuntimeDenseTests
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
var rows = result.DenseJointTrajectory!;
Assert.Equal(41, rows.Count);
Assert.InRange(result.Duration.TotalSeconds, 0.318, 0.322);
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);
}
var expectedAlpha = new[]
/// <summary>
/// 验证报警样本中的 MoveJoint 预移动作不会因为点到点时间律重采样而产生任一轴跃度尖峰。
/// </summary>
[Fact]
public void MoveJointTrajectoryGenerator_ActualAlarmSampleKeepsJointOneJerkWithinLimit()
{
var robot = CreateMoveJointRuntimeLimitRobotProfile();
var startJoints = new[]
{
0.000000000000,
0.000012196163,
0.000106156906,
0.000764380061,
0.002550804028,
0.006029689194,
0.011765134027,
0.020321400844,
0.032262426551,
0.048152469303,
0.068555498563,
0.093895155669,
0.124210027377,
0.159174512929,
0.198230386318,
0.240813559900,
0.286359937276,
0.334305411725,
0.384085546646,
0.435136609163,
0.486894129077,
0.538794033110,
0.590272360135,
0.640764719629,
0.689707151220,
0.736535405849,
0.780685354316,
0.821592775628,
0.858693734065,
0.891423926949,
0.919286047395,
0.942156722091,
0.960255163676,
0.974119666692,
0.984314536393,
0.991403790959,
0.995951593494,
0.998522142663,
0.999679443354,
0.999987892657,
1.000000000000
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)
};
for (var index = 0; index < rows.Count; index++)
{
var actualDegrees = rows[index].Skip(1).Select(RadiansToDegrees).ToArray();
var alpha = ComputeLineAlpha(actualDegrees, startJoints, targetJoints);
Assert.Equal(expectedAlpha[index], alpha, precision: 6);
}
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
var rows = result.DenseJointTrajectory!;
AssertAllJointJerksWithinLimits(rows, robot);
}
[Fact]
public void MoveJointTrajectoryGenerator_DoesNotShortenBaseDurationWhenSpeedRatioDoesNotDivideWindow()
public void MoveJointTrajectoryGenerator_DoesNotShortenLimitDurationWhenSpeedRatioDoesNotDivideWindow()
{
var robot = CreateMoveJointReferenceRobotProfile();
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
@@ -486,7 +546,9 @@ public sealed class FanucControllerRuntimeDenseTests
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
var rows = result.DenseJointTrajectory!;
Assert.True(result.Duration.TotalSeconds >= 0.320, $"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
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());
}
@@ -513,12 +575,11 @@ public sealed class FanucControllerRuntimeDenseTests
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
var rows = result.DenseJointTrajectory!;
var expectedVelocityDuration = Math.PI * CapturedMvpointVelocityShapeCoefficient / robot.JointLimits[0].VelocityLimit;
var expectedAccelerationDuration = Math.Sqrt(Math.PI * CapturedMvpointAccelerationShapeCoefficient / robot.JointLimits[0].AccelerationLimit);
var expectedJerkDuration = Math.Cbrt(Math.PI * CapturedMvpointJerkShapeCoefficient / robot.JointLimits[0].JerkLimit);
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[]
{
0.320,
expectedVelocityDuration,
expectedAccelerationDuration,
expectedJerkDuration
@@ -605,6 +666,111 @@ public sealed class FanucControllerRuntimeDenseTests
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;
@@ -631,6 +797,30 @@ public sealed class FanucControllerRuntimeDenseTests
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);

32
tests/Flyshot.Core.Tests/FanucControllerRuntimeLoggingRegistrationTests.cs Normal file
View File

@@ -0,0 +1,32 @@
using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc;
using Microsoft.Extensions.DependencyInjection;
using System.Reflection;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 FANUC 运行时通过依赖注入解析时，能够稳定拿到日志依赖。
/// </summary>
public sealed class FanucControllerRuntimeLoggingRegistrationTests
{
/// <summary>
/// 验证宿主按 IControllerRuntime 解析 FANUC 运行时时，不会因为可选日志参数而退回到无日志实例。
/// </summary>
[Fact]
public void ServiceProvider_Resolves_FanucControllerRuntime_WithLogger()
{
var services = new ServiceCollection();
services.AddLogging();
services.AddSingleton<IControllerRuntime, FanucControllerRuntime>();
using var serviceProvider = services.BuildServiceProvider();
var runtime = serviceProvider.GetRequiredService<IControllerRuntime>();
var concreteRuntime = Assert.IsType<FanucControllerRuntime>(runtime);
var loggerField = typeof(FanucControllerRuntime).GetField("_logger", BindingFlags.Instance | BindingFlags.NonPublic);
Assert.NotNull(loggerField);
Assert.NotNull(loggerField!.GetValue(concreteRuntime));
}
}

90
tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
View File

@@ -2,6 +2,7 @@ using System.Buffers.Binary;
using System.Net;
using System.Net.Sockets;
using Flyshot.Runtime.Fanuc.Protocol;
using Microsoft.Extensions.Logging;
namespace Flyshot.Core.Tests;
@@ -287,6 +288,31 @@ public sealed class FanucJ519ClientTests : IDisposable
});
}
/// <summary>
/// 验证状态变化日志会附带最近一次实际发送的目标关节轴，便于联调时对照控制目标。
/// </summary>
[Fact]
public async Task ReceiveLoop_LogsLastSentTargetJointsWhenStatusChanges()
{
var logger = new CapturingLogger<FanucJ519Client>();
using var client = new FanucJ519Client(logger);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]));
client.StartMotion();
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 42);
_ = await _server.ReceiveAsync(_cts.Token);
await Task.Delay(200, _cts.Token);
Assert.Contains(
logger.Entries,
entry => entry.Level == LogLevel.Information
&& entry.Message.Contains("J519 最后一条发送目标关节轴", StringComparison.Ordinal)
&& entry.Message.Contains("1.000, 2.000, 3.000, 4.000, 5.000, 6.000", StringComparison.Ordinal));
}
/// <summary>
/// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
/// </summary>
@@ -299,4 +325,68 @@ public sealed class FanucJ519ClientTests : IDisposable
responsePacket[0x0c] = 15;
await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);
}
/// <summary>
/// 收集测试过程中的结构化日志，便于断言运行期输出内容。
/// </summary>
private sealed class CapturingLogger<T> : ILogger<T>
{
/// <summary>
/// 获取已记录的日志条目。
/// </summary>
public List<LogEntry> Entries { get; } = [];
/// <summary>
/// 开始日志作用域；当前测试无需作用域，直接返回空对象。
/// </summary>
public IDisposable BeginScope<TState>(TState state)
where TState : notnull
{
return NullScope.Instance;
}
/// <summary>
/// 指示所有日志级别均启用，便于测试完整捕获输出。
/// </summary>
public bool IsEnabled(LogLevel logLevel)
{
return true;
}
/// <summary>
/// 记录一条格式化后的日志消息。
/// </summary>
public void Log<TState>(
LogLevel logLevel,
EventId eventId,
TState state,
Exception? exception,
Func<TState, Exception?, string> formatter)
{
Entries.Add(new LogEntry(logLevel, formatter(state, exception)));
}
/// <summary>
/// 表示一次日志记录。
/// </summary>
public sealed record LogEntry(LogLevel Level, string Message);
/// <summary>
/// 提供空日志作用域，避免测试中额外分配。
/// </summary>
private sealed class NullScope : IDisposable
{
/// <summary>
/// 获取单例空作用域。
/// </summary>
public static NullScope Instance { get; } = new();
/// <summary>
/// 释放空作用域；无需实际动作。
/// </summary>
public void Dispose()
{
}
}
}
}

169
tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
View File

@@ -337,6 +337,49 @@ public sealed class RuntimeOrchestrationTests
}
}
/// <summary>
/// 验证正式飞拍前若真实反馈仍未接近起点，则兼容层会拒绝继续执行。
/// </summary>
[Fact]
public void ControllerClientCompatService_ExecuteTrajectoryByName_RejectsFlyshotWhenFeedbackIsNotNearStartAfterMoveToStart()
{
var configRoot = CreateTempConfigRoot();
try
{
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
};
var runtime = new StickyFeedbackControllerRuntime(
initialJointPositions: [0.4, 0.0, 0.0, 0.0, 0.0, 0.0],
firstMotionCompletionDelay: TimeSpan.FromMilliseconds(20));
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime,
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SetActiveController(sim: false);
service.Connect("192.168.10.101");
service.EnableRobot(2);
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
var exception = Assert.Throws<InvalidOperationException>(() =>
service.ExecuteTrajectoryByName(
"demo-flyshot",
new FlyshotExecutionOptions(moveToStart: true, method: "icsp", saveTrajectory: false, useCache: false)));
Assert.Contains("not near flyshot start", exception.Message, StringComparison.OrdinalIgnoreCase);
Assert.Single(runtime.ExecuteCalls);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 验证 ExecuteFlyShotTraj(wait=true) 会等待正式飞拍轨迹完成后再返回。
/// </summary>
@@ -897,3 +940,129 @@ internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
}
}
}
/// <summary>
/// 模拟 move_to_start 逻辑完成后，真实反馈仍停留在旧位置的测试运行时。
/// </summary>
internal sealed class StickyFeedbackControllerRuntime : IControllerRuntime
{
private readonly object _lock = new();
private readonly TimeSpan _firstMotionCompletionDelay;
private double[] _jointPositions;
private bool _isEnabled;
private bool _isInMotion;
private bool _firstMotionCompleted;
public StickyFeedbackControllerRuntime(
IReadOnlyList<double> initialJointPositions,
TimeSpan firstMotionCompletionDelay)
{
_jointPositions = initialJointPositions.ToArray();
_firstMotionCompletionDelay = firstMotionCompletionDelay;
}
public List<(TrajectoryResult Result, IReadOnlyList<double> FinalJointPositions)> ExecuteCalls { get; } = [];
public void ResetRobot(RobotProfile robot, string robotName)
{
}
public void SetActiveController(bool sim)
{
}
public void Connect(string robotIp)
{
}
public void Disconnect()
{
}
public void EnableRobot(int bufferSize)
{
_isEnabled = true;
}
public void DisableRobot()
{
_isEnabled = false;
}
public void StopMove()
{
lock (_lock)
{
_isInMotion = false;
}
}
public double GetSpeedRatio() => 1.0;
public void SetSpeedRatio(double ratio)
{
}
public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];
public void SetTcp(double x, double y, double z)
{
}
public bool GetIo(int port, string ioType) => false;
public void SetIo(int port, bool value, string ioType)
{
}
public IReadOnlyList<double> GetJointPositions()
{
lock (_lock)
{
return _jointPositions.ToArray();
}
}
public IReadOnlyList<double> GetPose() => Array.Empty<double>();
public ControllerStateSnapshot GetSnapshot()
{
lock (_lock)
{
return new ControllerStateSnapshot(
capturedAt: DateTimeOffset.UtcNow,
connectionState: "Connected",
isEnabled: _isEnabled,
isInMotion: _isInMotion,
speedRatio: 1.0,
jointPositions: _jointPositions.ToArray(),
cartesianPose: Array.Empty<double>(),
activeAlarms: Array.Empty<RuntimeAlarm>());
}
}
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
{
lock (_lock)
{
ExecuteCalls.Add((result, finalJointPositions.ToArray()));
if (!_firstMotionCompleted)
{
_isInMotion = true;
_ = Task.Run(async () =>
{
await Task.Delay(_firstMotionCompletionDelay).ConfigureAwait(false);
lock (_lock)
{
// 模拟控制链逻辑上结束，但真实反馈仍卡在旧位置。
_isInMotion = false;
_firstMotionCompleted = true;
}
});
return;
}
_jointPositions = finalJointPositions.ToArray();
}
}
}