xsxiaosa/FlyShotHost
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

♻️ refactor(compat): 替换 MoveJoint 时间律为解析式 7 阶平滑函数并添加离散限位校验

Browse Source 
* 将预捕获 alpha 数据表替换为解析式 7 阶平滑点到点时间律
  s(u)=35u⁴-84u⁵+70u⁶-20u⁷，形状系数按 1~3 阶导数最大值重算
* 新增离散限位校验：按真实 8ms 采样点反算速度/加速度/jerk，
  不满足时自动拉长总时长后重采样，最多迭代 10000 次
* 实发轨迹落盘：ActualSendJointTraj.txt（角度制）、
  ActualSendJerkStats.txt（点间跃度统计），按时间目录归档
* J519 AcceptsCommand 门控：只有机器人就绪时才发送下一帧，
  减少无效下发；状态日志附带最近发送目标关节轴
* FanucControllerRuntime 构造函数改为必选 ILogger 注入，
  确保 DI 解析时稳定拿到日志实例
* LegacyHttpApiController 移除已废弃的 ConnectServer 调用，
  EnableRobot 参数从 2 改为 4
* 新增跃度报警分析文档和六轴限值表，补充反馈远离拒绝测试

Co-authored-by: Copilot <copilot@github.com>
This commit is contained in:
yunxiao.zhu
2026-05-06 09:06:28 +08:00
parent af65ca03a0
commit b1710e5d01
13 changed files with 1654 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files

316
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
View File

