FlyShotHost/src/Flyshot.Core.Domain/RobotKinematicsModel.cs

using System.Text.Json.Serialization;

namespace Flyshot.Core.Domain;

/// <summary>
/// 描述机器人运动学链所需的完整关节几何信息，从现场固化的机器人 JSON 中提取。
///
/// 为什么与 RobotProfile 分开？
/// ---
/// RobotProfile 只存规划侧需要的限速和 couple 元数据，是"规划约束视图"。
/// RobotKinematicsModel 存的是几何链（origin、axis、变换顺序），是"运动学视图"。
/// 两者生命周期和用途不同，分开可以避免规划层被迫依赖完整几何数据。
/// </summary>
public sealed class RobotKinematicsModel
{
    /// <summary>
    /// 初始化一份已验证的运动学模型。
    /// </summary>
    public RobotKinematicsModel(string name, IEnumerable<RobotJointGeometry> joints)
    {
        if (string.IsNullOrWhiteSpace(name))
        {
            throw new ArgumentException("机器人名称不能为空。", nameof(name));
        }

        ArgumentNullException.ThrowIfNull(joints);

        var copiedJoints = joints.ToArray();
        if (copiedJoints.Length == 0)
        {
            throw new ArgumentException("关节列表不能为空。", nameof(joints));
        }

        Name = name;
        Joints = copiedJoints;
    }

    /// <summary>
    /// 获取机器人名称。
    /// </summary>
    [JsonPropertyName("name")]
    public string Name { get; }

    /// <summary>
    /// 获取按运动学链顺序排列的关节几何列表。
    /// </summary>
    [JsonPropertyName("joints")]
    public IReadOnlyList<RobotJointGeometry> Joints { get; }
}

/// <summary>
/// 描述单个关节的几何属性，用于正运动学计算。
/// </summary>
public sealed class RobotJointGeometry
{
    /// <summary>
    /// 初始化一份已验证的关节几何描述。
    /// </summary>
    public RobotJointGeometry(
        string name,
        string parent,
        string child,
        int jointType,
        double[] axis,
        double[] originXyz,
        double[] originQuatXyzw,
        string? coupleMaster = null,
        double coupleMultiplier = 0.0,
        double coupleOffset = 0.0)
    {
        if (string.IsNullOrWhiteSpace(name))
        {
            throw new ArgumentException("关节名称不能为空。", nameof(name));
        }

        if (string.IsNullOrWhiteSpace(parent))
        {
            throw new ArgumentException("父节点名称不能为空。", nameof(parent));
        }

        if (string.IsNullOrWhiteSpace(child))
        {
            throw new ArgumentException("子节点名称不能为空。", nameof(child));
        }

        if (axis is null || axis.Length != 3)
        {
            throw new ArgumentException("关节轴必须是长度为 3 的数组。", nameof(axis));
        }

        if (originXyz is null || originXyz.Length != 3)
        {
            throw new ArgumentException("原点平移必须是长度为 3 的数组。", nameof(originXyz));
        }

        if (originQuatXyzw is null || originQuatXyzw.Length != 4)
        {
            throw new ArgumentException("原点旋转四元数必须是长度为 4 的数组（xyzw）。", nameof(originQuatXyzw));
        }

        Name = name;
        Parent = parent;
        Child = child;
        JointType = jointType;
        Axis = (double[])axis.Clone();
        OriginXyz = (double[])originXyz.Clone();
        OriginQuatXyzw = (double[])originQuatXyzw.Clone();
        CoupleMaster = coupleMaster;
        CoupleMultiplier = coupleMultiplier;
        CoupleOffset = coupleOffset;
    }

    /// <summary>
    /// 获取关节名称。
    /// </summary>
    [JsonPropertyName("name")]
    public string Name { get; }

    /// <summary>
    /// 获取父连杆名称。
    /// </summary>
    [JsonPropertyName("parent")]
    public string Parent { get; }

    /// <summary>
    /// 获取子连杆名称。
    /// </summary>
    [JsonPropertyName("child")]
    public string Child { get; }

    /// <summary>
    /// 获取关节类型：0=fixed, 1=prismatic, 2=revolute。
    /// </summary>
    [JsonPropertyName("jointType")]
    public int JointType { get; }

    /// <summary>
    /// 获取关节旋转轴（单位向量）。
    /// </summary>
    [JsonPropertyName("axis")]
    public double[] Axis { get; }

    /// <summary>
    /// 获取关节原点平移 [x, y, z]。
    /// </summary>
    [JsonPropertyName("originXyz")]
    public double[] OriginXyz { get; }

    /// <summary>
    /// 获取关节原点旋转四元数 [x, y, z, w]。
    /// </summary>
    [JsonPropertyName("originQuatXyzw")]
    public double[] OriginQuatXyzw { get; }

    /// <summary>
    /// 获取耦合主关节名称（如无则为 null）。
    /// </summary>
    [JsonPropertyName("coupleMaster")]
    public string? CoupleMaster { get; }

    /// <summary>
    /// 获取耦合乘数。
    /// </summary>
    [JsonPropertyName("coupleMultiplier")]
    public double CoupleMultiplier { get; }

    /// <summary>
    /// 获取耦合偏移。
    /// </summary>
    [JsonPropertyName("coupleOffset")]
    public double CoupleOffset { get; }
}