✨ 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:
172
src/Flyshot.Core.Domain/RobotKinematicsModel.cs
Normal file
172
src/Flyshot.Core.Domain/RobotKinematicsModel.cs
Normal file
@@ -0,0 +1,172 @@
|
||||
using System.Text.Json.Serialization;
|
||||
|
||||
namespace Flyshot.Core.Domain;
|
||||
|
||||
/// <summary>
|
||||
/// 描述机器人运动学链所需的完整关节几何信息,从 .robot GLB 中提取。
|
||||
///
|
||||
/// 为什么与 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; }
|
||||
}
|
||||
Reference in New Issue
Block a user