137 lines
4.6 KiB
C#
137 lines
4.6 KiB
C#
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;
|
||
}
|
||
}
|