✨ 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:
106
src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs
Normal file
106
src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user