💥 feat(planning)!: 拆分 C# ICSP 并默认 Rust
* 新增可选 Flyshot.CSharp.Planning 项目 * 将 icsp 映射到 Rust 后端并拒绝旧 icsp_rust * 补充后端注册、文档、调试页和回归测试 BREAKING CHANGE: method=icsp 现在默认调用 Rust 后端；C# ICSP 改为 method=icsp-csharp，旧 method=icsp_rust 不再作为正式入口。
This commit is contained in:
275
src/Flyshot.CSharp.Planning/ICspPlanner.cs
Normal file
275
src/Flyshot.CSharp.Planning/ICspPlanner.cs
Normal file
@@ -0,0 +1,275 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.CSharp.Planning;
|
||||
|
||||
/// <summary>
|
||||
/// 基于 CubicSpline + 逐段时间缩放迭代的 ICSP 规划器。
|
||||
///
|
||||
/// 算法核心逻辑（与逆向文档一致）：
|
||||
/// 1. 初始段时长取关节空间欧氏距离；
|
||||
/// 2. 每轮用当前时长构造 CubicSpline，解析求每段 1/2/3 阶导峰值；
|
||||
/// 3. 按速度一次方、加速度平方根、jerk 立方根缩放时长；
|
||||
/// 4. 以 sum(|scale_i - 1|) 为收敛指标，保存历史最优结果；
|
||||
/// 5. 最终用最优时长构造 CubicSpline 并输出时间轴。
|
||||
/// </summary>
|
||||
public sealed class ICspPlanner
|
||||
{
|
||||
/// <summary>
|
||||
/// 默认收敛阈值，对应原实现中的 1e-3。
|
||||
/// </summary>
|
||||
public const double DefaultThreshold = 1e-3;
|
||||
|
||||
/// <summary>
|
||||
/// 默认最大迭代轮数，对应原实现中的 1000。
|
||||
/// </summary>
|
||||
public const int DefaultMaxIterations = 1000;
|
||||
|
||||
/// <summary>
|
||||
/// 默认最终 scale 容差。当前 C# spline 与旧系统对齐样本存在约 1% 内的数值余量。
|
||||
/// </summary>
|
||||
public const double DefaultFinalScaleTolerance = 1e-2;
|
||||
|
||||
private readonly double _threshold;
|
||||
private readonly int _maxIterations;
|
||||
private readonly bool _enforceFinalScale;
|
||||
private readonly double _finalScaleTolerance;
|
||||
private readonly ILogger<ICspPlanner>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 ICSP 规划器。
|
||||
/// </summary>
|
||||
/// <param name="threshold">收敛阈值。</param>
|
||||
/// <param name="maxIterations">最大迭代轮数。</param>
|
||||
/// <param name="enforceFinalScale">是否在最终最优 scale 仍大于 1.0 时抛出失败。</param>
|
||||
/// <param name="finalScaleTolerance">最终 scale 判定容差。</param>
|
||||
/// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
|
||||
public ICspPlanner(
|
||||
double threshold = DefaultThreshold,
|
||||
int maxIterations = DefaultMaxIterations,
|
||||
bool enforceFinalScale = true,
|
||||
double finalScaleTolerance = DefaultFinalScaleTolerance,
|
||||
ILogger<ICspPlanner>? logger = null)
|
||||
{
|
||||
if (threshold <= 0.0 || double.IsNaN(threshold) || double.IsInfinity(threshold))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(threshold), "收敛阈值必须为有限正数。");
|
||||
}
|
||||
|
||||
if (maxIterations < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(maxIterations), "最大迭代轮数不能为负数。");
|
||||
}
|
||||
|
||||
if (finalScaleTolerance < 0.0 || double.IsNaN(finalScaleTolerance) || double.IsInfinity(finalScaleTolerance))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(finalScaleTolerance), "最终 scale 容差必须为有限非负数。");
|
||||
}
|
||||
|
||||
_threshold = threshold;
|
||||
_maxIterations = maxIterations;
|
||||
_enforceFinalScale = enforceFinalScale;
|
||||
_finalScaleTolerance = finalScaleTolerance;
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 执行 ICSP 规划，返回包含完整时间轴和收敛信息的轨迹。
|
||||
/// </summary>
|
||||
public PlannedTrajectory Plan(TrajectoryRequest request)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(request);
|
||||
|
||||
var waypoints = request.Program.Waypoints;
|
||||
if (waypoints.Count < 4)
|
||||
{
|
||||
throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ICSP 规划开始: 名称={Name}, 路点数={WaypointCount}, 自由度={Dof}, threshold={Threshold}, maxIterations={MaxIterations}",
|
||||
request.Program.Name, waypoints.Count, request.Robot.DegreesOfFreedom, _threshold, _maxIterations);
|
||||
_logger?.LogDebug(
|
||||
"ICSP 输入路点: {Waypoints}",
|
||||
string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Positions.Select(j => j.ToString("F4")))}]")));
|
||||
|
||||
var qs = WaypointsToArray(waypoints);
|
||||
var (velLimits, accLimits, jerkLimits) = ExtractLimits(request.Robot);
|
||||
|
||||
_logger?.LogDebug(
|
||||
"ICSP 约束限值: vel=[{Vel}], acc=[{Acc}], jerk=[{Jerk}]",
|
||||
string.Join(", ", velLimits.Select(v => v.ToString("F2"))),
|
||||
string.Join(", ", accLimits.Select(a => a.ToString("F2"))),
|
||||
string.Join(", ", jerkLimits.Select(j => j.ToString("F2"))));
|
||||
|
||||
// 初始段时长直接取相邻示教点的关节空间欧氏距离。
|
||||
var segmentDurations = ComputeInitialDurations(qs);
|
||||
int nseg = segmentDurations.Length;
|
||||
int dof = qs[0].Length;
|
||||
|
||||
double bestThreshold = double.PositiveInfinity;
|
||||
double[]? bestDurations = null;
|
||||
double[]? bestScales = null;
|
||||
CubicSplineInterpolator? bestSpline = null;
|
||||
int bestIterations = 0;
|
||||
double[]? bestWaypointTimes = null;
|
||||
|
||||
for (int iteration = 0; iteration <= _maxIterations; iteration++)
|
||||
{
|
||||
var waypointTimes = CumulativeTimes(segmentDurations);
|
||||
var spline = new CubicSplineInterpolator(waypointTimes, qs);
|
||||
var (maxDq, maxDdq, maxDddq) = spline.SegmentMaxAbsDerivatives();
|
||||
|
||||
var scales = new double[nseg];
|
||||
for (int seg = 0; seg < nseg; seg++)
|
||||
{
|
||||
double segScale = 0.0;
|
||||
for (int d = 0; d < dof; d++)
|
||||
{
|
||||
double sv = Math.Abs(maxDq[seg, d] / velLimits[d]);
|
||||
double sa = Math.Sqrt(Math.Abs(maxDdq[seg, d] / accLimits[d]));
|
||||
double sj = Math.Cbrt(Math.Abs(maxDddq[seg, d] / jerkLimits[d]));
|
||||
segScale = Math.Max(segScale, Math.Max(sv, Math.Max(sa, sj)));
|
||||
}
|
||||
|
||||
scales[seg] = segScale;
|
||||
}
|
||||
|
||||
double currentThreshold = 0.0;
|
||||
for (int seg = 0; seg < nseg; seg++)
|
||||
{
|
||||
currentThreshold += Math.Abs(scales[seg] - 1.0);
|
||||
}
|
||||
|
||||
if (currentThreshold < bestThreshold)
|
||||
{
|
||||
bestThreshold = currentThreshold;
|
||||
bestDurations = (double[])segmentDurations.Clone();
|
||||
bestScales = (double[])scales.Clone();
|
||||
bestSpline = spline;
|
||||
bestIterations = iteration + 1;
|
||||
bestWaypointTimes = (double[])waypointTimes.Clone();
|
||||
}
|
||||
|
||||
if (currentThreshold < _threshold)
|
||||
{
|
||||
_logger?.LogDebug(
|
||||
"ICSP 第 {Iteration} 轮收敛: threshold={CurrentThreshold:E6}",
|
||||
iteration + 1, currentThreshold);
|
||||
break;
|
||||
}
|
||||
|
||||
for (int seg = 0; seg < nseg; seg++)
|
||||
{
|
||||
segmentDurations[seg] *= scales[seg];
|
||||
}
|
||||
}
|
||||
|
||||
if (bestSpline is null || bestDurations is null || bestScales is null || bestWaypointTimes is null)
|
||||
{
|
||||
throw new InvalidOperationException("ICSP 规划未能产生有效结果。");
|
||||
}
|
||||
|
||||
var globalScale = bestScales.Max();
|
||||
if (_enforceFinalScale && globalScale > 1.0 + _finalScaleTolerance)
|
||||
{
|
||||
_logger?.LogError(
|
||||
"ICSP 规划未收敛: global_scale={GlobalScale:F6} > {Tolerance:F6}, 段缩放=[{Scales}]",
|
||||
globalScale, 1.0 + _finalScaleTolerance,
|
||||
string.Join(", ", bestScales.Select(s => s.ToString("F4"))));
|
||||
throw new InvalidOperationException(
|
||||
$"ICSP 规划未收敛，global_scale={globalScale:F6} > {1.0 + _finalScaleTolerance:F6}，轨迹不可执行。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ICSP 规划完成: 名称={Name}, 迭代轮数={Iterations}, 收敛阈值={Threshold:E6}, 总时长={Duration:F4}s, global_scale={GlobalScale:F6}",
|
||||
request.Program.Name, bestIterations, bestThreshold, bestWaypointTimes[^1], globalScale);
|
||||
_logger?.LogDebug(
|
||||
"ICSP 段时长: [{Durations}], 段缩放: [{Scales}]",
|
||||
string.Join(", ", bestDurations.Select(d => d.ToString("F4"))),
|
||||
string.Join(", ", bestScales.Select(s => s.ToString("F4"))));
|
||||
|
||||
return new PlannedTrajectory(
|
||||
robot: request.Robot,
|
||||
originalProgram: request.Program,
|
||||
plannedWaypoints: waypoints,
|
||||
waypointTimes: bestWaypointTimes,
|
||||
segmentDurations: bestDurations,
|
||||
segmentScales: bestScales,
|
||||
method: PlanningMethod.IcspCSharp,
|
||||
iterations: bestIterations,
|
||||
threshold: bestThreshold);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把领域层路点列表转换成 double[][]，方便数学运算。
|
||||
/// </summary>
|
||||
private static double[][] WaypointsToArray(IReadOnlyList<JointWaypoint> waypoints)
|
||||
{
|
||||
var result = new double[waypoints.Count][];
|
||||
for (int i = 0; i < waypoints.Count; i++)
|
||||
{
|
||||
result[i] = waypoints[i].Positions.ToArray();
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从机器人配置中提取速度/加速度/jerk 限值数组。
|
||||
/// </summary>
|
||||
private static (double[] vel, double[] acc, double[] jerk) ExtractLimits(RobotProfile robot)
|
||||
{
|
||||
int dof = robot.DegreesOfFreedom;
|
||||
var vel = new double[dof];
|
||||
var acc = new double[dof];
|
||||
var jerk = new double[dof];
|
||||
for (int d = 0; d < dof; d++)
|
||||
{
|
||||
vel[d] = robot.JointLimits[d].VelocityLimit;
|
||||
acc[d] = robot.JointLimits[d].AccelerationLimit;
|
||||
jerk[d] = robot.JointLimits[d].JerkLimit;
|
||||
}
|
||||
|
||||
return (vel, acc, jerk);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算初始段时长：取相邻路点在关节空间的欧氏距离。
|
||||
/// </summary>
|
||||
private static double[] ComputeInitialDurations(double[][] qs)
|
||||
{
|
||||
int n = qs.Length;
|
||||
var durations = new double[n - 1];
|
||||
for (int i = 0; i < n - 1; i++)
|
||||
{
|
||||
double sumSq = 0.0;
|
||||
for (int d = 0; d < qs[i].Length; d++)
|
||||
{
|
||||
double diff = qs[i + 1][d] - qs[i][d];
|
||||
sumSq += diff * diff;
|
||||
}
|
||||
|
||||
durations[i] = Math.Sqrt(sumSq);
|
||||
}
|
||||
|
||||
return durations;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 由段时长累加得到绝对时间节点（首项为 0）。
|
||||
/// </summary>
|
||||
private static double[] CumulativeTimes(double[] segmentDurations)
|
||||
{
|
||||
int nseg = segmentDurations.Length;
|
||||
var times = new double[nseg + 1];
|
||||
times[0] = 0.0;
|
||||
for (int i = 0; i < nseg; i++)
|
||||
{
|
||||
times[i + 1] = times[i] + segmentDurations[i];
|
||||
}
|
||||
|
||||
return times;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user