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

* 将预捕获 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:
2026-05-06 09:06:28 +08:00
parent af65ca03a0
commit b1710e5d01
13 changed files with 1654 additions and 163 deletions

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();
}
}
}