✨ feat(fanuc): 统一 speedRatio 执行倍率语义
* 将 speedRatio 前移到规划/准备阶段,运行时只消费已生成的 8ms 队列 * 区分旧格式规划导出与 ActualSend 实发诊断工件 * 补充普通轨迹、MoveJoint、飞拍队列和严格限幅回归测试
This commit is contained in:
@@ -1,4 +1,6 @@
|
||||
using Flyshot.Core.Planning.Sampling;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
@@ -43,6 +45,58 @@ public sealed class J519SendTrajectorySamplerTests
|
||||
Assert.Equal(180.0, samples[^1].JointsDegrees[0], precision: 6);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍 speed_ratio 安全队列从连续规划样条直接采样,而不是先把规划轨迹离散成稠密点后再线性插值。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void SamplePlannedTrajectory_DirectlyEvaluatesContinuousSplineForSpeedRatio()
|
||||
{
|
||||
var trajectory = CreateSingleJointTrajectory();
|
||||
|
||||
var samples = J519SendTrajectorySampler.SamplePlannedTrajectory(
|
||||
trajectory,
|
||||
servoPeriodSeconds: 0.008,
|
||||
speedRatio: 0.5);
|
||||
|
||||
var sample = Assert.Single(samples.Where(static item => Math.Abs(item.TrajectoryTime - 0.004) < 1e-9));
|
||||
Assert.Equal(0.008, sample.SendTime, precision: 6);
|
||||
Assert.Equal(0.004, sample.TrajectoryTime, precision: 6);
|
||||
Assert.NotEqual(45.0, sample.JointsDegrees[0], precision: 3);
|
||||
|
||||
var expected = new CubicSplineInterpolator(
|
||||
trajectory.WaypointTimes.ToArray(),
|
||||
trajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray())
|
||||
.Evaluate(0.004)[0] * 180.0 / Math.PI;
|
||||
Assert.Equal(expected, sample.JointsDegrees[0], precision: 6);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 speed_ratio 等价于执行侧时间轴拉长:原轨迹 0.8 倍执行应与时间整体拉长 1/0.8 的轨迹 1 倍执行一致。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void SamplePlannedTrajectory_SpeedRatioMatchesUniformPlanningTimeStretch()
|
||||
{
|
||||
var trajectory = CreateSingleJointTrajectory();
|
||||
var stretchedTrajectory = StretchTrajectoryTime(trajectory, stretchFactor: 1.0 / 0.8);
|
||||
|
||||
var ratioSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
|
||||
trajectory,
|
||||
servoPeriodSeconds: 0.008,
|
||||
speedRatio: 0.8);
|
||||
var stretchedSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
|
||||
stretchedTrajectory,
|
||||
servoPeriodSeconds: 0.008,
|
||||
speedRatio: 1.0);
|
||||
|
||||
Assert.Equal(stretchedSamples.Count, ratioSamples.Count);
|
||||
for (var index = 0; index < ratioSamples.Count; index++)
|
||||
{
|
||||
Assert.Equal(stretchedSamples[index].SendTime, ratioSamples[index].SendTime, precision: 6);
|
||||
Assert.Equal(stretchedSamples[index].TrajectoryTime * 0.8, ratioSamples[index].TrajectoryTime, precision: 6);
|
||||
Assert.Equal(stretchedSamples[index].JointsDegrees[0], ratioSamples[index].JointsDegrees[0], precision: 6);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证空稠密轨迹会直接暴露为调用错误,避免生成无意义下发点。
|
||||
/// </summary>
|
||||
@@ -96,4 +150,84 @@ public sealed class J519SendTrajectorySamplerTests
|
||||
|
||||
Assert.Equal([2.0, 0.016, 0.008, 0.5], row);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证倍率诊断行在保留旧 4 列的同时追加请求倍率和历史改写次数。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void BuildTimingRow_WithRatioDiagnostics_AppendsRequestedRatioAndIterationCount()
|
||||
{
|
||||
var sample = new J519SendSample(
|
||||
sampleIndex: 2,
|
||||
sendTime: 0.016,
|
||||
trajectoryTime: 0.0076,
|
||||
speedRatio: 0.475,
|
||||
jointsDegrees: [90.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
|
||||
var row = J519SendTrajectorySampler.BuildTimingRow(
|
||||
sample,
|
||||
requestedSpeedRatio: 0.5,
|
||||
stretchIterationCount: 1);
|
||||
|
||||
Assert.Equal([2.0, 0.016, 0.0076, 0.475, 0.5, 1.0], row);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造一个最小单关节规划轨迹,便于验证 speed_ratio 直接样条采样语义。
|
||||
/// </summary>
|
||||
private static PlannedTrajectory CreateSingleJointTrajectory()
|
||||
{
|
||||
var robot = new RobotProfile(
|
||||
name: "TestRobot",
|
||||
modelPath: "Models/Test.robot",
|
||||
degreesOfFreedom: 1,
|
||||
jointLimits: [new JointLimit("J1", 100.0, 1000.0, 10000.0)],
|
||||
jointCouplings: Array.Empty<JointCoupling>(),
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
var program = new FlyshotProgram(
|
||||
name: "spline-sample",
|
||||
waypoints:
|
||||
[
|
||||
new JointWaypoint([0.0]),
|
||||
new JointWaypoint([Math.PI / 2.0]),
|
||||
new JointWaypoint([Math.PI])
|
||||
],
|
||||
shotFlags: [false, false, false],
|
||||
offsetValues: [0, 0, 0],
|
||||
addressGroups:
|
||||
[
|
||||
new IoAddressGroup(Array.Empty<int>()),
|
||||
new IoAddressGroup(Array.Empty<int>()),
|
||||
new IoAddressGroup(Array.Empty<int>())
|
||||
]);
|
||||
|
||||
return new PlannedTrajectory(
|
||||
robot: robot,
|
||||
originalProgram: program,
|
||||
plannedWaypoints: program.Waypoints,
|
||||
waypointTimes: [0.0, 0.008, 0.016],
|
||||
segmentDurations: [0.008, 0.008],
|
||||
segmentScales: [1.0, 1.0],
|
||||
method: PlanningMethod.Icsp,
|
||||
iterations: 1,
|
||||
threshold: 0.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造统一拉长时间轴后的轨迹,模拟更低 planning_speed_scale 生成的等几何时间结果。
|
||||
/// </summary>
|
||||
private static PlannedTrajectory StretchTrajectoryTime(PlannedTrajectory trajectory, double stretchFactor)
|
||||
{
|
||||
return new PlannedTrajectory(
|
||||
robot: trajectory.Robot,
|
||||
originalProgram: trajectory.OriginalProgram,
|
||||
plannedWaypoints: trajectory.PlannedWaypoints,
|
||||
waypointTimes: trajectory.WaypointTimes.Select(time => time * stretchFactor).ToArray(),
|
||||
segmentDurations: trajectory.SegmentDurations.Select(duration => duration * stretchFactor).ToArray(),
|
||||
segmentScales: trajectory.SegmentScales.Select(scale => scale / stretchFactor).ToArray(),
|
||||
method: trajectory.Method,
|
||||
iterations: trajectory.Iterations,
|
||||
threshold: trajectory.Threshold);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user