yunxiao.zhu
74761bb5da
* 新增 FlyshotExecutionSendSequenceBuilder，负责在运行时前构建最终 8ms 发送队列，并进行离散限幅校验。 * 引入 FlyshotPreparedExecution 类，封装最终发送结果及相关诊断信息。 * 调整 ControllerClientCompatService 和 FanucControllerRuntime，确保运行时直接使用预生成的发送队列，避免临场重采样。 * 更新 TrajectoryResult 和 PlannedExecutionBundle，支持准备好的执行队列。 * 增加单元测试，验证非 1 倍 speedRatio 下的执行行为与预生成队列的使用。
608 lines
27 KiB
C#
608 lines
27 KiB
C#
using Flyshot.Core.Config;
|
||
using Flyshot.Core.Domain;
|
||
using Flyshot.Core.Planning;
|
||
using Flyshot.Core.Planning.Sampling;
|
||
using Flyshot.Core.Triggering;
|
||
using Microsoft.Extensions.Logging;
|
||
|
||
namespace Flyshot.ControllerClientCompat;
|
||
|
||
/// <summary>
|
||
/// 负责把 ControllerClient 兼容层的轨迹输入转换为规划结果和触发时间轴。
|
||
/// </summary>
|
||
public sealed class ControllerClientTrajectoryOrchestrator
|
||
{
|
||
/// <summary>
|
||
/// 稠密轨迹离散限幅失败后允许统一拉长时间轴的最大次数。
|
||
/// </summary>
|
||
private const int MaxDenseLimitStretchIterations = 100;
|
||
|
||
/// <summary>
|
||
/// 每次离散限幅失败后统一放大的时间倍率。
|
||
/// </summary>
|
||
private const double DenseLimitStretchFactor = 1.01;
|
||
|
||
/// <summary>
|
||
/// 平滑起停重定时混合系数：0 表示不平滑，1 表示完全使用平滑时间律。
|
||
/// 当前取值用于“弱平滑”，仅轻微拉伸首尾时间段，避免起停过慢。
|
||
/// </summary>
|
||
private const double SmoothStartStopBlend = 0.60;
|
||
|
||
private readonly ICspPlanner _icspPlanner;
|
||
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner;
|
||
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
|
||
private readonly Dictionary<string, PlannedExecutionBundle> _flyshotCache = new(StringComparer.Ordinal);
|
||
private readonly ILogger<ControllerClientTrajectoryOrchestrator>? _logger;
|
||
|
||
/// <summary>
|
||
/// 初始化轨迹编排器。
|
||
/// </summary>
|
||
/// <param name="logger">日志记录器；允许 null。</param>
|
||
/// <param name="loggerFactory">日志工厂；允许 null。</param>
|
||
public ControllerClientTrajectoryOrchestrator(
|
||
ILogger<ControllerClientTrajectoryOrchestrator>? logger = null,
|
||
ILoggerFactory? loggerFactory = null)
|
||
{
|
||
_logger = logger;
|
||
_icspPlanner = new(logger: loggerFactory?.CreateLogger<ICspPlanner>());
|
||
_selfAdaptIcspPlanner = new(logger: loggerFactory?.CreateLogger<SelfAdaptIcspPlanner>());
|
||
}
|
||
|
||
/// <summary>
|
||
/// 对普通轨迹执行 ICSP 规划。
|
||
/// </summary>
|
||
/// <param name="robot">当前机器人配置。</param>
|
||
/// <param name="waypoints">普通轨迹关节路点。</param>
|
||
/// <param name="options">执行参数。</param>
|
||
/// <param name="planningSpeedScale">规划速度倍率。</param>
|
||
/// <returns>包含规划轨迹、空触发时间轴和执行结果的结果包。</returns>
|
||
public PlannedExecutionBundle PlanOrdinaryTrajectory(
|
||
RobotProfile robot,
|
||
IReadOnlyList<IReadOnlyList<double>> waypoints,
|
||
TrajectoryExecutionOptions? options = null,
|
||
double planningSpeedScale = 1.0)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(robot);
|
||
ArgumentNullException.ThrowIfNull(waypoints);
|
||
options ??= new TrajectoryExecutionOptions();
|
||
var planningRobot = ApplyPlanningSpeedScale(robot, planningSpeedScale);
|
||
|
||
_logger?.LogInformation(
|
||
"PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}",
|
||
waypoints.Count, options.Method, planningSpeedScale);
|
||
|
||
var program = CreateProgram(
|
||
name: "ordinary-trajectory",
|
||
waypoints: waypoints,
|
||
shotFlags: Enumerable.Repeat(false, waypoints.Count),
|
||
offsetValues: Enumerable.Repeat(0, waypoints.Count),
|
||
addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty<int>()));
|
||
|
||
var method = ParseOrdinaryMethod(options.Method);
|
||
var request = new TrajectoryRequest(
|
||
robot: planningRobot,
|
||
program: program,
|
||
method: method,
|
||
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(
|
||
executionTrajectory,
|
||
shotTimeline,
|
||
denseJointTrajectory,
|
||
usedCache: false,
|
||
triggerSampleIndexOffsetCycles: 0);
|
||
|
||
_logger?.LogInformation(
|
||
"PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}",
|
||
result.Duration.TotalSeconds,
|
||
result.DenseJointTrajectory?.Count ?? 0);
|
||
|
||
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 对已经上传的飞拍轨迹执行自适应 ICSP 规划并生成触发时间轴。
|
||
/// </summary>
|
||
/// <param name="robot">当前机器人配置。</param>
|
||
/// <param name="uploaded">兼容层保存的上传轨迹。</param>
|
||
/// <param name="options">执行参数。</param>
|
||
/// <param name="settings">兼容层机器人设置。</param>
|
||
/// <param name="planningSpeedScale">规划速度倍率。</param>
|
||
/// <returns>包含规划轨迹、触发时间轴和执行结果的结果包。</returns>
|
||
public PlannedExecutionBundle PlanUploadedFlyshot(
|
||
RobotProfile robot,
|
||
ControllerClientCompatUploadedTrajectory uploaded,
|
||
FlyshotExecutionOptions? options = null,
|
||
CompatibilityRobotSettings? settings = null,
|
||
double? planningSpeedScale = null)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(robot);
|
||
ArgumentNullException.ThrowIfNull(uploaded);
|
||
options ??= new FlyshotExecutionOptions();
|
||
settings ??= CreateDefaultRobotSettings();
|
||
var effectivePlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale;
|
||
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
|
||
|
||
_logger?.LogInformation(
|
||
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}",
|
||
uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache, effectivePlanningSpeedScale, settings.SmoothStartStopTiming);
|
||
|
||
var program = CreateProgram(
|
||
name: uploaded.Name,
|
||
waypoints: uploaded.Waypoints,
|
||
shotFlags: uploaded.ShotFlags,
|
||
offsetValues: uploaded.OffsetValues,
|
||
addressGroups: uploaded.AddressGroups);
|
||
|
||
var method = ParseFlyshotMethod(options.Method);
|
||
var cacheKey = CreateFlyshotCacheKey(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale);
|
||
if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
|
||
{
|
||
_logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
|
||
|
||
// 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。
|
||
return cachedBundle;
|
||
}
|
||
|
||
var request = new TrajectoryRequest(
|
||
robot: planningRobot,
|
||
program: program,
|
||
method: method,
|
||
moveToStart: options.MoveToStart,
|
||
saveTrajectoryArtifacts: options.SaveTrajectory,
|
||
useCache: options.UseCache);
|
||
|
||
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,
|
||
denseJointTrajectory,
|
||
usedCache: false,
|
||
triggerSampleIndexOffsetCycles: settings.TriggerSampleIndexOffsetCycles);
|
||
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
||
|
||
_logger?.LogInformation(
|
||
"PlanUploadedFlyshot 完成: name={Name}, 时长={Duration}s, 触发事件数={TriggerCount}, 采样点数={SampleCount}",
|
||
uploaded.Name, result.Duration.TotalSeconds, result.TriggerTimeline.Count, result.DenseJointTrajectory?.Count ?? 0);
|
||
|
||
if (options.UseCache)
|
||
{
|
||
_flyshotCache[cacheKey] = bundle;
|
||
}
|
||
|
||
return bundle;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按普通轨迹执行接口约束解析 method 参数。
|
||
/// </summary>
|
||
/// <param name="method">旧 SDK 传入的方法名。</param>
|
||
/// <returns>领域层规划方法。</returns>
|
||
private static PlanningMethod ParseOrdinaryMethod(string method)
|
||
{
|
||
var normalized = NormalizeMethod(method);
|
||
return normalized switch
|
||
{
|
||
"icsp" => PlanningMethod.Icsp,
|
||
"doubles" => PlanningMethod.Doubles,
|
||
_ => throw new ArgumentException($"Unsupported ExecuteTrajectory method: {method}", nameof(method))
|
||
};
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按飞拍轨迹执行接口约束解析 method 参数。
|
||
/// </summary>
|
||
/// <param name="method">旧 SDK 传入的方法名。</param>
|
||
/// <returns>领域层规划方法。</returns>
|
||
private static PlanningMethod ParseFlyshotMethod(string method)
|
||
{
|
||
var normalized = NormalizeMethod(method);
|
||
return normalized switch
|
||
{
|
||
"icsp" => PlanningMethod.Icsp,
|
||
"self-adapt-icsp" => PlanningMethod.SelfAdaptIcsp,
|
||
"doubles" => PlanningMethod.Doubles,
|
||
_ => throw new ArgumentException($"Unsupported ExecuteFlyShotTraj method: {method}", nameof(method))
|
||
};
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按领域枚举分派到当前已经落地的规划器。
|
||
/// </summary>
|
||
/// <param name="request">规划请求。</param>
|
||
/// <param name="method">规划方法。</param>
|
||
/// <returns>规划轨迹。</returns>
|
||
private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method, CompatibilityRobotSettings? settings = null)
|
||
{
|
||
return method switch
|
||
{
|
||
PlanningMethod.Icsp => _icspPlanner.Plan(request),
|
||
PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request, settings?.AdaptIcspTryNum ?? 5),
|
||
PlanningMethod.Doubles => throw new NotSupportedException("doubles 轨迹规划尚未落地。"),
|
||
_ => throw new ArgumentOutOfRangeException(nameof(method), method, "未知轨迹规划方法。")
|
||
};
|
||
}
|
||
|
||
/// <summary>
|
||
/// 归一化旧 SDK 的 method 字符串。
|
||
/// </summary>
|
||
/// <param name="method">原始方法名。</param>
|
||
/// <returns>小写短横线方法名。</returns>
|
||
private static string NormalizeMethod(string method)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(method))
|
||
{
|
||
return "icsp";
|
||
}
|
||
|
||
return method.Trim().ToLowerInvariant();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 为已上传飞拍轨迹构造包含参数和轨迹内容的缓存键，避免同名覆盖后误用旧规划结果。
|
||
/// </summary>
|
||
/// <param name="robot">机器人配置。</param>
|
||
/// <param name="uploaded">上传轨迹。</param>
|
||
/// <param name="options">执行参数。</param>
|
||
/// <returns>缓存键。</returns>
|
||
private static string CreateFlyshotCacheKey(
|
||
RobotProfile robot,
|
||
ControllerClientCompatUploadedTrajectory uploaded,
|
||
FlyshotExecutionOptions options,
|
||
CompatibilityRobotSettings settings,
|
||
double planningSpeedScale)
|
||
{
|
||
var hash = new HashCode();
|
||
hash.Add(robot.Name, StringComparer.Ordinal);
|
||
hash.Add(planningSpeedScale);
|
||
hash.Add(uploaded.Name, StringComparer.Ordinal);
|
||
hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal);
|
||
hash.Add(options.MoveToStart);
|
||
hash.Add(options.SaveTrajectory);
|
||
hash.Add(settings.UseDo);
|
||
hash.Add(settings.IoKeepCycles);
|
||
hash.Add(settings.TriggerSampleIndexOffsetCycles);
|
||
hash.Add(settings.AdaptIcspTryNum);
|
||
hash.Add(settings.SmoothStartStopTiming);
|
||
|
||
foreach (var limit in robot.JointLimits)
|
||
{
|
||
hash.Add(limit.JointName, StringComparer.Ordinal);
|
||
hash.Add(limit.VelocityLimit);
|
||
hash.Add(limit.AccelerationLimit);
|
||
hash.Add(limit.JerkLimit);
|
||
}
|
||
|
||
foreach (var waypoint in uploaded.Waypoints)
|
||
{
|
||
foreach (var value in waypoint)
|
||
{
|
||
hash.Add(value);
|
||
}
|
||
}
|
||
|
||
foreach (var flag in uploaded.ShotFlags)
|
||
{
|
||
hash.Add(flag);
|
||
}
|
||
|
||
foreach (var offset in uploaded.OffsetValues)
|
||
{
|
||
hash.Add(offset);
|
||
}
|
||
|
||
foreach (var group in uploaded.AddressGroups)
|
||
{
|
||
foreach (var address in group)
|
||
{
|
||
hash.Add(address);
|
||
}
|
||
}
|
||
|
||
return hash.ToHashCode().ToString("X8");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造编排器直接调用时的默认兼容配置，保持既有单元测试中的 DO 生成行为。
|
||
/// </summary>
|
||
/// <returns>默认机器人兼容配置。</returns>
|
||
private static CompatibilityRobotSettings CreateDefaultRobotSettings()
|
||
{
|
||
return new CompatibilityRobotSettings(
|
||
useDo: true,
|
||
ioAddresses: Array.Empty<int>(),
|
||
ioKeepCycles: 0,
|
||
triggerSampleIndexOffsetCycles: 7,
|
||
accLimitScale: 1.0,
|
||
jerkLimitScale: 1.0,
|
||
adaptIcspTryNum: 5);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按运行配置决定是否对规划结果做执行前时间轴重映射。
|
||
/// </summary>
|
||
/// <param name="plannedTrajectory">规划阶段得到的轨迹。</param>
|
||
/// <param name="settings">当前 RobotConfig.json 解析出的兼容设置。</param>
|
||
/// <returns>运行时真正用于采样和触发的轨迹。</returns>
|
||
private static PlannedTrajectory ApplyExecutionTiming(PlannedTrajectory plannedTrajectory, CompatibilityRobotSettings settings)
|
||
{
|
||
// legacy-fit 模式需要严格保留 waypoint.txt 反推出的节点时间，不能再二次改写时间轴。
|
||
return settings.SmoothStartStopTiming
|
||
? ApplySmoothStartStopTiming(plannedTrajectory)
|
||
: plannedTrajectory;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按规划全局速度倍率生成规划专用机器人约束。
|
||
/// </summary>
|
||
/// <param name="robot">原始机器人约束。</param>
|
||
/// <param name="planningSpeedScale">规划阶段的全局速度倍率，1.0 表示不额外缩放。</param>
|
||
/// <returns>已按速度倍率缩放后的规划机器人约束。</returns>
|
||
private static RobotProfile ApplyPlanningSpeedScale(RobotProfile robot, double planningSpeedScale)
|
||
{
|
||
if (double.IsNaN(planningSpeedScale) || double.IsInfinity(planningSpeedScale) || planningSpeedScale <= 0.0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。");
|
||
}
|
||
|
||
if (Math.Abs(planningSpeedScale - 1.0) < 1e-12)
|
||
{
|
||
return robot;
|
||
}
|
||
|
||
// RVBUST 规划阶段会用独立限速倍率缩放有效限制；运行时 speedRatio 仍只负责 J519 下发重采样。
|
||
var scaledLimits = robot.JointLimits
|
||
.Select(limit => new JointLimit(
|
||
limit.JointName,
|
||
limit.VelocityLimit * planningSpeedScale,
|
||
limit.AccelerationLimit * planningSpeedScale * planningSpeedScale,
|
||
limit.JerkLimit * planningSpeedScale * planningSpeedScale * planningSpeedScale))
|
||
.ToArray();
|
||
|
||
return new RobotProfile(
|
||
name: robot.Name,
|
||
modelPath: robot.ModelPath,
|
||
degreesOfFreedom: robot.DegreesOfFreedom,
|
||
jointLimits: scaledLimits,
|
||
jointCouplings: robot.JointCouplings,
|
||
servoPeriod: robot.ServoPeriod,
|
||
triggerPeriod: robot.TriggerPeriod);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 把兼容层输入数组转换成领域层 FlyshotProgram。
|
||
/// </summary>
|
||
/// <param name="name">轨迹名称。</param>
|
||
/// <param name="waypoints">关节路点。</param>
|
||
/// <param name="shotFlags">拍照标志。</param>
|
||
/// <param name="offsetValues">偏移周期。</param>
|
||
/// <param name="addressGroups">IO 地址组。</param>
|
||
/// <returns>领域层飞拍程序。</returns>
|
||
private static FlyshotProgram CreateProgram(
|
||
string name,
|
||
IEnumerable<IReadOnlyList<double>> waypoints,
|
||
IEnumerable<bool> shotFlags,
|
||
IEnumerable<int> offsetValues,
|
||
IEnumerable<IReadOnlyList<int>> addressGroups)
|
||
{
|
||
return new FlyshotProgram(
|
||
name: name,
|
||
waypoints: waypoints.Select(static waypoint => new JointWaypoint(waypoint)).ToArray(),
|
||
shotFlags: shotFlags.ToArray(),
|
||
offsetValues: offsetValues.ToArray(),
|
||
addressGroups: addressGroups.Select(static group => new IoAddressGroup(group)).ToArray());
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从规划轨迹和触发时间轴构造运行时可消费的稳定结果对象。
|
||
/// </summary>
|
||
/// <param name="plannedTrajectory">规划后的轨迹。</param>
|
||
/// <param name="shotTimeline">触发时间轴。</param>
|
||
/// <returns>运行时执行结果描述。</returns>
|
||
private static TrajectoryResult CreateResult(
|
||
PlannedTrajectory plannedTrajectory,
|
||
ShotTimeline shotTimeline,
|
||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||
bool usedCache,
|
||
int triggerSampleIndexOffsetCycles)
|
||
{
|
||
return new TrajectoryResult(
|
||
programName: plannedTrajectory.OriginalProgram.Name,
|
||
method: plannedTrajectory.Method,
|
||
isValid: true,
|
||
duration: TimeSpan.FromSeconds(plannedTrajectory.WaypointTimes[^1]),
|
||
shotEvents: shotTimeline.ShotEvents,
|
||
triggerTimeline: shotTimeline.TriggerTimeline,
|
||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||
failureReason: null,
|
||
usedCache: usedCache,
|
||
originalWaypointCount: plannedTrajectory.OriginalWaypointCount,
|
||
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount,
|
||
triggerSampleIndexOffsetCycles: triggerSampleIndexOffsetCycles,
|
||
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>
|
||
/// 为飞拍执行生成平滑起停时间轴（仅重定时，不改几何路点）。
|
||
/// 该方法保持 <see cref="PlannedTrajectory.PlannedWaypoints"/> 不变，只重映射 <see cref="PlannedTrajectory.WaypointTimes"/>，
|
||
/// 让轨迹在起点和终点附近具有更柔和的速度变化，从而降低“突然起步/突然收尾”带来的离散差分尖峰。
|
||
/// </summary>
|
||
/// <param name="plannedTrajectory">规划器输出的原始轨迹（通常是线性时间轴）。</param>
|
||
/// <returns>
|
||
/// 若满足平滑条件，则返回新的重定时轨迹；若路点数量不足或总时长无效，则直接返回输入轨迹。
|
||
/// </returns>
|
||
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];
|
||
// 强制固定边界：起点仍在 0，终点仍在总时长，保证任务总耗时不变。
|
||
smoothedTimes[0] = 0.0;
|
||
smoothedTimes[^1] = totalDuration;
|
||
|
||
for (var index = 1; index < originalTimes.Count - 1; index++)
|
||
{
|
||
// 把原始时刻归一化到 [0, 1]，用于统一时间律变换。
|
||
var normalizedProgress = originalTimes[index] / totalDuration;
|
||
// 先求完整平滑时间律对应的时刻，再与原线性时刻按比例混合。
|
||
// 这里采用弱平滑（blend=0.35）：保留大部分原节奏，仅轻微降低首尾突变。
|
||
var smoothedProgress = InvertSmoothStartStopProgress(normalizedProgress);
|
||
var blendedProgress = ((1.0 - SmoothStartStopBlend) * normalizedProgress)
|
||
+ (SmoothStartStopBlend * smoothedProgress);
|
||
smoothedTimes[index] = totalDuration * blendedProgress;
|
||
}
|
||
|
||
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>
|
||
/// 反解平滑时间律进度：给定目标进度 p，求满足 f(t)=p 的 t。
|
||
/// 其中 f(t) 由 <see cref="EvaluateSmoothStartStopProgress"/> 给出，是单调递增的 7 次 smootherstep 曲线，
|
||
/// 通过二分法可稳定得到对应的归一化时间，避免显式求高次方程根带来的复杂性和数值不稳定。
|
||
/// </summary>
|
||
/// <param name="normalizedProgress">目标归一化进度，理论范围 [0, 1]。</param>
|
||
/// <returns>与目标进度对应的归一化时间，范围 [0, 1]。</returns>
|
||
private static double InvertSmoothStartStopProgress(double normalizedProgress)
|
||
{
|
||
// 防御式裁剪，避免上游浮点误差将进度推到区间外。
|
||
var target = Math.Clamp(normalizedProgress, 0.0, 1.0);
|
||
var low = 0.0;
|
||
var high = 1.0;
|
||
// 固定迭代次数确保耗时可预测；40 次足以把区间收敛到工程可用精度。
|
||
for (var iteration = 0; iteration < 40; iteration++)
|
||
{
|
||
var middle = (low + high) / 2.0;
|
||
var progress = EvaluateSmoothStartStopProgress(middle);
|
||
// f(middle) 小于目标，说明根在右半区间；否则在左半区间。
|
||
if (progress < target)
|
||
{
|
||
low = middle;
|
||
}
|
||
else
|
||
{
|
||
high = middle;
|
||
}
|
||
}
|
||
|
||
return (low + high) / 2.0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算 7 次 smootherstep 的进度函数值 f(u)。
|
||
/// 该函数在 u=0 和 u=1 处具有更高阶导数连续性，可让起停段速度变化更平滑，
|
||
/// 适合用于飞拍轨迹的时间重参数化，减少离散导数在边界处的突变。
|
||
/// </summary>
|
||
/// <param name="normalizedTime">归一化时间 u，理论范围 [0, 1]。</param>
|
||
/// <returns>归一化进度 f(u)，范围 [0, 1]。</returns>
|
||
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);
|
||
}
|
||
}
|