♻️ 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:
@@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user