✨ feat: 初始化飞拍替换方案仓库骨架

* 建立 .NET 8 解决方案及分层项目结构
* 添加 Flyshot.Core.Domain 领域模型(机器人、轨迹、运动学)
* 添加 Flyshot.Core.Planning 规划层(ICSP、CubicSpline、采样器)
* 添加 Flyshot.Core.Triggering 触发时序与 IO 时间轴
* 添加 Flyshot.Core.Config 配置兼容与 .robot 解析
* 添加 Flyshot.Server.Host 最小宿主及 /healthz 端点
* 补充单元测试与集成测试项目
* 添加 CLAUDE.md、AGENTS.md、README.md 项目规范
This commit is contained in:
2026-04-23 17:35:37 +08:00
commit 4eeaa3fef3
47 changed files with 5140 additions and 0 deletions

View File

@@ -0,0 +1,106 @@
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)
{
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 pos = spline.Evaluate(t);
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)
{
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 jointPos = spline.Evaluate(t);
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;
}
}