feat(*): 添加轨迹产物导出与规划速度倍率隔离

* 新增 FlyshotTrajectoryArtifactWriter,支持 saveTrajectory
  将规划结果导出到 Config/Data/name(JointTraj、CartTraj、
  ShotEvents 等)
* RobotConfig 新增 PlanningSpeedScale,区分规划阶段限速倍率
  与运行时 J519 下发倍率
* 轨迹缓存键纳入 planningSpeedScale,避免降速规划误用缓存
* 完善 FanucCommandClient 命令参数日志与状态通道重连
* 补充 RuntimeOrchestrationTests 覆盖产物导出与倍率隔离
* 更新 README 进度文档
This commit is contained in:
2026-04-30 13:52:09 +08:00
parent a6579f1e5b
commit 91c1494cde
20 changed files with 593 additions and 133 deletions

View File

@@ -38,15 +38,17 @@ public sealed class ControllerClientTrajectoryOrchestrator
public PlannedExecutionBundle PlanOrdinaryTrajectory(
RobotProfile robot,
IReadOnlyList<IReadOnlyList<double>> waypoints,
TrajectoryExecutionOptions? options = null)
TrajectoryExecutionOptions? options = null,
double planningSpeedScale = 1.0)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(waypoints);
options ??= new TrajectoryExecutionOptions();
var planningRobot = ApplyPlanningSpeedScale(robot, planningSpeedScale);
_logger?.LogInformation(
"PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}",
waypoints.Count, options.Method);
"PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}",
waypoints.Count, options.Method, planningSpeedScale);
var program = CreateProgram(
name: "ordinary-trajectory",
@@ -57,7 +59,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
var method = ParseOrdinaryMethod(options.Method);
var request = new TrajectoryRequest(
robot: robot,
robot: planningRobot,
program: program,
method: method,
saveTrajectoryArtifacts: options.SaveTrajectory);
@@ -84,16 +86,19 @@ public sealed class ControllerClientTrajectoryOrchestrator
RobotProfile robot,
ControllerClientCompatUploadedTrajectory uploaded,
FlyshotExecutionOptions? options = null,
CompatibilityRobotSettings? settings = null)
CompatibilityRobotSettings? settings = null,
double? planningSpeedScale = null)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(uploaded);
options ??= new FlyshotExecutionOptions();
settings ??= CreateDefaultRobotSettings();
var effectivePlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale;
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
_logger?.LogInformation(
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}",
uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache);
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}",
uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache, effectivePlanningSpeedScale);
var program = CreateProgram(
name: uploaded.Name,
@@ -103,7 +108,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
addressGroups: uploaded.AddressGroups);
var method = ParseFlyshotMethod(options.Method);
var cacheKey = CreateFlyshotCacheKey(robot, uploaded, options, settings);
var cacheKey = CreateFlyshotCacheKey(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale);
if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
{
_logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
@@ -115,7 +120,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
}
var request = new TrajectoryRequest(
robot: robot,
robot: planningRobot,
program: program,
method: method,
moveToStart: options.MoveToStart,
@@ -126,7 +131,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
var shotTimeline = _shotTimelineBuilder.Build(
plannedTrajectory,
holdCycles: settings.IoKeepCycles,
samplePeriod: robot.ServoPeriod,
samplePeriod: planningRobot.ServoPeriod,
useDo: settings.UseDo);
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
@@ -219,10 +224,12 @@ public sealed class ControllerClientTrajectoryOrchestrator
RobotProfile robot,
ControllerClientCompatUploadedTrajectory uploaded,
FlyshotExecutionOptions options,
CompatibilityRobotSettings settings)
CompatibilityRobotSettings settings,
double planningSpeedScale)
{
var hash = new HashCode();
hash.Add(robot.Name, StringComparer.Ordinal);
hash.Add(planningSpeedScale);
hash.Add(uploaded.Name, StringComparer.Ordinal);
hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal);
hash.Add(options.MoveToStart);
@@ -231,6 +238,14 @@ public sealed class ControllerClientTrajectoryOrchestrator
hash.Add(settings.IoKeepCycles);
hash.Add(settings.AdaptIcspTryNum);
foreach (var limit in robot.JointLimits)
{
hash.Add(limit.JointName, StringComparer.Ordinal);
hash.Add(limit.VelocityLimit);
hash.Add(limit.AccelerationLimit);
hash.Add(limit.JerkLimit);
}
foreach (var waypoint in uploaded.Waypoints)
{
foreach (var value in waypoint)
@@ -275,6 +290,43 @@ public sealed class ControllerClientTrajectoryOrchestrator
adaptIcspTryNum: 5);
}
/// <summary>
/// 按规划全局速度倍率生成规划专用机器人约束。
/// </summary>
/// <param name="robot">原始机器人约束。</param>
/// <param name="planningSpeedScale">规划阶段的全局速度倍率1.0 表示不额外缩放。</param>
/// <returns>已按速度倍率缩放后的规划机器人约束。</returns>
private static RobotProfile ApplyPlanningSpeedScale(RobotProfile robot, double planningSpeedScale)
{
if (double.IsNaN(planningSpeedScale) || double.IsInfinity(planningSpeedScale) || planningSpeedScale <= 0.0)
{
throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。");
}
if (Math.Abs(planningSpeedScale - 1.0) < 1e-12)
{
return robot;
}
// RVBUST 规划阶段会用独立限速倍率缩放有效限制;运行时 speedRatio 仍只负责 J519 下发重采样。
var scaledLimits = robot.JointLimits
.Select(limit => new JointLimit(
limit.JointName,
limit.VelocityLimit * planningSpeedScale,
limit.AccelerationLimit * planningSpeedScale * planningSpeedScale,
limit.JerkLimit * planningSpeedScale * planningSpeedScale * planningSpeedScale))
.ToArray();
return new RobotProfile(
name: robot.Name,
modelPath: robot.ModelPath,
degreesOfFreedom: robot.DegreesOfFreedom,
jointLimits: scaledLimits,
jointCouplings: robot.JointCouplings,
servoPeriod: robot.ServoPeriod,
triggerPeriod: robot.TriggerPeriod);
}
/// <summary>
/// 把兼容层输入数组转换成领域层 FlyshotProgram。
/// </summary>