yunxiao.zhu
70b0ccd414
- 改为高优先级 J519 接收线程与复用缓冲区发送链路 - 增加稠密执行前的 J519 就绪重试与状态诊断 - 修正程序状态响应字段顺序与 EnableRobot 默认参数 - 为飞拍轨迹补充平滑起停时间轴与首尾整形验证 - 补充真实运行配置、报警窗口与边界对比测试 - 同步更新限值文档、分析脚本与 .NET 8 SDK 固定配置
218 lines
8.5 KiB
C#
218 lines
8.5 KiB
C#
namespace Flyshot.ControllerClientCompat;
|
||
|
||
/// <summary>
|
||
/// 对飞拍稠密关节轨迹的首尾采样点做速度整形，降低启动和结束时的单步角度变化。
|
||
/// </summary>
|
||
internal static class FlyshotTrajectoryEdgeShaper
|
||
{
|
||
/// <summary>
|
||
/// 首尾整形默认覆盖的采样点数（含锚点）。
|
||
/// </summary>
|
||
internal const int DefaultEdgePointCount = 10;
|
||
|
||
/// <summary>
|
||
/// 对稠密关节轨迹做首尾整形，时间列保持不变，首段采用 ease-in，尾段采用 ease-out。
|
||
/// </summary>
|
||
/// <param name="denseJointTrajectory">输入稠密关节轨迹，每行格式为 [time, j1..jN]，关节单位为弧度。</param>
|
||
/// <param name="maxEdgeStepDegrees">保留旧签名兼容调用方；当前实现不再按角度阈值扩窗。</param>
|
||
/// <param name="maxWindowPoints">单侧整形覆盖的采样点数（含锚点），默认首尾各 10 点。</param>
|
||
/// <returns>经过首尾整形后的新轨迹；若不满足整形条件则返回原轨迹副本。</returns>
|
||
internal static IReadOnlyList<IReadOnlyList<double>> ShapeDenseJointTrajectory(
|
||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||
double maxEdgeStepDegrees = 0.0,
|
||
int maxWindowPoints = DefaultEdgePointCount)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(denseJointTrajectory);
|
||
if (denseJointTrajectory.Count == 0)
|
||
{
|
||
return Array.Empty<IReadOnlyList<double>>();
|
||
}
|
||
|
||
var copiedRows = denseJointTrajectory
|
||
.Select(static row => row.ToArray())
|
||
.ToArray();
|
||
if (copiedRows.Length < 5 || maxWindowPoints < 2)
|
||
{
|
||
return copiedRows;
|
||
}
|
||
|
||
var lastIndex = copiedRows.Length - 1;
|
||
var window = Math.Min(maxWindowPoints, lastIndex / 2);
|
||
if (window < 2)
|
||
{
|
||
return copiedRows;
|
||
}
|
||
|
||
// 以原始轨迹为参考估计窗口边界的速度，并在位移累计量上做单段单调整形，
|
||
// 目标是让首尾 10 点表现为更平滑的加减速，而不是硬匹配高阶导数导致振荡。
|
||
var originalRows = copiedRows
|
||
.Select(static row => row.ToArray())
|
||
.ToArray();
|
||
|
||
ApplyLeadingHermiteBlend(copiedRows, originalRows, window);
|
||
ApplyTrailingHermiteBlend(copiedRows, originalRows, window);
|
||
|
||
return copiedRows;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 对首段做单段 Hermite 累计位移整形：起点速度为 0，窗口末端按原轨迹边界速度接回中段。
|
||
/// </summary>
|
||
private static void ApplyLeadingHermiteBlend(double[][] rows, double[][] originalRows, int window)
|
||
{
|
||
var startRow = originalRows[0];
|
||
var endRow = originalRows[window];
|
||
var totalDuration = endRow[0] - startRow[0];
|
||
if (totalDuration <= 0.0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
for (var jointIndex = 1; jointIndex < startRow.Length; jointIndex++)
|
||
{
|
||
var delta = endRow[jointIndex] - startRow[jointIndex];
|
||
if (Math.Abs(delta) <= 1e-12)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
var endVelocity = EstimateVelocity(originalRows, window, jointIndex);
|
||
var normalizedEndSlope = ClampNormalizedSlope((endVelocity * totalDuration) / delta);
|
||
|
||
for (var index = 1; index < window; index++)
|
||
{
|
||
var normalizedTime = (rows[index][0] - startRow[0]) / totalDuration;
|
||
var shapedValue = startRow[jointIndex]
|
||
+ (delta * EvaluateHermiteProgress(normalizedTime, startSlope: 0.0, endSlope: normalizedEndSlope));
|
||
var blendWeight = Math.Pow(1.0 - normalizedTime, 2.0);
|
||
rows[index][jointIndex] = Lerp(originalRows[index][jointIndex], shapedValue, blendWeight);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 对尾段做单段 Hermite 累计位移整形：窗口起点按原轨迹边界速度接入，终点速度减到 0。
|
||
/// </summary>
|
||
private static void ApplyTrailingHermiteBlend(double[][] rows, double[][] originalRows, int window)
|
||
{
|
||
var startIndex = rows.Length - 1 - window;
|
||
var startRow = originalRows[startIndex];
|
||
var endRow = originalRows[^1];
|
||
var totalDuration = endRow[0] - startRow[0];
|
||
if (totalDuration <= 0.0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
for (var jointIndex = 1; jointIndex < startRow.Length; jointIndex++)
|
||
{
|
||
var delta = endRow[jointIndex] - startRow[jointIndex];
|
||
if (Math.Abs(delta) <= 1e-12)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
var startVelocity = EstimateVelocity(originalRows, startIndex, jointIndex);
|
||
var normalizedStartSlope = ClampNormalizedSlope((startVelocity * totalDuration) / delta);
|
||
|
||
for (var index = 1; index < window; index++)
|
||
{
|
||
var normalizedTime = (rows[startIndex + index][0] - startRow[0]) / totalDuration;
|
||
var shapedValue = startRow[jointIndex]
|
||
+ (delta * EvaluateHermiteProgress(normalizedTime, startSlope: normalizedStartSlope, endSlope: 0.0));
|
||
var blendWeight = Math.Pow(normalizedTime, 2.0);
|
||
rows[startIndex + index][jointIndex] = Lerp(originalRows[startIndex + index][jointIndex], shapedValue, blendWeight);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 估算给定行在原始轨迹上的一阶导，首尾退化为单边差分。
|
||
/// </summary>
|
||
private static double EstimateVelocity(double[][] rows, int index, int jointIndex)
|
||
{
|
||
if (index <= 0)
|
||
{
|
||
var dt = rows[1][0] - rows[0][0];
|
||
return dt <= 0.0 ? 0.0 : (rows[1][jointIndex] - rows[0][jointIndex]) / dt;
|
||
}
|
||
|
||
if (index >= rows.Length - 1)
|
||
{
|
||
var dt = rows[^1][0] - rows[^2][0];
|
||
return dt <= 0.0 ? 0.0 : (rows[^1][jointIndex] - rows[^2][jointIndex]) / dt;
|
||
}
|
||
|
||
var previousDt = rows[index][0] - rows[index - 1][0];
|
||
var nextDt = rows[index + 1][0] - rows[index][0];
|
||
if (previousDt <= 0.0 || nextDt <= 0.0)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
var backward = (rows[index][jointIndex] - rows[index - 1][jointIndex]) / previousDt;
|
||
var forward = (rows[index + 1][jointIndex] - rows[index][jointIndex]) / nextDt;
|
||
return (backward + forward) / 2.0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 估算给定行在原始轨迹上的二阶导，端点退化为 0 以避免放大边界噪声。
|
||
/// </summary>
|
||
private static double EstimateAcceleration(double[][] rows, int index, int jointIndex)
|
||
{
|
||
if (index <= 0 || index >= rows.Length - 1)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
var previousDt = rows[index][0] - rows[index - 1][0];
|
||
var nextDt = rows[index + 1][0] - rows[index][0];
|
||
if (previousDt <= 0.0 || nextDt <= 0.0)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
var backward = (rows[index][jointIndex] - rows[index - 1][jointIndex]) / previousDt;
|
||
var forward = (rows[index + 1][jointIndex] - rows[index][jointIndex]) / nextDt;
|
||
var averageDt = (previousDt + nextDt) / 2.0;
|
||
return averageDt <= 0.0 ? 0.0 : (forward - backward) / averageDt;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算 Hermite 累计位移曲线在 0..1 归一化时间上的进度值。
|
||
/// </summary>
|
||
private static double EvaluateHermiteProgress(double normalizedTime, double startSlope, double endSlope)
|
||
{
|
||
var u = Math.Clamp(normalizedTime, 0.0, 1.0);
|
||
var u2 = u * u;
|
||
var u3 = u2 * u;
|
||
var h00 = (2.0 * u3) - (3.0 * u2) + 1.0;
|
||
var h10 = u3 - (2.0 * u2) + u;
|
||
var h01 = (-2.0 * u3) + (3.0 * u2);
|
||
var h11 = u3 - u2;
|
||
return (h00 * 0.0) + (h10 * startSlope) + (h01 * 1.0) + (h11 * endSlope);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 把归一化边界斜率限制在单调 Hermite 常见的稳定区间内，避免过冲和窗口内振荡。
|
||
/// </summary>
|
||
private static double ClampNormalizedSlope(double normalizedSlope)
|
||
{
|
||
if (double.IsNaN(normalizedSlope) || double.IsInfinity(normalizedSlope))
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
return Math.Clamp(normalizedSlope, 0.0, 3.0);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在线性插值基础上做温和混合，避免首尾窗口为了追赶锚点而产生过大的局部跃度。
|
||
/// </summary>
|
||
private static double Lerp(double originalValue, double shapedValue, double weight)
|
||
{
|
||
var clampedWeight = Math.Clamp(weight, 0.0, 1.0);
|
||
return originalValue + ((shapedValue - originalValue) * clampedWeight);
|
||
}
|
||
}
|