✨ feat(fanuc): 统一 speedRatio 执行倍率语义
* 将 speedRatio 前移到规划/准备阶段,运行时只消费已生成的 8ms 队列 * 区分旧格式规划导出与 ActualSend 实发诊断工件 * 补充普通轨迹、MoveJoint、飞拍队列和严格限幅回归测试
This commit is contained in:
@@ -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_scale;ActualSend* 才反映当前运行时 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>
|
||||
|
||||
Reference in New Issue
Block a user