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

38
src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs
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;
}
}