feat(fanuc): 统一 speedRatio 执行倍率语义

* 将 speedRatio 前移到规划/准备阶段,运行时只消费已生成的 8ms 队列
* 区分旧格式规划导出与 ActualSend 实发诊断工件
* 补充普通轨迹、MoveJoint、飞拍队列和严格限幅回归测试
This commit is contained in:
2026-05-11 17:21:18 +08:00
parent d82128da4a
commit d120ef2a39
17 changed files with 1395 additions and 534 deletions

View File

@@ -405,14 +405,16 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
// 普通轨迹必须按调用方指定 method 规划,再把规划结果交给运行时执行。
var planningSpeedScale = RequireRobotSettings().PlanningSpeedScale;
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale);
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale, speedRatio);
_logger?.LogInformation(
"ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}",
"ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}",
bundle.Result.Method,
bundle.Result.Duration.TotalSeconds,
bundle.Result.IsValid,
bundle.Result.DenseJointTrajectory?.Count ?? 0,
planningSpeedScale);
planningSpeedScale,
speedRatio);
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
}
@@ -495,32 +497,40 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
var settings = RequireRobotSettings();
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio);
ExportFlyshotArtifactsIfRequested(name, options.SaveTrajectory, robot, bundle);
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale, speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
ExportFlyshotArtifactsIfRequested(
name,
options.SaveTrajectory,
robot,
trajectory,
options,
settings,
bundle,
settings.PlanningSpeedScale,
speedRatio);
_logger?.LogInformation(
"ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}",
"ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}",
name,
bundle.Result.Method,
bundle.Result.Duration.TotalSeconds,
bundle.Result.TriggerTimeline.Count,
bundle.Result.UsedCache,
settings.PlanningSpeedScale);
settings.PlanningSpeedScale,
speedRatio);
if (options.MoveToStart)
{
_logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点");
ExecuteMoveJointAndWaitLocked(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
EnsureFeedbackNearFlyshotStart(bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, name);
}
else
{
//检验当前机械臂的关节坐标与计划轨迹的第一个点之前的差异,如果差异过大.就不报警,不执行下去
var currentJointPositions = _runtime.GetJointPositions();
var targetJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[0].Positions;
var diff = currentJointPositions.Zip(targetJointPositions, (c, t) => Math.Abs(c - t)).Sum();
if (diff > 0.01)
// 正式飞拍前必须确认机器人反馈已经在轨迹起点附近,避免 J519 目标突变。
if (!IsFeedbackNearFlyshotStart(bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, name))
{
_logger?.LogWarning("ExecuteTrajectoryByName 当前关节坐标与计划轨迹的第一个点之前的差异过大 name={Name}", name);
return;
}
}
@@ -529,7 +539,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
if (options.Wait)
{
WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", bundle.Result.Duration);
var executionDuration = bundle.PreparedExecution is null
? bundle.Result.Duration
: TimeSpan.FromSeconds(bundle.PreparedExecution.FinalDurationSeconds);
WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", executionDuration);
}
}
@@ -561,6 +574,33 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
}
/// <summary>
/// 校验当前反馈是否接近飞拍起点;不接近时直接抛出兼容错误。
/// </summary>
private void EnsureFeedbackNearFlyshotStart(IReadOnlyList<double> targetJointPositions, string name)
{
if (!IsFeedbackNearFlyshotStart(targetJointPositions, name))
{
throw new InvalidOperationException("Robot feedback is not near flyshot start.");
}
}
/// <summary>
/// 检查当前机械臂关节反馈与计划轨迹第一个点之间的差异。
/// </summary>
private bool IsFeedbackNearFlyshotStart(IReadOnlyList<double> targetJointPositions, string name)
{
var currentJointPositions = _runtime.GetJointPositions();
var diff = currentJointPositions.Zip(targetJointPositions, (current, target) => Math.Abs(current - target)).Sum();
if (diff <= 0.01)
{
return true;
}
_logger?.LogWarning("ExecuteTrajectoryByName 当前关节坐标与计划轨迹的第一个点之间的差异过大 name={Name}, diff={Diff}", name, diff);
return false;
}
/// <summary>
/// 等待运行时报告当前运动结束,用于把 move_to_start 与正式飞拍轨迹串行化。
/// </summary>
@@ -620,15 +660,26 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
// 先通过规划校验避免静默接受非法参数,同时把轨迹信息强制刷写到本地 JSON。
var planningSettings = RequireRobotSettings();
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(useCache:false,saveTrajectory: true, method: method),
planningSettings,
planningSettings.PlanningSpeedScale);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio);
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
_logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地");
ExportFlyshotArtifactsIfRequested(name, saveTrajectory: true, robot, bundle);
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory: true,
robot,
trajectory,
new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
// var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
// var settings = _robotSettings ?? CreateDefaultRobotSettings();
@@ -658,14 +709,25 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
}
var planningSettings = RequireRobotSettings();
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
planningSettings.PlanningSpeedScale);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio);
ExportFlyshotArtifactsIfRequested(name, saveTrajectory, robot, bundle);
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory,
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
duration = bundle.Result.Duration;
_logger?.LogInformation(
@@ -785,20 +847,42 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
/// <param name="name">飞拍轨迹名称。</param>
/// <param name="saveTrajectory">是否导出规划结果点位。</param>
/// <param name="robot">当前机器人模型。</param>
/// <param name="bundle">规划结果包。</param>
/// <param name="uploaded">已上传的飞拍轨迹。</param>
/// <param name="options">本次规划选项。</param>
/// <param name="settings">当前机器人兼容设置。</param>
/// <param name="executionBundle">运行时和 ActualSend 诊断使用的规划结果包。</param>
/// <param name="planningSpeedScale">规划速度倍率。</param>
/// <param name="speedRatio">本次运行时速度倍率。</param>
private void ExportFlyshotArtifactsIfRequested(
string name,
bool saveTrajectory,
RobotProfile robot,
PlannedExecutionBundle bundle)
ControllerClientCompatUploadedTrajectory uploaded,
FlyshotExecutionOptions options,
CompatibilityRobotSettings settings,
PlannedExecutionBundle executionBundle,
double planningSpeedScale,
double speedRatio)
{
if (!saveTrajectory)
{
return;
}
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
_artifactWriter.WriteUploadedFlyshot(name, robot, bundle, speedRatio);
// 旧格式 Joint/Cart 导出必须只反映 planning_speed_scaleActualSend* 才反映当前运行时 speedRatio
var exportPlanningBundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
uploaded,
new FlyshotExecutionOptions(
moveToStart: options.MoveToStart,
useCache: false,
saveTrajectory: options.SaveTrajectory,
method: options.Method,
wait: options.Wait),
settings,
planningSpeedScale,
speedRatio: 1.0);
_artifactWriter.WriteUploadedFlyshot(name, robot, exportPlanningBundle, executionBundle, speedRatio);
}
/// <summary>
@@ -811,9 +895,11 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
{
var preparedExecution = FlyshotExecutionSendSequenceBuilder.Build(
robot,
bundle.ExecutionTrajectory,
bundle.Result,
robot.ServoPeriod.TotalSeconds,
speedRatio);
requestedSpeedRatio: speedRatio,
samplingSpeedRatio: 1.0);
var preparedResult = new TrajectoryResult(
programName: bundle.Result.ProgramName,
method: bundle.Result.Method,
@@ -829,7 +915,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
triggerSampleIndexOffsetCycles: bundle.Result.TriggerSampleIndexOffsetCycles,
denseJointTrajectory: bundle.Result.DenseJointTrajectory,
preparedFlyshotExecution: preparedExecution);
return new PlannedExecutionBundle(bundle.PlannedTrajectory, bundle.ShotTimeline, preparedResult, preparedExecution);
return new PlannedExecutionBundle(
bundle.PlannedTrajectory,
bundle.ShotTimeline,
preparedResult,
preparedExecution,
bundle.ExecutionTrajectory);
}
/// <summary>