yunxiao.zhu
c6829d214a
* 新增 J519 实发采样器，按 8ms 周期生成 timing/jerk 诊断行并完成 rad->deg 转换 * 兼容层产物导出补充 speedRatio，规划编排补齐 smoothStartStopTiming 与日志透传 * 配置与机型加载切换到运行目录 JSON 模型，并补齐 7L 展开模型与相关单元测试
261 lines
13 KiB
C#
261 lines
13 KiB
C#
using System.Text.Json;
|
||
using Flyshot.Core.Domain;
|
||
using Microsoft.Extensions.Logging;
|
||
|
||
namespace Flyshot.Core.Config;
|
||
|
||
/// <summary>
|
||
/// 从现场固化的机器人 JSON 模型中提取关节限制、几何链和 couple 元数据。
|
||
/// </summary>
|
||
public sealed class RobotModelLoader
|
||
{
|
||
private readonly ILogger<RobotModelLoader>? _logger;
|
||
|
||
/// <summary>
|
||
/// 初始化 RobotModelLoader。
|
||
/// </summary>
|
||
/// <param name="logger">日志记录器；允许 null。</param>
|
||
public RobotModelLoader(ILogger<RobotModelLoader>? logger = null)
|
||
{
|
||
_logger = logger;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 加载机器人 JSON 文件并生成规划侧可直接消费的 RobotProfile。
|
||
/// </summary>
|
||
/// <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("机器人 JSON 路径不能为空。", nameof(modelPath));
|
||
}
|
||
|
||
if (accLimitScale <= 0.0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(accLimitScale), "加速度倍率必须大于 0。");
|
||
}
|
||
|
||
if (jerkLimitScale <= 0.0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
|
||
}
|
||
|
||
_logger?.LogInformation("RobotModel JSON 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale);
|
||
|
||
var resolvedModelPath = Path.GetFullPath(modelPath);
|
||
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>();
|
||
|
||
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。");
|
||
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: multiplier,
|
||
offset: offset));
|
||
_logger?.LogInformation("关节 {JointName} 的耦合关系: 主关节={MasterJointName}, 比例={Multiplier}, 偏移={Offset}", jointName, masterJointName, multiplier, offset);
|
||
}
|
||
}
|
||
|
||
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,
|
||
modelPath: resolvedModelPath,
|
||
degreesOfFreedom: jointLimits.Count,
|
||
jointLimits: jointLimits,
|
||
jointCouplings: jointCouplings,
|
||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从机器人主体构造正运动学几何视图。
|
||
/// </summary>
|
||
private RobotKinematicsModel BuildKinematicsModel(JsonElement robotBody, string profileName)
|
||
{
|
||
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;
|
||
// 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;
|
||
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;
|
||
}
|
||
|
||
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: 父 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("几何模型构建完成: profileName={ProfileName}, jointCount={JointCount}", profileName, joints.Count);
|
||
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;
|
||
}
|
||
}
|