✨ feat(fanuc): 优化 J519 实时下发与飞拍起停整形

- 改为高优先级 J519 接收线程与复用缓冲区发送链路
- 增加稠密执行前的 J519 就绪重试与状态诊断
- 修正程序状态响应字段顺序与 EnableRobot 默认参数
- 为飞拍轨迹补充平滑起停时间轴与首尾整形验证
- 补充真实运行配置、报警窗口与边界对比测试
- 同步更新限值文档、分析脚本与 .NET 8 SDK 固定配置
This commit is contained in:
2026-05-06 22:37:31 +08:00
parent 783716ff44
commit 70b0ccd414
17 changed files with 1629 additions and 117 deletions

View File

@@ -19,7 +19,8 @@ public static class TrajectorySampler
public static IReadOnlyList<IReadOnlyList<double>> SampleJointTrajectory(
PlannedTrajectory trajectory,
double samplePeriod = 0.016,
int decimals = 6)
int decimals = 6,
bool smoothStartStop = false)
{
var spline = RebuildSpline(trajectory);
double duration = trajectory.WaypointTimes[^1];
@@ -28,7 +29,10 @@ public static class TrajectorySampler
foreach (var t in times)
{
var pos = spline.Evaluate(t);
var evaluationTime = smoothStartStop
? MapSmoothStartStopEvaluationTime(t, duration)
: t;
var pos = spline.Evaluate(evaluationTime);
var row = new List<double>(pos.Length + 1);
row.Add(Math.Round(t, decimals));
foreach (var value in pos)
@@ -49,7 +53,8 @@ public static class TrajectorySampler
PlannedTrajectory trajectory,
RobotKinematicsModel kinematicsModel,
double samplePeriod = 0.016,
int decimals = 6)
int decimals = 6,
bool smoothStartStop = false)
{
var spline = RebuildSpline(trajectory);
double duration = trajectory.WaypointTimes[^1];
@@ -58,7 +63,10 @@ public static class TrajectorySampler
foreach (var t in times)
{
var jointPos = spline.Evaluate(t);
var evaluationTime = smoothStartStop
? MapSmoothStartStopEvaluationTime(t, duration)
: t;
var jointPos = spline.Evaluate(evaluationTime);
var pose = RobotKinematics.ForwardKinematics(kinematicsModel, jointPos);
var row = new List<double>(pose.Length + 1);
row.Add(Math.Round(t, decimals));
@@ -103,4 +111,26 @@ public static class TrajectorySampler
return times;
}
/// <summary>
/// 把线性采样时间映射为整段平滑起停的评估时间。
/// 使用 7 次 smootherstep 时间律,让起点和终点的一到三阶导都自然收敛到 0。
/// </summary>
private static double MapSmoothStartStopEvaluationTime(double sampleTime, double duration)
{
if (duration <= 0.0)
{
return 0.0;
}
var normalizedTime = Math.Clamp(sampleTime / duration, 0.0, 1.0);
var u2 = normalizedTime * normalizedTime;
var u3 = u2 * normalizedTime;
var u4 = u3 * normalizedTime;
var u5 = u4 * normalizedTime;
var u6 = u5 * normalizedTime;
var u7 = u6 * normalizedTime;
var progress = (35.0 * u4) - (84.0 * u5) + (70.0 * u6) - (20.0 * u7);
return duration * progress;
}
}