✨ 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:
216
src/Flyshot.Core.Config/RobotModelLoader.cs
Normal file
216
src/Flyshot.Core.Config/RobotModelLoader.cs
Normal file
@@ -0,0 +1,216 @@
|
||||
using System.Text;
|
||||
using System.Text.Json;
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Core.Config;
|
||||
|
||||
/// <summary>
|
||||
/// 从旧版 .robot(GLB) 文件中提取关节限制、模型名和 couple 元数据。
|
||||
/// </summary>
|
||||
public sealed class RobotModelLoader
|
||||
{
|
||||
private const uint JsonChunkType = 0x4E4F534A;
|
||||
|
||||
/// <summary>
|
||||
/// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。
|
||||
/// </summary>
|
||||
/// <param name="modelPath">.robot 文件路径。</param>
|
||||
/// <param name="accLimitScale">加速度全局倍率。</param>
|
||||
/// <param name="jerkLimitScale">Jerk 全局倍率。</param>
|
||||
/// <returns>包含关节限制和 couple 信息的 RobotProfile。</returns>
|
||||
public RobotProfile LoadProfile(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(modelPath))
|
||||
{
|
||||
throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
|
||||
}
|
||||
|
||||
if (accLimitScale <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(accLimitScale), "加速度倍率必须大于 0。");
|
||||
}
|
||||
|
||||
if (jerkLimitScale <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
|
||||
}
|
||||
|
||||
var resolvedModelPath = Path.GetFullPath(modelPath);
|
||||
var jsonText = ReadJsonChunk(resolvedModelPath);
|
||||
using var document = JsonDocument.Parse(jsonText);
|
||||
|
||||
var robotBody = FindRobotBody(document.RootElement);
|
||||
var profileName = robotBody.TryGetProperty("name", out var nameElement)
|
||||
? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
|
||||
: Path.GetFileNameWithoutExtension(resolvedModelPath);
|
||||
|
||||
var jointLimits = new List<JointLimit>();
|
||||
var jointCouplings = new List<JointCoupling>();
|
||||
|
||||
foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray())
|
||||
{
|
||||
if (!IsPlanningJoint(jointElement))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var jointName = jointElement.GetProperty("name").GetString() ?? throw new InvalidDataException("关节缺少 name。");
|
||||
var limitElement = jointElement.GetProperty("limit");
|
||||
|
||||
jointLimits.Add(new JointLimit(
|
||||
jointName: jointName,
|
||||
velocityLimit: limitElement.GetProperty("velocity").GetDouble(),
|
||||
accelerationLimit: limitElement.GetProperty("acceleration").GetDouble() * accLimitScale,
|
||||
jerkLimit: limitElement.GetProperty("jerk").GetDouble() * jerkLimitScale));
|
||||
|
||||
if (jointElement.TryGetProperty("couple", out var coupleElement))
|
||||
{
|
||||
var masterJointName = coupleElement.GetProperty("master_joint").GetString()
|
||||
?? throw new InvalidDataException($"关节 {jointName} 的 couple 缺少 master_joint。");
|
||||
|
||||
jointCouplings.Add(new JointCoupling(
|
||||
slaveJointName: jointName,
|
||||
masterJointName: masterJointName,
|
||||
multiplier: coupleElement.TryGetProperty("multiplier", out var multiplierElement) ? multiplierElement.GetDouble() : 0.0,
|
||||
offset: coupleElement.TryGetProperty("offset", out var offsetElement) ? offsetElement.GetDouble() : 0.0));
|
||||
}
|
||||
}
|
||||
|
||||
return new RobotProfile(
|
||||
name: profileName,
|
||||
modelPath: resolvedModelPath,
|
||||
degreesOfFreedom: jointLimits.Count,
|
||||
jointLimits: jointLimits,
|
||||
jointCouplings: jointCouplings,
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从 GLB 文件中提取 JSON chunk 文本。
|
||||
/// </summary>
|
||||
private static string ReadJsonChunk(string modelPath)
|
||||
{
|
||||
using var stream = File.OpenRead(modelPath);
|
||||
using var reader = new BinaryReader(stream, Encoding.UTF8, leaveOpen: false);
|
||||
|
||||
var magic = Encoding.ASCII.GetString(reader.ReadBytes(4));
|
||||
if (!string.Equals(magic, "glTF", StringComparison.Ordinal))
|
||||
{
|
||||
throw new InvalidDataException($"{modelPath} 不是合法的 GLB 文件。");
|
||||
}
|
||||
|
||||
var version = reader.ReadUInt32();
|
||||
if (version != 2)
|
||||
{
|
||||
throw new NotSupportedException($"当前仅支持 GLB 2.0，实际版本为 {version}。");
|
||||
}
|
||||
|
||||
var totalLength = reader.ReadUInt32();
|
||||
while (stream.Position < totalLength)
|
||||
{
|
||||
var chunkLength = reader.ReadUInt32();
|
||||
var chunkType = reader.ReadUInt32();
|
||||
var chunkBytes = reader.ReadBytes((int)chunkLength);
|
||||
if (chunkType == JsonChunkType)
|
||||
{
|
||||
return Encoding.UTF8.GetString(chunkBytes);
|
||||
}
|
||||
}
|
||||
|
||||
throw new InvalidDataException($"{modelPath} 不包含 JSON chunk。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在 robotics.bodies 中找到 type=2 的机器人主体。
|
||||
/// </summary>
|
||||
private static JsonElement FindRobotBody(JsonElement root)
|
||||
{
|
||||
var bodies = root
|
||||
.GetProperty("scenes")[0]
|
||||
.GetProperty("extras")
|
||||
.GetProperty("rvbust")
|
||||
.GetProperty("robotics")
|
||||
.GetProperty("bodies");
|
||||
|
||||
foreach (var body in bodies.EnumerateArray())
|
||||
{
|
||||
if (body.TryGetProperty("type", out var typeElement) && typeElement.GetInt32() == 2)
|
||||
{
|
||||
return body;
|
||||
}
|
||||
}
|
||||
|
||||
throw new InvalidDataException("未在 .robot 文件中找到 type=2 的机器人主体。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载 .robot 文件并生成运动学侧需要的完整几何模型。
|
||||
/// </summary>
|
||||
/// <param name="modelPath">.robot 文件路径。</param>
|
||||
/// <returns>包含完整关节几何链的运动学模型。</returns>
|
||||
public RobotKinematicsModel LoadKinematicsModel(string modelPath)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(modelPath))
|
||||
{
|
||||
throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
|
||||
}
|
||||
|
||||
var resolvedModelPath = Path.GetFullPath(modelPath);
|
||||
var jsonText = ReadJsonChunk(resolvedModelPath);
|
||||
using var document = JsonDocument.Parse(jsonText);
|
||||
|
||||
var robotBody = FindRobotBody(document.RootElement);
|
||||
var profileName = robotBody.TryGetProperty("name", out var nameElement)
|
||||
? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
|
||||
: Path.GetFileNameWithoutExtension(resolvedModelPath);
|
||||
|
||||
var joints = new List<RobotJointGeometry>();
|
||||
foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray())
|
||||
{
|
||||
var jointName = jointElement.GetProperty("name").GetString()
|
||||
?? throw new InvalidDataException("关节缺少 name。");
|
||||
var jointType = jointElement.TryGetProperty("type", out var typeElement)
|
||||
? typeElement.GetInt32()
|
||||
: 0;
|
||||
var origin = jointElement.GetProperty("origin").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
|
||||
var axis = jointElement.GetProperty("axis").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
|
||||
// axis 字段有时存的是 4 元组 [x, y, z, scale]，取最后 3 个作为方向向量。
|
||||
var axisVector = axis.Length >= 3 ? axis[^3..] : axis;
|
||||
var originXyz = origin.Length >= 3 ? origin[..3] : origin;
|
||||
var originQuat = origin.Length >= 7 ? origin[3..7] : new double[] { 0.0, 0.0, 0.0, 1.0 };
|
||||
|
||||
string? coupleMaster = null;
|
||||
double coupleMultiplier = 0.0;
|
||||
double coupleOffset = 0.0;
|
||||
if (jointElement.TryGetProperty("couple", out var coupleElement))
|
||||
{
|
||||
coupleMaster = coupleElement.GetProperty("master_joint").GetString();
|
||||
coupleMultiplier = coupleElement.TryGetProperty("multiplier", out var m) ? m.GetDouble() : 0.0;
|
||||
coupleOffset = coupleElement.TryGetProperty("offset", out var o) ? o.GetDouble() : 0.0;
|
||||
}
|
||||
|
||||
joints.Add(new RobotJointGeometry(
|
||||
name: jointName,
|
||||
parent: jointElement.GetProperty("parent").GetString() ?? string.Empty,
|
||||
child: jointElement.GetProperty("child").GetString() ?? string.Empty,
|
||||
jointType: jointType,
|
||||
axis: axisVector,
|
||||
originXyz: originXyz,
|
||||
originQuatXyzw: originQuat,
|
||||
coupleMaster: coupleMaster,
|
||||
coupleMultiplier: coupleMultiplier,
|
||||
coupleOffset: coupleOffset));
|
||||
}
|
||||
|
||||
return new RobotKinematicsModel(name: profileName, joints: joints);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断当前 joint 是否属于规划侧需要保留的旋转关节。
|
||||
/// </summary>
|
||||
private static bool IsPlanningJoint(JsonElement jointElement)
|
||||
{
|
||||
return jointElement.TryGetProperty("type", out var typeElement) && typeElement.GetInt32() == 2;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user