yunxiao.zhu
91c1494cde
* 新增 FlyshotTrajectoryArtifactWriter，支持 saveTrajectory 将规划结果导出到 Config/Data/name（JointTraj、CartTraj、 ShotEvents 等） * RobotConfig 新增 PlanningSpeedScale，区分规划阶段限速倍率 与运行时 J519 下发倍率 * 轨迹缓存键纳入 planningSpeedScale，避免降速规划误用缓存 * 完善 FanucCommandClient 命令参数日志与状态通道重连 * 补充 RuntimeOrchestrationTests 覆盖产物导出与倍率隔离 * 更新 README 进度文档
238 lines
10 KiB
C#
238 lines
10 KiB
C#
using System.Text;
|
|
using System.Text.Json;
|
|
using Flyshot.Core.Domain;
|
|
using Microsoft.Extensions.Logging;
|
|
|
|
namespace Flyshot.Core.Config;
|
|
|
|
/// <summary>
|
|
/// 从旧版 .robot(GLB) 文件中提取关节限制、模型名和 couple 元数据。
|
|
/// </summary>
|
|
public sealed class RobotModelLoader
|
|
{
|
|
private const uint JsonChunkType = 0x4E4F534A;
|
|
private readonly ILogger<RobotModelLoader>? _logger;
|
|
|
|
/// <summary>
|
|
/// 初始化 RobotModelLoader。
|
|
/// </summary>
|
|
/// <param name="logger">日志记录器；允许 null。</param>
|
|
public RobotModelLoader(ILogger<RobotModelLoader>? logger = null)
|
|
{
|
|
_logger = logger;
|
|
}
|
|
|
|
/// <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。");
|
|
}
|
|
|
|
_logger?.LogInformation("RobotModel 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale);
|
|
|
|
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));
|
|
}
|
|
}
|
|
|
|
_logger?.LogInformation(
|
|
"RobotModel 加载完成: profileName={ProfileName}, dof={Dof}, 关节限制数={JointLimitCount}, couple数={CouplingCount}, resolvedPath={ResolvedPath}",
|
|
profileName, jointLimits.Count, jointLimits.Count, jointCouplings.Count, resolvedModelPath);
|
|
|
|
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));
|
|
}
|
|
|
|
_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。");
|
|
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));
|
|
}
|
|
|
|
_logger?.LogInformation("RobotKinematicsModel 加载完成: profileName={ProfileName}, 关节数={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;
|
|
}
|
|
}
|