FlyShotHost/src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs

using Flyshot.Core.Domain;
using Flyshot.Core.Planning.Kinematics;

namespace Flyshot.Core.Planning.Sampling;

/// <summary>
/// 从规划轨迹生成稠密关节/笛卡尔采样序列。
///
/// 为什么采样周期默认 0.016s？
/// ---
/// 真机伺服周期是 8ms（125Hz），但规划层通常以 16ms（约 60Hz）输出到上层。
/// 0.016 是一个经验默认值，与真机观测到的稠密轨迹间隔一致。
/// </summary>
public static class TrajectorySampler
{
    /// <summary>
    /// 生成稠密关节轨迹采样，每行格式：[time, j1, j2, ..., jN]，保留 6 位小数。
    /// </summary>
    public static IReadOnlyList<IReadOnlyList<double>> SampleJointTrajectory(
        PlannedTrajectory trajectory,
        double samplePeriod = 0.016,
        int decimals = 6,
        bool smoothStartStop = false)
    {
        var spline = RebuildSpline(trajectory);
        double duration = trajectory.WaypointTimes[^1];
        var times = GenerateSampleTimes(duration, samplePeriod);
        var result = new List<IReadOnlyList<double>>(times.Count);

        foreach (var t in times)
        {
            var evaluationTime = smoothStartStop
                ? MapSmoothStartStopEvaluationTime(t, duration)
                : t;
            var pos = spline.Evaluate(evaluationTime);
            var row = new List<double>(pos.Length + 1);
            row.Add(Math.Round(t, decimals));
            foreach (var value in pos)
            {
                row.Add(Math.Round(value, decimals));
            }

            result.Add(row);
        }

        return result;
    }

    /// <summary>
    /// 生成稠密笛卡尔轨迹采样，每行格式：[time, x, y, z, qx, qy, qz, qw]，保留 6 位小数。
    /// </summary>
    public static IReadOnlyList<IReadOnlyList<double>> SampleCartesianTrajectory(
        PlannedTrajectory trajectory,
        RobotKinematicsModel kinematicsModel,
        double samplePeriod = 0.016,
        int decimals = 6,
        bool smoothStartStop = false)
    {
        var spline = RebuildSpline(trajectory);
        double duration = trajectory.WaypointTimes[^1];
        var times = GenerateSampleTimes(duration, samplePeriod);
        var result = new List<IReadOnlyList<double>>(times.Count);

        foreach (var t in times)
        {
            var evaluationTime = smoothStartStop
                ? MapSmoothStartStopEvaluationTime(t, duration)
                : t;
            var jointPos = spline.Evaluate(evaluationTime);
            var pose = RobotKinematics.ForwardKinematics(kinematicsModel, jointPos);
            var row = new List<double>(pose.Length + 1);
            row.Add(Math.Round(t, decimals));
            foreach (var value in pose)
            {
                row.Add(Math.Round(value, decimals));
            }

            result.Add(row);
        }

        return result;
    }

    /// <summary>
    /// 从规划轨迹重建 CubicSplineInterpolator 用于稠密采样。
    /// </summary>
    private static CubicSplineInterpolator RebuildSpline(PlannedTrajectory trajectory)
    {
        var times = trajectory.WaypointTimes.ToArray();
        var values = trajectory.PlannedWaypoints.Select(wp => wp.Positions.ToArray()).ToArray();
        return new CubicSplineInterpolator(times, values);
    }

    /// <summary>
    /// 生成采样时间点，强制包含终点。
    /// </summary>
    private static IReadOnlyList<double> GenerateSampleTimes(double duration, double samplePeriod)
    {
        var times = new List<double>();
        double t = 0.0;
        while (t < duration - 1e-12)
        {
            times.Add(t);
            t += samplePeriod;
        }

        if (times.Count == 0 || Math.Abs(times[^1] - duration) > 1e-12)
        {
            times.Add(duration);
        }

        return times;
    }

    /// <summary>
    /// 把线性采样时间映射为整段平滑起停的评估时间。
    /// 使用 7 次 smootherstep 时间律，让起点和终点的一到三阶导都自然收敛到 0。
    /// </summary>
    private static double MapSmoothStartStopEvaluationTime(double sampleTime, double duration)
    {
        if (duration <= 0.0)
        {
            return 0.0;
        }

        var normalizedTime = Math.Clamp(sampleTime / duration, 0.0, 1.0);
        var u2 = normalizedTime * normalizedTime;
        var u3 = u2 * normalizedTime;
        var u4 = u3 * normalizedTime;
        var u5 = u4 * normalizedTime;
        var u6 = u5 * normalizedTime;
        var u7 = u6 * normalizedTime;
        var progress = (35.0 * u4) - (84.0 * u5) + (70.0 * u6) - (20.0 * u7);
        return duration * progress;
    }
}