✨ feat(*): 添加轨迹产物导出与规划速度倍率隔离
* 新增 FlyshotTrajectoryArtifactWriter,支持 saveTrajectory 将规划结果导出到 Config/Data/name(JointTraj、CartTraj、 ShotEvents 等) * RobotConfig 新增 PlanningSpeedScale,区分规划阶段限速倍率 与运行时 J519 下发倍率 * 轨迹缓存键纳入 planningSpeedScale,避免降速规划误用缓存 * 完善 FanucCommandClient 命令参数日志与状态通道重连 * 补充 RuntimeOrchestrationTests 覆盖产物导出与倍率隔离 * 更新 README 进度文档
This commit is contained in:
@@ -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>
|
||||
|
||||
Reference in New Issue
Block a user