354 lines
14 KiB
C#
354 lines
14 KiB
C#
using Flyshot.Core.Domain;
|
||
using Microsoft.Extensions.Logging;
|
||
|
||
namespace Flyshot.ControllerClientCompat;
|
||
|
||
/// <summary>
|
||
/// MovePose 直角坐标轨迹生成器。
|
||
/// 将起始 TCP 位姿到目标 TCP 位姿的单段运动，按保守直角速度、加速度和 jerk 限制生成 8ms 稠密点。
|
||
/// </summary>
|
||
internal static class MovePoseTrajectoryGenerator
|
||
{
|
||
/// <summary>
|
||
/// 直角位置轴最大速度，单位为 mm/s。
|
||
/// </summary>
|
||
private const double LinearVelocityLimit = 50.0;
|
||
|
||
/// <summary>
|
||
/// 直角位置轴最大加速度，单位为 mm/s^2。
|
||
/// </summary>
|
||
private const double LinearAccelerationLimit = 250.0;
|
||
|
||
/// <summary>
|
||
/// 直角位置轴最大 jerk，单位为 mm/s^3。
|
||
/// </summary>
|
||
private const double LinearJerkLimit = 1250.0;
|
||
|
||
/// <summary>
|
||
/// 姿态轴最大速度，单位为 deg/s。
|
||
/// </summary>
|
||
private const double AngularVelocityLimit = 10.0;
|
||
|
||
/// <summary>
|
||
/// 姿态轴最大加速度，单位为 deg/s^2。
|
||
/// </summary>
|
||
private const double AngularAccelerationLimit = 50.0;
|
||
|
||
/// <summary>
|
||
/// 姿态轴最大 jerk，单位为 deg/s^3。
|
||
/// </summary>
|
||
private const double AngularJerkLimit = 250.0;
|
||
|
||
/// <summary>
|
||
/// 7 阶平滑点到点时间律的一阶导数最大值。
|
||
/// </summary>
|
||
private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
|
||
|
||
/// <summary>
|
||
/// 7 阶平滑点到点时间律的二阶导数最大值。
|
||
/// </summary>
|
||
private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
|
||
|
||
/// <summary>
|
||
/// 7 阶平滑点到点时间律的三阶导数最大值。
|
||
/// </summary>
|
||
private const double SmoothPtpJerkShapeCoefficient = 52.5;
|
||
|
||
/// <summary>
|
||
/// 单次 MovePose 最大采样点数上限，避免极低速度倍率生成过大的队列。
|
||
/// </summary>
|
||
private const int MaxMovePoseSampleCount = 1_000_000;
|
||
|
||
/// <summary>
|
||
/// 离散限位校验允许的浮点容差。
|
||
/// </summary>
|
||
private const double DiscreteLimitTolerance = 1.000001;
|
||
|
||
/// <summary>
|
||
/// 离散限位校验失败时最多拉长的采样周期次数。
|
||
/// </summary>
|
||
private const int MaxDiscreteLimitStretchIterations = 10_000;
|
||
|
||
/// <summary>
|
||
/// 计算 MovePose 轨迹的完整结果。
|
||
/// </summary>
|
||
/// <param name="startPose">起始位姿 [x,y,z,w,p,r]，单位为 mm/deg。</param>
|
||
/// <param name="targetPose">目标位姿 [x,y,z,w,p,r]，单位为 mm/deg。</param>
|
||
/// <param name="servoPeriod">J519 伺服发送周期。</param>
|
||
/// <param name="speedRatio">速度倍率，必须大于 0。</param>
|
||
/// <param name="logger">可选诊断日志。</param>
|
||
/// <returns>包含稠密直角轨迹的规划结果。</returns>
|
||
public static TrajectoryResult CreateResult(
|
||
IReadOnlyList<double> startPose,
|
||
IReadOnlyList<double> targetPose,
|
||
TimeSpan servoPeriod,
|
||
double speedRatio,
|
||
ILogger? logger = null)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(startPose);
|
||
ArgumentNullException.ThrowIfNull(targetPose);
|
||
EnsurePoseVector(startPose, nameof(startPose));
|
||
EnsurePoseVector(targetPose, nameof(targetPose));
|
||
|
||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||
{
|
||
throw new InvalidOperationException("Speed ratio must be greater than zero for MovePose execution.");
|
||
}
|
||
|
||
var samplePeriodSeconds = servoPeriod.TotalSeconds;
|
||
if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
|
||
{
|
||
throw new InvalidOperationException("MovePose servo period must be a finite positive duration.");
|
||
}
|
||
|
||
var requestedDurationSeconds = ResolveDurationSeconds(startPose, targetPose, speedRatio);
|
||
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
|
||
var denseCartesianTrajectory = GenerateDenseTrajectory(startPose, targetPose, durationSeconds, samplePeriodSeconds);
|
||
|
||
var stretchCount = 0;
|
||
while (!SatisfiesDefaultCartesianLimits(denseCartesianTrajectory, speedRatio))
|
||
{
|
||
stretchCount++;
|
||
if (stretchCount > MaxDiscreteLimitStretchIterations)
|
||
{
|
||
throw new InvalidOperationException("MovePose duration cannot be stretched enough to satisfy Cartesian limits.");
|
||
}
|
||
|
||
// 离散差分以真实下发点为准；不满足时逐周期拉长后重采样。
|
||
durationSeconds = AlignDurationToServoStep(durationSeconds + samplePeriodSeconds, samplePeriodSeconds);
|
||
denseCartesianTrajectory = GenerateDenseTrajectory(startPose, targetPose, durationSeconds, samplePeriodSeconds);
|
||
}
|
||
|
||
logger?.LogDebug(
|
||
"MovePoseTrajectoryGenerator: requestedDuration={RequestedDuration:F4}s, duration={Duration:F4}s, speedRatio={SpeedRatio}, samplePeriod={SamplePeriod:F6}s, sampleCount={SampleCount}, stretchCount={StretchCount}",
|
||
requestedDurationSeconds,
|
||
durationSeconds,
|
||
speedRatio,
|
||
samplePeriodSeconds,
|
||
denseCartesianTrajectory.Count,
|
||
stretchCount);
|
||
|
||
return new TrajectoryResult(
|
||
programName: "move-pose",
|
||
method: PlanningMethod.Doubles,
|
||
isValid: true,
|
||
duration: TimeSpan.FromSeconds(durationSeconds),
|
||
shotEvents: Array.Empty<ShotEvent>(),
|
||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||
failureReason: null,
|
||
usedCache: false,
|
||
originalWaypointCount: 2,
|
||
plannedWaypointCount: denseCartesianTrajectory.Count,
|
||
denseCartesianTrajectory: denseCartesianTrajectory);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查稠密直角轨迹是否满足默认六维速度、加速度和 jerk 限制。
|
||
/// </summary>
|
||
/// <param name="rows">稠密轨迹行，每行为 [time,x,y,z,w,p,r]。</param>
|
||
/// <param name="speedRatio">生成该轨迹时使用的速度倍率。</param>
|
||
/// <returns>满足限制时返回 true，否则返回 false。</returns>
|
||
public static bool SatisfiesDefaultCartesianLimits(IReadOnlyList<IReadOnlyList<double>> rows, double speedRatio)
|
||
{
|
||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||
{
|
||
throw new InvalidOperationException("Speed ratio must be greater than zero for MovePose limit validation.");
|
||
}
|
||
|
||
double? previousTime = null;
|
||
double[]? previousPositions = null;
|
||
double[]? previousVelocities = null;
|
||
double[]? previousAccelerations = null;
|
||
|
||
foreach (var row in rows)
|
||
{
|
||
if (row.Count != 7)
|
||
{
|
||
throw new InvalidOperationException("MovePose dense trajectory rows must contain time plus six pose values.");
|
||
}
|
||
|
||
var currentTime = row[0];
|
||
var currentPositions = row.Skip(1).Take(6).ToArray();
|
||
|
||
if (previousTime is not null && previousPositions is not null)
|
||
{
|
||
var dt = currentTime - previousTime.Value;
|
||
if (dt <= 0.0)
|
||
{
|
||
throw new InvalidOperationException("MovePose dense trajectory timestamps must be strictly increasing.");
|
||
}
|
||
|
||
var velocities = new double[6];
|
||
var accelerations = new double[6];
|
||
for (var index = 0; index < 6; index++)
|
||
{
|
||
var limits = GetAxisLimits(index, speedRatio);
|
||
velocities[index] = (currentPositions[index] - previousPositions[index]) / dt;
|
||
if (Math.Abs(velocities[index]) > limits.Velocity * DiscreteLimitTolerance)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
accelerations[index] = previousVelocities is null
|
||
? 0.0
|
||
: (velocities[index] - previousVelocities[index]) / dt;
|
||
if (Math.Abs(accelerations[index]) > limits.Acceleration * DiscreteLimitTolerance)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (previousAccelerations is not null)
|
||
{
|
||
var jerk = (accelerations[index] - previousAccelerations[index]) / dt;
|
||
if (Math.Abs(jerk) > limits.Jerk * DiscreteLimitTolerance)
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
|
||
previousVelocities = velocities;
|
||
previousAccelerations = accelerations;
|
||
}
|
||
|
||
previousTime = currentTime;
|
||
previousPositions = currentPositions;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据 7 阶平滑点到点时间律和直角六维限制计算理论最短时长。
|
||
/// </summary>
|
||
private static double ResolveDurationSeconds(IReadOnlyList<double> startPose, IReadOnlyList<double> targetPose, double speedRatio)
|
||
{
|
||
var duration = 0.0;
|
||
for (var index = 0; index < 6; index++)
|
||
{
|
||
var distance = Math.Abs(targetPose[index] - startPose[index]);
|
||
if (distance <= 0.0)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
var limits = GetAxisLimits(index, speedRatio);
|
||
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limits.Velocity;
|
||
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limits.Acceleration);
|
||
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limits.Jerk);
|
||
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
|
||
}
|
||
|
||
return duration;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 生成从起始位姿到目标位姿的稠密等时间隔直角轨迹点序列。
|
||
/// </summary>
|
||
private static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
|
||
IReadOnlyList<double> startPose,
|
||
IReadOnlyList<double> targetPose,
|
||
double durationSeconds,
|
||
double samplePeriodSeconds)
|
||
{
|
||
var sampleCount = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) + 1;
|
||
var rows = new List<IReadOnlyList<double>>(checked((int)sampleCount));
|
||
|
||
for (var index = 0L; index < sampleCount; index++)
|
||
{
|
||
var time = Math.Min(index * samplePeriodSeconds, durationSeconds);
|
||
rows.Add(CreateRow(time, durationSeconds, startPose, targetPose));
|
||
}
|
||
|
||
return rows;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将请求时长向上对齐到整数个伺服周期。
|
||
/// </summary>
|
||
private static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
|
||
{
|
||
return ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) * samplePeriodSeconds;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算时长对应的采样间隔数，轨迹至少包含起点和终点两帧。
|
||
/// </summary>
|
||
private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
|
||
{
|
||
var rawIntervals = durationSeconds / samplePeriodSeconds;
|
||
if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
|
||
{
|
||
throw new InvalidOperationException("MovePose sample count is not representable.");
|
||
}
|
||
|
||
var intervals = (long)Math.Ceiling(Math.Max(0.0, rawIntervals) - 1e-9);
|
||
intervals = Math.Max(1, intervals);
|
||
if (intervals + 1 > MaxMovePoseSampleCount)
|
||
{
|
||
throw new InvalidOperationException($"MovePose sample count must be between 2 and {MaxMovePoseSampleCount}.");
|
||
}
|
||
|
||
return intervals;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造单个轨迹行：[time_seconds,x,y,z,w,p,r]。
|
||
/// </summary>
|
||
private static IReadOnlyList<double> CreateRow(
|
||
double timeSeconds,
|
||
double durationSeconds,
|
||
IReadOnlyList<double> startPose,
|
||
IReadOnlyList<double> targetPose)
|
||
{
|
||
var u = durationSeconds <= 0.0 ? 1.0 : Math.Clamp(timeSeconds / durationSeconds, 0.0, 1.0);
|
||
var scale = MoveJointTrajectoryGenerator.EvaluateSmoothPtpPositionScale(u);
|
||
var row = new double[7];
|
||
row[0] = Math.Round(timeSeconds, 9);
|
||
|
||
for (var index = 0; index < 6; index++)
|
||
{
|
||
row[index + 1] = startPose[index] + ((targetPose[index] - startPose[index]) * scale);
|
||
}
|
||
|
||
return row;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取指定直角轴在当前速度倍率下的有效限制。
|
||
/// </summary>
|
||
private static CartesianAxisLimit GetAxisLimits(int index, double speedRatio)
|
||
{
|
||
var linearAxis = index < 3;
|
||
var velocity = linearAxis ? LinearVelocityLimit : AngularVelocityLimit;
|
||
var acceleration = linearAxis ? LinearAccelerationLimit : AngularAccelerationLimit;
|
||
var jerk = linearAxis ? LinearJerkLimit : AngularJerkLimit;
|
||
return new CartesianAxisLimit(
|
||
velocity * speedRatio,
|
||
acceleration * speedRatio * speedRatio,
|
||
jerk * speedRatio * speedRatio * speedRatio);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 校验位姿向量必须为六维有限数。
|
||
/// </summary>
|
||
private static void EnsurePoseVector(IReadOnlyList<double> pose, string parameterName)
|
||
{
|
||
if (pose.Count != 6)
|
||
{
|
||
throw new ArgumentException("MovePose expects pose [x,y,z,w,p,r].", parameterName);
|
||
}
|
||
|
||
if (pose.Any(static value => double.IsNaN(value) || double.IsInfinity(value)))
|
||
{
|
||
throw new ArgumentException("MovePose pose values must be finite.", parameterName);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 表示单个直角轴的有效速度、加速度和 jerk 限制。
|
||
/// </summary>
|
||
private readonly record struct CartesianAxisLimit(double Velocity, double Acceleration, double Jerk);
|
||
}
|