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

✨ feat(fanuc): 打通飞拍轨迹完整执行链路

Browse Source 
* 增加 J519 稠密发送采样校验与保姿回发逻辑
* 调整 saveTrajectory 导出与 sequence buffer 行为
* 补充 10010 解析脚本、ICSP 说明和回归测试
This commit is contained in:
yunxiao.zhu
2026-05-08 13:25:02 +08:00
parent c6829d214a
commit 1779067b5c
13 changed files with 1363 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -12,6 +12,16 @@ namespace Flyshot.ControllerClientCompat;
/// </summary>
public sealed class ControllerClientTrajectoryOrchestrator
{
/// <summary>
/// 稠密轨迹离散限幅失败后允许统一拉长时间轴的最大次数。
/// </summary>
private const int MaxDenseLimitStretchIterations = 100;
/// <summary>
/// 每次离散限幅失败后统一放大的时间倍率。
/// </summary>
private const double DenseLimitStretchFactor = 1.01;
private readonly ICspPlanner _icspPlanner;
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner;
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
@@ -68,8 +78,10 @@ public sealed class ControllerClientTrajectoryOrchestrator
saveTrajectoryArtifacts: options.SaveTrajectory);
var plannedTrajectory = PlanByMethod(request, method);
var executionTrajectory = plannedTrajectory;
var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref executionTrajectory, shapeTrajectoryEdges: false);
var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false, shapeTrajectoryEdges: false);
var result = CreateResult(executionTrajectory, shotTimeline, denseJointTrajectory, usedCache: false);
_logger?.LogInformation(
"PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}",
@@ -138,12 +150,13 @@ public sealed class ControllerClientTrajectoryOrchestrator
var plannedTrajectory = PlanByMethod(request, method, settings);
var smoothedExecutionTrajectory = ApplyExecutionTiming(plannedTrajectory, settings);
var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref smoothedExecutionTrajectory, shapeTrajectoryEdges: false);
var shotTimeline = _shotTimelineBuilder.Build(
smoothedExecutionTrajectory,
holdCycles: settings.IoKeepCycles,
samplePeriod: planningRobot.ServoPeriod,
useDo: settings.UseDo);
var result = CreateResult(smoothedExecutionTrajectory, shotTimeline, usedCache: false, shapeTrajectoryEdges: false);
var result = CreateResult(smoothedExecutionTrajectory, shotTimeline, denseJointTrajectory, usedCache: false);
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
_logger?.LogInformation(
@@ -385,14 +398,9 @@ public sealed class ControllerClientTrajectoryOrchestrator
private static TrajectoryResult CreateResult(
PlannedTrajectory plannedTrajectory,
ShotTimeline shotTimeline,
bool usedCache,
bool shapeTrajectoryEdges)
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
bool usedCache)
{
var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory(
plannedTrajectory,
samplePeriod: plannedTrajectory.Robot.ServoPeriod.TotalSeconds,
smoothStartStop: shapeTrajectoryEdges);
return new TrajectoryResult(
programName: plannedTrajectory.OriginalProgram.Name,
method: plannedTrajectory.Method,
@@ -408,6 +416,65 @@ public sealed class ControllerClientTrajectoryOrchestrator
denseJointTrajectory: denseJointTrajectory);
}
/// <summary>
/// 生成满足离散速度、加速度和 Jerk 限制的稠密执行轨迹。
/// </summary>
private IReadOnlyList<IReadOnlyList<double>> CreateLimitCompliantDenseTrajectory(
ref PlannedTrajectory executionTrajectory,
bool shapeTrajectoryEdges)
{
for (var iteration = 0; iteration <= MaxDenseLimitStretchIterations; iteration++)
{
var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory(
executionTrajectory,
samplePeriod: executionTrajectory.Robot.ServoPeriod.TotalSeconds,
smoothStartStop: shapeTrajectoryEdges);
try
{
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
executionTrajectory.Robot,
denseJointTrajectory,
trajectoryName: executionTrajectory.OriginalProgram.Name);
return denseJointTrajectory;
}
catch (InvalidOperationException ex) when (iteration < MaxDenseLimitStretchIterations)
{
_logger?.LogWarning(ex, "稠密轨迹离散限幅校验失败，准备拉长时间轴重试");
// 离散差分超限时统一拉长时间轴，保持路点几何不变并降低速度、加速度和 Jerk。
executionTrajectory = StretchTrajectoryTiming(executionTrajectory, DenseLimitStretchFactor);
_logger?.LogInformation(
"离散差分超限，拉长时间轴，iteration={Iteration}, factor={StretchFactor}",
iteration,
DenseLimitStretchFactor);
_logger?.LogInformation("拉长之后的总时间={TotalTime}", executionTrajectory.WaypointTimes[^1]);
}
}
throw new InvalidOperationException("稠密轨迹离散限幅校验未能产生有效结果。");
}
/// <summary>
/// 按统一倍率拉长轨迹时间轴，保留原始路点和触发元数据。
/// </summary>
private PlannedTrajectory StretchTrajectoryTiming(PlannedTrajectory trajectory, double stretchFactor)
{
var waypointTimes = trajectory.WaypointTimes.Select(time => time * stretchFactor).ToArray();
var segmentDurations = trajectory.SegmentDurations.Select(duration => duration * stretchFactor).ToArray();
var segmentScales = trajectory.SegmentScales.Select(scale => scale / stretchFactor).ToArray();
return new PlannedTrajectory(
robot: trajectory.Robot,
originalProgram: trajectory.OriginalProgram,
plannedWaypoints: trajectory.PlannedWaypoints,
waypointTimes: waypointTimes,
segmentDurations: segmentDurations,
segmentScales: segmentScales,
method: trajectory.Method,
iterations: trajectory.Iterations,
threshold: trajectory.Threshold);
}
/// <summary>
/// 为飞拍执行生成一条平滑起停的时间轴。
/// 保持路点位置不变，只重映射路点时刻，让起点和终点附近的速度自然收敛。