@@ -12,9 +12,9 @@ namespace Flyshot.Core.Tests;
/// </summary>
public sealed class FanucControllerRuntimeDenseTests
{
private const double CapturedMvpointVelocityShapeCoefficient = 2.0759961613199973;
private const double CapturedMvpointAccelerationShapeCoefficient = 7.986313199999984;
private const double CapturedMvpointJerkShapeCoefficient = 36.12609273600853;
private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
private const double SmoothPtpJerkShapeCoefficient = 52.5;
/// <summary>
/// 验证真机 J519 发送按 8ms 实发周期、speed_ratio 轨迹时间步进，并输出角度制目标。
@@ -61,6 +61,84 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.Equal([0.0, 45.0, 90.0, 135.0, 180.0], commands.Select(static command => command.TargetJoints[0]));
}
/// <summary>
/// 验证真机稠密发送会把实际下发给机器人的整条点位与点间跃度按时间目录落盘。
/// </summary>
[Fact]
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_WritesActualSentPointsAndJerkStats()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
using var j519Client = new FanucJ519Client();
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
var robot = TestRobotFactory.CreateRobotProfile();
var programName = $"dense-send-{Guid.NewGuid():N}";
var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", programName);
try
{
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
j519Client.EnableCommandHistoryForTests();
ForceRealModeEnabled(runtime, speedRatio: 0.5);
var denseTrajectory = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var result = new TrajectoryResult(
programName: programName,
method: PlanningMethod.Icsp,
isValid: true,
duration: TimeSpan.FromSeconds(0.016),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 4,
plannedWaypointCount: 4,
denseJointTrajectory: denseTrajectory);
runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
var runDir = GetSingleDenseSendRunDirectory(outputRoot);
var pointsPath = Path.Combine(runDir, "ActualSendJointTraj.txt");
var jerkPath = Path.Combine(runDir, "ActualSendJerkStats.txt");
Assert.True(File.Exists(pointsPath));
Assert.True(File.Exists(jerkPath));
var pointLines = File.ReadAllLines(pointsPath);
var jerkLines = File.ReadAllLines(jerkPath);
Assert.Equal(commands.Count, pointLines.Length);
Assert.Equal(Math.Max(0, commands.Count - 1), jerkLines.Length);
var firstColumns = ParseColumns(pointLines[0]);
var lastColumns = ParseColumns(pointLines[^1]);
Assert.Equal(9, firstColumns.Length);
Assert.Equal(9, lastColumns.Length);
Assert.Equal(0.0, firstColumns[0], precision: 6);
Assert.Equal(180.0, lastColumns[1], precision: 6);
var firstJerkColumns = ParseColumns(jerkLines[0]);
Assert.Equal(10, firstJerkColumns.Length);
Assert.Equal(0.0, firstJerkColumns[0], precision: 6);
Assert.Equal(0.004, firstJerkColumns[2], precision: 6);
}
finally
{
if (Directory.Exists(outputRoot))
{
Directory.Delete(outputRoot, recursive: true);
}
}
}
/// <summary>
/// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。
/// </summary>
@@ -135,9 +213,9 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.True(speed07.DenseJointTrajectory!.Count > fullSpeed.DenseJointTrajectory!.Count);
Assert.True(speed05.DenseJointTrajectory!.Count > speed07.DenseJointTrajectory!.Count);
Assert.InRange(fullSpeed.Duration.TotalSeconds, 0.318, 0.322);
Assert.True(speed07.Duration.TotalSeconds >= 0.320);
Assert.InRange(speed05.Duration.TotalSeconds, 0.318, 0.322);
Assert.True(fullSpeed.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
Assert.True(speed07.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
}
[Fact]
@@ -395,7 +473,7 @@ public sealed class FanucControllerRuntimeDenseTests
}
[Fact]
public void MoveJointTrajectoryGenerator_MatchesCapturedMvpointAlphaLawAtSpeedOne()
public void MoveJointTrajectoryGenerator_GeneratesSmoothJerkLimitedPtpTrajectoryAtSpeedOne()
{
var robot = CreateMoveJointReferenceRobotProfile();
var startJoints = new[]
@@ -420,64 +498,46 @@ public sealed class FanucControllerRuntimeDenseTests
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
var rows = result.DenseJointTrajectory!;
Assert.Equal(41, rows.Count);
Assert.InRange(result.Duration.TotalSeconds, 0.318, 0.322);
Assert.True(rows.Count > 41, $"平滑 MoveJoint 不应再固定输出 41 点，actual={rows.Count}。");
Assert.True(result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
AssertAllJointJerksWithinLimits(rows, robot);
}
var expectedAlpha = new[]
/// <summary>
/// 验证报警样本中的 MoveJoint 预移动作不会因为点到点时间律重采样而产生任一轴跃度尖峰。
/// </summary>
[Fact]
public void MoveJointTrajectoryGenerator_ActualAlarmSampleKeepsJointOneJerkWithinLimit()
{
var robot = CreateMoveJointRuntimeLimitRobotProfile();
var startJoints = new[]
{
0.000000000000,
0.000012196163,
0.000106156906,
0.000764380061,
0.002550804028,
0.006029689194,
0.011765134027,
0.020321400844,
0.032262426551,
0.048152469303,
0.068555498563,
0.093895155669,
0.124210027377,
0.159174512929,
0.198230386318,
0.240813559900,
0.286359937276,
0.334305411725,
0.384085546646,
0.435136609163,
0.486894129077,
0.538794033110,
0.590272360135,
0.640764719629,
0.689707151220,
0.736535405849,
0.780685354316,
0.821592775628,
0.858693734065,
0.891423926949,
0.919286047395,
0.942156722091,
0.960255163676,
0.974119666692,
0.984314536393,
0.991403790959,
0.995951593494,
0.998522142663,
0.999679443354,
0.999987892657,
1.000000000000
DegreesToRadians(71.454618),
DegreesToRadians(0.688433),
DegreesToRadians(-1.074197),
DegreesToRadians(-0.869001),
DegreesToRadians(1.218057),
DegreesToRadians(0.547036)
};
var targetJoints = new[]
{
DegreesToRadians(60.546227),
DegreesToRadians(0.668344),
DegreesToRadians(-1.025155),
DegreesToRadians(-0.869105),
DegreesToRadians(1.231405),
DegreesToRadians(0.548197)
};
for (var index = 0; index < rows.Count; index++)
{
var actualDegrees = rows[index].Skip(1).Select(RadiansToDegrees).ToArray();
var alpha = ComputeLineAlpha(actualDegrees, startJoints, targetJoints);
Assert.Equal(expectedAlpha[index], alpha, precision: 6);
}
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
var rows = result.DenseJointTrajectory!;
AssertAllJointJerksWithinLimits(rows, robot);
}
[Fact]
public void MoveJointTrajectoryGenerator_DoesNotShortenBaseDurationWhenSpeedRatioDoesNotDivideWindow()
public void MoveJointTrajectoryGenerator_DoesNotShortenLimitDurationWhenSpeedRatioDoesNotDivideWindow()
{
var robot = CreateMoveJointReferenceRobotProfile();
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
@@ -486,7 +546,9 @@ public sealed class FanucControllerRuntimeDenseTests
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
var rows = result.DenseJointTrajectory!;
Assert.True(result.Duration.TotalSeconds >= 0.320, $"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
Assert.True(
result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints),
$"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
}
@@ -513,12 +575,11 @@ public sealed class FanucControllerRuntimeDenseTests
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
var rows = result.DenseJointTrajectory!;
var expectedVelocityDuration = Math.PI * CapturedMvpointVelocityShapeCoefficient / robot.JointLimits[0].VelocityLimit;
var expectedAccelerationDuration = Math.Sqrt(Math.PI * CapturedMvpointAccelerationShapeCoefficient / robot.JointLimits[0].AccelerationLimit);
var expectedJerkDuration = Math.Cbrt(Math.PI * CapturedMvpointJerkShapeCoefficient / robot.JointLimits[0].JerkLimit);
var expectedVelocityDuration = Math.PI * SmoothPtpVelocityShapeCoefficient / robot.JointLimits[0].VelocityLimit;
var expectedAccelerationDuration = Math.Sqrt(Math.PI * SmoothPtpAccelerationShapeCoefficient / robot.JointLimits[0].AccelerationLimit);
var expectedJerkDuration = Math.Cbrt(Math.PI * SmoothPtpJerkShapeCoefficient / robot.JointLimits[0].JerkLimit);
var expectedMinimumDuration = new[]
{
0.320,
expectedVelocityDuration,
expectedAccelerationDuration,
expectedJerkDuration
@@ -605,6 +666,111 @@ public sealed class FanucControllerRuntimeDenseTests
triggerPeriod: TimeSpan.FromMilliseconds(8));
}
/// <summary>
/// 创建当前运行配置下的 MoveJoint 限值模型，覆盖报警样本使用的 acc_limit/jerk_limit=0.74。
/// </summary>
private static RobotProfile CreateMoveJointRuntimeLimitRobotProfile()
{
const double limitScale = 0.74;
return new RobotProfile(
name: "FANUC_LR_Mate_200iD",
modelPath: "Models/FANUC_LR_Mate_200iD.robot",
degreesOfFreedom: 6,
jointLimits:
[
new JointLimit("J1", 6.45, 26.90 * limitScale, 224.22 * limitScale),
new JointLimit("J2", 5.41, 22.54 * limitScale, 187.86 * limitScale),
new JointLimit("J3", 7.15, 29.81 * limitScale, 248.46 * limitScale),
new JointLimit("J4", 9.59, 39.99 * limitScale, 333.30 * limitScale),
new JointLimit("J5", 9.51, 39.63 * limitScale, 330.27 * limitScale),
new JointLimit("J6", 17.45, 72.72 * limitScale, 606.01 * limitScale)
],
jointCouplings: Array.Empty<JointCoupling>(),
servoPeriod: TimeSpan.FromMilliseconds(8),
triggerPeriod: TimeSpan.FromMilliseconds(8));
}
/// <summary>
/// 以发送周期上的二阶后向差分估算单轴最大绝对跃度，用来复现 ActualSendJerkStats 的判定口径。
/// </summary>
private static double MaxAbsJointJerk(IReadOnlyList<IReadOnlyList<double>> rows, int jointIndex)
{
double? previousTime = null;
double? previousPosition = null;
double? previousVelocity = null;
double? previousAcceleration = null;
var maxAbsJerk = 0.0;
foreach (var row in rows)
{
var currentTime = row[0];
var currentPosition = row[jointIndex + 1];
if (previousTime is not null && previousPosition is not null)
{
var dt = currentTime - previousTime.Value;
Assert.True(dt > 0.0, "MoveJoint 稠密轨迹时间戳必须严格递增。");
var velocity = (currentPosition - previousPosition.Value) / dt;
var acceleration = previousVelocity is null ? 0.0 : (velocity - previousVelocity.Value) / dt;
if (previousAcceleration is not null)
{
var jerk = (acceleration - previousAcceleration.Value) / dt;
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(jerk));
}
previousVelocity = velocity;
previousAcceleration = acceleration;
}
previousTime = currentTime;
previousPosition = currentPosition;
}
return maxAbsJerk;
}
/// <summary>
/// 验证稠密 MoveJoint 轨迹按 8ms 差分后不会超过机器人每轴 jerk 限值。
/// </summary>
private static void AssertAllJointJerksWithinLimits(IReadOnlyList<IReadOnlyList<double>> rows, RobotProfile robot)
{
for (var jointIndex = 0; jointIndex < robot.DegreesOfFreedom; jointIndex++)
{
var peakJerk = MaxAbsJointJerk(rows, jointIndex);
var limit = robot.JointLimits[jointIndex].JerkLimit;
Assert.True(
peakJerk <= limit * 1.000001,
$"J{jointIndex + 1} jerk {peakJerk:F6} rad/s^3 exceeds limit {limit:F6} rad/s^3.");
}
}
/// <summary>
/// 按 7 阶平滑点到点时间律的速度、加速度、jerk 形状系数计算理论最小时长。
/// </summary>
private static double ExpectedSmoothPtpDuration(
RobotProfile robot,
IReadOnlyList<double> startJoints,
IReadOnlyList<double> targetJoints)
{
var duration = 0.0;
for (var index = 0; index < robot.DegreesOfFreedom; index++)
{
var distance = Math.Abs(targetJoints[index] - startJoints[index]);
if (distance <= 0.0)
{
continue;
}
var limit = robot.JointLimits[index];
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limit.VelocityLimit;
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limit.AccelerationLimit);
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limit.JerkLimit);
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
}
return duration;
}
private static double DegreesToRadians(double degrees)
{
return degrees * Math.PI / 180.0;
@@ -631,6 +797,30 @@ public sealed class FanucControllerRuntimeDenseTests
throw new TimeoutException("Timed out waiting for dense trajectory send task to finish.");
}
/// <summary>
/// 获取一次测试执行生成的唯一稠密发送记录目录。
/// </summary>
private static string GetSingleDenseSendRunDirectory(string outputRoot)
{
var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
Assert.True(Directory.Exists(denseSendRoot));
var runDirectories = Directory.GetDirectories(denseSendRoot);
Assert.Single(runDirectories);
return runDirectories[0];
}
/// <summary>
/// 解析空格分隔的纯文本数值列。
/// </summary>
private static double[] ParseColumns(string line)
{
return line
.Split(' ', StringSplitOptions.RemoveEmptyEntries)
.Select(static value => double.Parse(value))
.ToArray();
}
private static void SetPrivateField<T>(FanucControllerRuntime runtime, string name, T value)
{
var field = typeof(FanucControllerRuntime).GetField(name, BindingFlags.Instance | BindingFlags.NonPublic);