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

Merge commit '0952958af7685646cdbf2a4907c6e2ae59d4d352'

Browse Source
This commit is contained in:
yunxiao.zhu
2026-05-28 10:44:03 +08:00
parent 4eeaa3fef3
commit 35bdb08d11
144 changed files with 33730 additions and 392 deletions
Download Patch File Download Diff File Expand all files Collapse all files

353
src/Flyshot.ControllerClientCompat/MovePoseTrajectoryGenerator.cs Normal file
View File

@@ -0,0 +1,353 @@
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);
}