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

✨ feat(fanuc): 优化 J519 实时下发与飞拍起停整形

Browse Source 
- 改为高优先级 J519 接收线程与复用缓冲区发送链路
- 增加稠密执行前的 J519 就绪重试与状态诊断
- 修正程序状态响应字段顺序与 EnableRobot 默认参数
- 为飞拍轨迹补充平滑起停时间轴与首尾整形验证
- 补充真实运行配置、报警窗口与边界对比测试
- 同步更新限值文档、分析脚本与 .NET 8 SDK 固定配置
This commit is contained in:
yunxiao.zhu
2026-05-06 22:37:31 +08:00
parent 783716ff44
commit 70b0ccd414
17 changed files with 1629 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

112
src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs
View File

@@ -66,7 +66,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
var plannedTrajectory = PlanByMethod(request, method);
var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false, shapeTrajectoryEdges: false);
_logger?.LogInformation(
"PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}",
@@ -112,11 +112,17 @@ public sealed class ControllerClientTrajectoryOrchestrator
if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
{
_logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
var executionTrajectory = ApplySmoothStartStopTiming(cachedBundle.PlannedTrajectory);
var executionTimeline = _shotTimelineBuilder.Build(
executionTrajectory,
holdCycles: settings.IoKeepCycles,
samplePeriod: planningRobot.ServoPeriod,
useDo: settings.UseDo);
// 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。
return new PlannedExecutionBundle(
cachedBundle.PlannedTrajectory,
cachedBundle.ShotTimeline,
CreateResult(cachedBundle.PlannedTrajectory, cachedBundle.ShotTimeline, usedCache: true));
executionTimeline,
CreateResult(executionTrajectory, executionTimeline, usedCache: true, shapeTrajectoryEdges: false));
}
var request = new TrajectoryRequest(
@@ -128,12 +134,13 @@ public sealed class ControllerClientTrajectoryOrchestrator
useCache: options.UseCache);
var plannedTrajectory = PlanByMethod(request, method, settings);
var smoothedExecutionTrajectory = ApplySmoothStartStopTiming(plannedTrajectory);
var shotTimeline = _shotTimelineBuilder.Build(
plannedTrajectory,
smoothedExecutionTrajectory,
holdCycles: settings.IoKeepCycles,
samplePeriod: planningRobot.ServoPeriod,
useDo: settings.UseDo);
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
var result = CreateResult(smoothedExecutionTrajectory, shotTimeline, usedCache: false, shapeTrajectoryEdges: false);
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
_logger?.LogInformation(
@@ -357,11 +364,16 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// <param name="plannedTrajectory">规划后的轨迹。</param>
/// <param name="shotTimeline">触发时间轴。</param>
/// <returns>运行时执行结果描述。</returns>
private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, bool usedCache)
private static TrajectoryResult CreateResult(
PlannedTrajectory plannedTrajectory,
ShotTimeline shotTimeline,
bool usedCache,
bool shapeTrajectoryEdges)
{
var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory(
plannedTrajectory,
samplePeriod: plannedTrajectory.Robot.ServoPeriod.TotalSeconds);
samplePeriod: plannedTrajectory.Robot.ServoPeriod.TotalSeconds,
smoothStartStop: shapeTrajectoryEdges);
return new TrajectoryResult(
programName: plannedTrajectory.OriginalProgram.Name,
@@ -377,4 +389,90 @@ public sealed class ControllerClientTrajectoryOrchestrator
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount,
denseJointTrajectory: denseJointTrajectory);
}
/// <summary>
/// 为飞拍执行生成一条平滑起停的时间轴。
/// 保持路点位置不变，只重映射路点时刻，让起点和终点附近的速度自然收敛。
/// </summary>
private static PlannedTrajectory ApplySmoothStartStopTiming(PlannedTrajectory plannedTrajectory)
{
var originalTimes = plannedTrajectory.WaypointTimes;
if (originalTimes.Count < 3)
{
return plannedTrajectory;
}
var totalDuration = originalTimes[^1];
if (totalDuration <= 0.0)
{
return plannedTrajectory;
}
var smoothedTimes = new double[originalTimes.Count];
smoothedTimes[0] = 0.0;
smoothedTimes[^1] = totalDuration;
for (var index = 1; index < originalTimes.Count - 1; index++)
{
var normalizedProgress = originalTimes[index] / totalDuration;
smoothedTimes[index] = totalDuration * InvertSmoothStartStopProgress(normalizedProgress);
}
var segmentDurations = new double[smoothedTimes.Length - 1];
for (var index = 0; index < segmentDurations.Length; index++)
{
segmentDurations[index] = smoothedTimes[index + 1] - smoothedTimes[index];
}
return new PlannedTrajectory(
robot: plannedTrajectory.Robot,
originalProgram: plannedTrajectory.OriginalProgram,
plannedWaypoints: plannedTrajectory.PlannedWaypoints,
waypointTimes: smoothedTimes,
segmentDurations: segmentDurations,
segmentScales: plannedTrajectory.SegmentScales,
method: plannedTrajectory.Method,
iterations: plannedTrajectory.Iterations,
threshold: plannedTrajectory.Threshold);
}
/// <summary>
/// 反解 7 次 smootherstep 的时间进度，用二分法把原始线性进度映射成平滑时间轴。
/// </summary>
private static double InvertSmoothStartStopProgress(double normalizedProgress)
{
var target = Math.Clamp(normalizedProgress, 0.0, 1.0);
var low = 0.0;
var high = 1.0;
for (var iteration = 0; iteration < 40; iteration++)
{
var middle = (low + high) / 2.0;
var progress = EvaluateSmoothStartStopProgress(middle);
if (progress < target)
{
low = middle;
}
else
{
high = middle;
}
}
return (low + high) / 2.0;
}
/// <summary>
/// 计算 7 次 smootherstep 进度值，用于整段平滑起停时间律。
/// </summary>
private static double EvaluateSmoothStartStopProgress(double normalizedTime)
{
var u = Math.Clamp(normalizedTime, 0.0, 1.0);
var u2 = u * u;
var u3 = u2 * u;
var u4 = u3 * u;
var u5 = u4 * u;
var u6 = u5 * u;
var u7 = u6 * u;
return (35.0 * u4) - (84.0 * u5) + (70.0 * u6) - (20.0 * u7);
}
}