✨ feat(*): 添加 J519 实发重采样与 JSON 机型模型

* 新增 J519 实发采样器，按 8ms 周期生成 timing/jerk 诊断行并完成 rad->deg 转换 * 兼容层产物导出补充 speedRatio，规划编排补齐 smoothStartStopTiming 与日志透传 * 配置与机型加载切换到运行目录 JSON 模型，并补齐 7L 展开模型与相关单元测试
2026-05-07 17:08:32 +08:00
parent 70b0ccd414
commit c6829d214a
26 changed files with 1417 additions and 409 deletions
--- a/src/Flyshot.Core.Config/RobotModelLoader.cs
+++ b/src/Flyshot.Core.Config/RobotModelLoader.cs
@@ -1,4 +1,3 @@
-using System.Text;
 using System.Text.Json;
 using Flyshot.Core.Domain;
 using Microsoft.Extensions.Logging;
@@ -6,11 +5,10 @@ using Microsoft.Extensions.Logging;
 namespace Flyshot.Core.Config;

 /// <summary>
-/// 从旧版 .robot(GLB) 文件中提取关节限制、模型名和 couple 元数据。
+/// 从现场固化的机器人 JSON 模型中提取关节限制、几何链和 couple 元数据。
 /// </summary>
 public sealed class RobotModelLoader
 {
-    private const uint JsonChunkType = 0x4E4F534A;
    private readonly ILogger<RobotModelLoader>? _logger;

    /// <summary>
@@ -23,17 +21,29 @@ public sealed class RobotModelLoader
    }

    /// <summary>
-    /// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。
+    /// 加载机器人 JSON 文件并生成规划侧可直接消费的 RobotProfile。
    /// </summary>
-    /// <param name="modelPath">.robot 文件路径。</param>
+    /// <param name="modelPath">机器人 JSON 文件路径。</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)
+    {
+        return LoadProfileAndKinematics(modelPath, accLimitScale, jerkLimitScale).Profile;
+    }
+
+    /// <summary>
+    /// 加载机器人 JSON 文件并一次性生成规划约束视图与运动学几何视图。
+    /// </summary>
+    /// <param name="modelPath">机器人 JSON 文件路径。</param>
+    /// <param name="accLimitScale">加速度全局倍率。</param>
+    /// <param name="jerkLimitScale">Jerk 全局倍率。</param>
+    /// <returns>包含规划约束视图和运动学几何视图的加载结果。</returns>
+    public LoadedRobotModel LoadProfileAndKinematics(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
    {
        if (string.IsNullOrWhiteSpace(modelPath))
        {
-            throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
+            throw new ArgumentException("机器人 JSON 路径不能为空。", nameof(modelPath));
        }

        if (accLimitScale <= 0.0)
@@ -46,17 +56,72 @@ public sealed class RobotModelLoader
            throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
        }

-        _logger?.LogInformation("RobotModel 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale);
+        _logger?.LogInformation("RobotModel JSON 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale);

        var resolvedModelPath = Path.GetFullPath(modelPath);
-        var jsonText = ReadJsonChunk(resolvedModelPath);
-        using var document = JsonDocument.Parse(jsonText);
+        using var document = JsonDocument.Parse(File.ReadAllText(resolvedModelPath));

        var robotBody = FindRobotBody(document.RootElement);
        var profileName = robotBody.TryGetProperty("name", out var nameElement)
            ? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
            : Path.GetFileNameWithoutExtension(resolvedModelPath);

+        var profile = BuildProfile(robotBody, profileName, resolvedModelPath, accLimitScale, jerkLimitScale);
+        var kinematicsModel = BuildKinematicsModel(robotBody, profileName);
+
+        _logger?.LogInformation(
+            "RobotModel JSON 加载完成: profileName={ProfileName}, dof={Dof}, 几何关节数={JointCount}, resolvedPath={ResolvedPath}",
+            profile.Name, profile.DegreesOfFreedom, kinematicsModel.Joints.Count, resolvedModelPath);
+
+        return new LoadedRobotModel(profile, kinematicsModel);
+    }
+
+    /// <summary>
+    /// 加载机器人 JSON 文件并生成运动学侧需要的完整几何模型。
+    /// </summary>
+    /// <param name="modelPath">机器人 JSON 文件路径。</param>
+    /// <returns>包含完整关节几何链的运动学模型。</returns>
+    public RobotKinematicsModel LoadKinematicsModel(string modelPath)
+    {
+        return LoadProfileAndKinematics(modelPath).KinematicsModel;
+    }
+
+    /// <summary>
+    /// 在 robotics.bodies 中找到当前现场机器人主体。
+    /// </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;
+            }
+        }
+
+        foreach (var body in bodies.EnumerateArray())
+        {
+            if (body.TryGetProperty("joints", out _) && body.TryGetProperty("name", out _))
+            {
+                return body;
+            }
+        }
+
+        throw new InvalidDataException("未在机器人 JSON 中找到包含 joints 的机器人主体。");
+    }
+
+    /// <summary>
+    /// 从机器人主体构造规划约束视图。
+    /// </summary>
+    private RobotProfile BuildProfile(JsonElement robotBody, string profileName, string resolvedModelPath, double accLimitScale, double jerkLimitScale)
+    {
        var jointLimits = new List<JointLimit>();
        var jointCouplings = new List<JointCoupling>();

@@ -80,18 +145,21 @@ public sealed class RobotModelLoader
            {
                var masterJointName = coupleElement.GetProperty("master_joint").GetString()
                    ?? throw new InvalidDataException($"关节 {jointName} 的 couple 缺少 master_joint。");
-
+                var multiplier = coupleElement.TryGetProperty("multiplier", out var multiplierElement) ? multiplierElement.GetDouble() : 0.0;
+                var offset = coupleElement.TryGetProperty("offset", out var offsetElement) ? offsetElement.GetDouble() : 0.0;
                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));
+                    multiplier: multiplier,
+                    offset: offset));
+                _logger?.LogInformation("关节 {JointName} 的耦合关系: 主关节={MasterJointName}, 比例={Multiplier}, 偏移={Offset}", jointName, masterJointName, multiplier, offset);
            }
        }

-        _logger?.LogInformation(
-            "RobotModel 加载完成: profileName={ProfileName}, dof={Dof}, 关节限制数={JointLimitCount}, couple数={CouplingCount}, resolvedPath={ResolvedPath}",
-            profileName, jointLimits.Count, jointLimits.Count, jointCouplings.Count, resolvedModelPath);
+        foreach (var jointLimit in jointLimits)
+        {
+            _logger?.LogInformation("关节 {JointName} 的限制值: 速度={VelocityLimit}, 加速度={AccelerationLimit}, Jerk={JerkLimit}", jointLimit.JointName, jointLimit.VelocityLimit, jointLimit.AccelerationLimit, jointLimit.JerkLimit);
+        }

        return new RobotProfile(
            name: profileName,
@@ -104,101 +172,32 @@ public sealed class RobotModelLoader
    }

    /// <summary>
-    /// 从 GLB 文件中提取 JSON chunk 文本。
+    /// 从机器人主体构造正运动学几何视图。
    /// </summary>
-    private static string ReadJsonChunk(string modelPath)
+    private RobotKinematicsModel BuildKinematicsModel(JsonElement robotBody, string profileName)
    {
-        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));
-        }
-
-        _logger?.LogInformation("RobotKinematicsModel 开始加载: modelPath={ModelPath}", 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。");
+            // jointType: 关节类型编码；用于区分旋转关节/其他结构关节，后续几何链路可据此决定求解策略。
            var jointType = jointElement.TryGetProperty("type", out var typeElement)
                ? typeElement.GetInt32()
                : 0;
+            // origin: 关节局部原点配置，格式通常为 [x, y, z, qx, qy, qz, qw]，定义父坐标到关节坐标的位姿。
            var origin = jointElement.GetProperty("origin").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
+            // axis: 关节运动轴；部分模型为 4 元组 [x, y, z, scale]，其中方向向量用于正运动学雅可比计算。
            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;
+            // originXyz: 平移分量 (x,y,z)，用于构建关节在父链路下的位置偏移。
            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 };
+            // originQuat: 旋转分量 (qx,qy,qz,qw)，用于构建关节在父链路下的姿态；缺省时回退单位四元数。
+            var originQuat = origin.Length >= 7 ? origin[3..7] : [0.0, 0.0, 0.0, 1.0];

+            // coupleMaster/coupleMultiplier/coupleOffset: 关节耦合参数，描述 slave 关节如何由 master 关节线性映射得到。
+            // 典型关系: slave = master * multiplier + offset。
            string? coupleMaster = null;
            double coupleMultiplier = 0.0;
            double coupleOffset = 0.0;
@@ -209,21 +208,45 @@ public sealed class RobotModelLoader
                coupleOffset = coupleElement.TryGetProperty("offset", out var o) ? o.GetDouble() : 0.0;
            }

+            var parentLink = jointElement.GetProperty("parent").GetString() ?? string.Empty;
+            var childLink = jointElement.GetProperty("child").GetString() ?? string.Empty;
+            _logger?.LogInformation(
+                "几何关节解析: name={JointName}, parent={Parent}, child={Child}, type={JointType}, axis={Axis}, originXyz={OriginXyz}, originQuat={OriginQuat}, coupleMaster={CoupleMaster}, coupleMultiplier={CoupleMultiplier}, coupleOffset={CoupleOffset}",
+                jointName,
+                parentLink,
+                childLink,
+                jointType,
+                string.Join(", ", axisVector.Select(static v => v.ToString("G17"))),
+                string.Join(", ", originXyz.Select(static v => v.ToString("G17"))),
+                string.Join(", ", originQuat.Select(static v => v.ToString("G17"))),
+                coupleMaster ?? "<none>",
+                coupleMultiplier,
+                coupleOffset);
+
            joints.Add(new RobotJointGeometry(
+                // name: 当前关节名，作为几何链和耦合关系的主键。
                name: jointName,
-                parent: jointElement.GetProperty("parent").GetString() ?? string.Empty,
-                child: jointElement.GetProperty("child").GetString() ?? string.Empty,
+                // parent: 父 link 名称，用于串起机器人树结构。
+                parent: parentLink,
+                // child: 子 link 名称，标识该关节输出到哪个连杆。
+                child: childLink,
+                // jointType: 关节类型编码，供运动学模型区分计算路径。
                jointType: jointType,
+                // axis: 关节轴方向向量，决定旋转/平移沿哪个局部方向发生。
                axis: axisVector,
+                // originXyz: 关节原点平移分量。
                originXyz: originXyz,
+                // originQuatXyzw: 关节原点旋转四元数分量。
                originQuatXyzw: originQuat,
+                // coupleMaster: 耦合主关节名（无耦合时为 null）。
                coupleMaster: coupleMaster,
+                // coupleMultiplier: 耦合线性比例系数。
                coupleMultiplier: coupleMultiplier,
+                // coupleOffset: 耦合常量偏移量。
                coupleOffset: coupleOffset));
        }

-        _logger?.LogInformation("RobotKinematicsModel 加载完成: profileName={ProfileName}, 关节数={JointCount}", profileName, joints.Count);
-
+        _logger?.LogInformation("几何模型构建完成: profileName={ProfileName}, jointCount={JointCount}", profileName, joints.Count);
        return new RobotKinematicsModel(name: profileName, joints: joints);
    }