QUYVN/BQP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/BatchTestResult.cs Normal file
View File

@@ -0,0 +1,27 @@
using RobotNet10.NavigationTune.Shared.Interfaces;
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// Batch test result
/// </summary>
public class BatchTestResult
{
public Guid BatchId { get; set; } = Guid.NewGuid();
public NavigationParameterSet Parameters { get; set; } = null!;
public List<TestExecutionResult> Results { get; set; } = new();
public int SuccessCount { get; set; }
public int FailureCount { get; set; }
public double AverageScore { get; set; }
}
/// <summary>
/// Configuration comparison result
/// </summary>
public class ComparisonResult
{
public TestScenario Scenario { get; set; } = null!;
public List<NavigationParameterSet> Configurations { get; set; } = new();
public Dictionary<string, TestExecutionResult> Results { get; set; } = new();
public string BestConfiguration { get; set; } = string.Empty;
}

77
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/CircleScenario.cs Normal file
View File

@@ -0,0 +1,77 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// Circle test scenario (client-side version)
/// </summary>
public class CircleScenario : TestScenario
{
public double Radius { get; set; } = 2.0; // meters
public double CenterX { get; set; } = 0.0;
public double CenterY { get; set; } = 0.0;
public double StartAngle { get; set; } = 0.0; // radians
public bool Clockwise { get; set; } = true;
public double Resolution { get; set; } = 0.05; // meters between points
public CircleScenario()
{
Name = $"Circle {Radius}m Radius";
Description = $"Robot moves in a circle with radius {Radius}m";
Type = TrajectoryType.Circle;
}
public override List<PathPoint> GenerateReferencePath()
{
var points = new List<PathPoint>();
double circumference = 2 * Math.PI * Radius;
int numPoints = (int)(circumference / Resolution);
double angleStep = 2 * Math.PI / numPoints;
if (!Clockwise)
angleStep = -angleStep;
var direction = Clockwise ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
double distance = 0.0;
for (int i = 0; i <= numPoints; i++)
{
double angle = StartAngle + i * angleStep;
double x = CenterX + Radius * Math.Cos(angle);
double y = CenterY + Radius * Math.Sin(angle);
distance = i * Resolution;
if (distance > circumference) distance = circumference;
points.Add(new PathPoint
{
X = x,
Y = y,
Direction = direction,
DistanceFromStart = distance
});
}
return points;
}
public override bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f)
{
// Check if robot is close to start position
double dx = currentPose.X - (CenterX + Radius * Math.Cos(StartAngle));
double dy = currentPose.Y - (CenterY + Radius * Math.Sin(StartAngle));
double distance = Math.Sqrt(dx * dx + dy * dy);
return distance <= tolerance;
}
public override Pose2D GetGoalPose()
{
// Goal is back at start position
double goalX = CenterX + Radius * Math.Cos(StartAngle);
double goalY = CenterY + Radius * Math.Sin(StartAngle);
double goalTheta = StartAngle + (Clockwise ? Math.PI / 2 : -Math.PI / 2);
while (goalTheta > Math.PI) goalTheta -= 2 * Math.PI;
while (goalTheta < -Math.PI) goalTheta += 2 * Math.PI;
return new Pose2D(goalX, goalY, goalTheta);
}
}

359
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/Configs.cs Normal file
View File

@@ -0,0 +1,359 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// PID Controller configuration
/// </summary>
public class PIDConfig
{
public double Kp { get; set; }
public double Ki { get; set; }
public double Kd { get; set; }
/// <summary>
/// Integral chỉ tích lũy khi |error| &lt;= IntegralZone.
/// Giá trị 0 = không giới hạn (integral luôn tích lũy).
/// </summary>
public double IntegralZone { get; set; }
}
/// <summary>
/// Motor Dynamics configuration
/// </summary>
public class MotorDynamicsConfig
{
/// <summary>
/// Time constant (τ) - thời gian để motor đạt 63.2% của target velocity
/// Đơn vị: giây (s)
/// Typical: 0.1 - 0.5s cho DC motor với driver PID
/// </summary>
public double Tau { get; set; }
/// <summary>
/// Pure delay (δ) - độ trễ trước khi motor bắt đầu phản ứng
/// Đơn vị: giây (s)
/// Bao gồm: communication delay + driver processing
/// Typical: 0.02 - 0.1s
/// </summary>
public double Delta { get; set; }
}
/// <summary>
/// Stanley Controller configuration
/// Path tracking using cross-track error and heading error
/// </summary>
public class StanleyConfig
{
#region Core Stanley Parameters
/// <summary>
/// Cross-track error gain (K)
/// Default: 2.5
///
/// Meaning: How aggressively to correct lateral position error
/// Formula: δ = ψ + arctan(K × e / (v + Ks))
///
/// ↑ Increase (3.0-5.0):
/// ✓ Faster correction of cross-track error
/// ✓ Tighter path following
/// ✗ May cause oscillation
/// ✗ Less smooth on noisy paths
///
/// ↓ Decrease (1.5-2.0):
/// ✓ Smoother motion
/// ✓ Less oscillation
/// ✗ Slower error correction
/// ✗ Larger cross-track error
///
/// Tuning Tips:
/// - Start: 2.5 for general use
/// - High precision: 3.0-4.0
/// - Smooth priority: 1.5-2.0
/// - Check stability by observing steering oscillation
/// </summary>
public double K { get; set; } = 2.5;
/// <summary>
/// Softening constant (Ks) - meters/second
/// Default: 0.1 m/s
///
/// Meaning: Added to velocity denominator to prevent division by zero at low speeds
/// Formula: δ = ψ + arctan(K × e / (v + Ks))
///
/// ↑ Increase (0.15-0.2):
/// ✓ Less aggressive correction at low speed
/// ✓ Smoother motion when starting
/// ✗ Slower error correction at low speed
///
/// ↓ Decrease (0.05-0.08):
/// ✓ More responsive at low speed
/// ✗ May cause oscillation when slow
/// ✗ Risk of instability near zero velocity
///
/// Tuning Tips:
/// - Should be ~10% of typical operating velocity
/// - If robot oscillates when slow: increase to 0.15-0.2
/// - If too sluggish at startup: decrease to 0.05-0.08
/// </summary>
public double Ks { get; set; } = 0.1;
#endregion
#region Vehicle Parameters
/// <summary>
/// Wheelbase (L) - distance between front and rear axles (meters)
/// Default: 0.5m
///
/// Meaning: Distance from rear axle (robot center) to virtual front axle
/// Used to calculate front axle position and convert steering angle to angular velocity
///
/// IMPORTANT: Must match actual robot geometry
///
/// Formula: ω = (v × tan(δ)) / L
/// </summary>
public double WheelBase { get; set; } = 0.6;
/// <summary>
/// Maximum steering angle (radians)
/// Default: 0.5 rad (≈28.6°)
///
/// Meaning: Physical limit of equivalent steering angle
///
/// ↑ Increase (0.6-0.8 rad ≈ 34-46°):
/// ✓ Sharper turns possible
/// ✗ May exceed robot's turning capability
///
/// ↓ Decrease (0.3-0.4 rad ≈ 17-23°):
/// ✓ Safer, gentler turns
/// ✗ Cannot track sharp curves
///
/// Tuning Tips:
/// - Test robot's max practical turn rate
/// - Calculate: δ_max = arctan(L × ω_max / v_typical)
/// - Example: L=0.5m, ω_max=2rad/s, v=1m/s → δ_max = 0.785 rad (45°)
/// - Conservative: 0.4-0.5 rad
/// </summary>
public double MaxSteeringAngle { get; set; } = 0.5;
#endregion
#region Curvature Feedforward Parameters
/// <summary>
/// Enable curvature feedforward term
/// Default: true
///
/// Meaning: Add path curvature prediction to steering command
/// Formula: δ = ψ + arctan(K×e/(v+Ks)) + arctan(κ×L)
///
/// ✓ Enabled:
/// ✓ Better tracking on curved paths
/// ✓ Anticipates turns, less lag
/// ✗ Requires accurate path curvature
///
/// ✗ Disabled:
/// ✓ Simpler, more predictable
/// ✓ Works with rough path data
/// ✗ May lag on curves
///
/// Tuning Tips:
/// - Enable for smooth, well-defined paths
/// - Disable if path is noisy or has discontinuities
/// </summary>
public bool EnableCurvatureFeedforward { get; set; } = true;
/// <summary>
/// Curvature feedforward gain
/// Default: 1.0
///
/// Meaning: Scaling factor for curvature term
/// Full formula: δ = ψ + arctan(K×e/(v+Ks)) + KCurvatureFF × arctan(κ×L)
///
/// ↑ Increase (1.2-1.5):
/// ✓ More aggressive curve anticipation
/// ✓ Less lag on sharp turns
/// ✗ May overshoot on curves
///
/// ↓ Decrease (0.7-0.9):
/// ✓ Gentler curve following
/// ✗ More lag on curves
///
/// Tuning Tips:
/// - Start at 1.0
/// - If cutting corners: increase to 1.1-1.3
/// - If overshooting curves: decrease to 0.8-0.9
/// </summary>
public double KCurvatureFF { get; set; } = 1.0;
#endregion
#region Goal Approach Parameters
/// <summary>
/// Distance to goal to consider "reached" (meters)
/// Default: 0.05m (5cm)
///
/// Meaning: Stop criterion - when within this distance, goal is reached
///
/// ↑ Increase (0.08-0.1m):
/// ✓ Faster completion
/// ✗ Lower precision
///
/// ↓ Decrease (0.02-0.03m):
/// ✓ Higher precision
/// ✗ May never reach due to localization error
///
/// Tuning Tips:
/// - Must be ≥ 2× localization RMS error
/// - Typical: 0.03-0.05m
/// </summary>
public double GoalTolerance { get; set; } = 0.05;
/// <summary>
/// Heading tolerance at goal (degrees)
/// Default: 5.0°
///
/// Meaning: Acceptable heading error when reaching goal
///
/// Tuning Tips:
/// - Strict docking: 2-3°
/// - Normal navigation: 5-10°
/// </summary>
public double HeadingTolerance { get; set; } = 5.0;
/// <summary>
/// Distance to start increasing K gain near goal (meters)
/// Default: 1.0m
///
/// Meaning: When within this distance, K gain increases linearly
/// to improve tracking accuracy during final approach
///
/// ↑ Increase (1.5-2.0m):
/// ✓ Earlier tightening, smoother transition
/// ✗ May be too aggressive on long approach
///
/// ↓ Decrease (0.5-0.8m):
/// ✓ Only tighten very close to goal
/// ✗ Less time to correct errors
///
/// Tuning Tips:
/// - Should be larger than GoalTolerance × 10
/// - Typical: 0.8-1.5m
/// </summary>
public double GoalApproachDistance { get; set; } = 1.0;
/// <summary>
/// K gain multiplier at goal position
/// Default: 2.0 (K doubles when at goal)
///
/// Meaning: At goal, effective K = K × GoalGainMultiplier
/// Linearly interpolated from 1.0 at GoalApproachDistance to this value at goal
///
/// ↑ Increase (2.5-3.0):
/// ✓ Much tighter tracking near goal
/// ✗ Risk of oscillation
///
/// ↓ Decrease (1.3-1.5):
/// ✓ Gentler increase
/// ✗ Less improvement near goal
///
/// Tuning Tips:
/// - Start at 2.0
/// - If oscillating near goal: decrease to 1.5
/// - If still drifting: increase to 2.5
/// </summary>
public double GoalGainMultiplier { get; set; } = 2.0;
#endregion
#region Low Speed Control Parameters
/// <summary>
/// Velocity threshold below which direct angular control activates (m/s)
/// Default: 0.3 m/s
///
/// Meaning: Below this speed, bicycle model is blended with direct proportional control.
/// This prevents the angular velocity from collapsing to zero when the robot
/// decelerates near the goal.
///
/// Problem it solves:
/// Bicycle model: ω = v × tan(δ) / L
/// When v → 0, ω → 0, even if δ is large → robot cannot correct
///
/// ↑ Increase (0.4-0.5):
/// ✓ Direct control kicks in earlier
/// ✗ May feel less smooth at moderate speeds
///
/// ↓ Decrease (0.15-0.2):
/// ✓ Only activates at very low speed
/// ✗ May still drift at medium-low speeds
///
/// Tuning Tips:
/// - Should be close to NavigationConfig.MinLinearVelocity × 2-3
/// - Typical: 0.2-0.4 m/s
/// </summary>
public double LowSpeedThreshold { get; set; } = 0.3;
/// <summary>
/// Angular velocity gain for direct control at low speeds
/// Default: 1.5
///
/// Meaning: At zero speed, ω = LowSpeedAngularGain × steeringAngle
/// Ensures the robot can still correct heading/cross-track errors
/// when the bicycle model would produce near-zero angular velocity.
///
/// ↑ Increase (2.0-3.0):
/// ✓ Stronger correction at low speed
/// ✗ May oscillate near goal
///
/// ↓ Decrease (0.8-1.0):
/// ✓ Gentler low-speed correction
/// ✗ Slower error correction
///
/// Tuning Tips:
/// - Start at 1.5
/// - If oscillating at low speed: decrease to 1.0
/// - If not correcting fast enough: increase to 2.0
/// </summary>
public double LowSpeedAngularGain { get; set; } = 1.5;
#endregion
#region Angular Velocity Limit
/// <summary>
/// Maximum angular velocity during final approach (rad/s)
/// Default: 1.0 rad/s
///
/// Meaning: Clamps the angular velocity output of Stanley controller
/// to prevent excessive rotation near the goal.
///
/// ↑ Increase (1.5-2.0):
/// ✓ Faster heading correction
/// ✗ May overshoot or oscillate
///
/// ↓ Decrease (0.5-0.8):
/// ✓ Smoother, gentler rotation near goal
/// ✗ Slower heading correction
///
/// Tuning Tips:
/// - Should be ≤ robot's physical max angular velocity
/// - Typically lower than PurePursuit MaxAngularVelocity for smoother final approach
/// - Start at 1.0, decrease if robot oscillates near goal
/// </summary>
public double MaxAngularVelocity { get; set; } = 1.0;
#endregion
#region Path Resolution
/// <summary>
/// Waypoint spacing for path sampling (meters)
/// Default: 0.05m (5cm)
///
/// Meaning: Distance between interpolated path points
/// Same as PurePursuit.ResolutionSplit for consistency
/// </summary>
public double ResolutionSplit { get; set; } = 0.05;
#endregion
}

285
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/CustomPathScenario.cs Normal file
View File

@@ -0,0 +1,285 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// Custom path scenario with user-defined edges
/// </summary>
public class CustomPathScenario : TestScenario
{
/// <summary>
/// List of edges defining the path
/// </summary>
public List<PathEdge> Edges { get; set; } = new();
/// <summary>
/// Resolution for splitting edges into points (meters)
/// </summary>
public double Resolution { get; set; } = 0.05; // meters between points
public CustomPathScenario()
{
Name = "Custom Path";
Description = "User-defined path with custom edges";
Type = TrajectoryType.Custom;
}
public override List<PathPoint> GenerateReferencePath()
{
var points = new List<PathPoint>();
if (Edges.Count == 0)
return points;
double cumulativeDistance = 0.0;
// Process each edge
foreach (var edge in Edges)
{
var edgePoints = SplitEdge(edge, Resolution);
if (edgePoints.Count == 0)
continue;
// Adjust cumulative distance for first point
if (points.Count > 0)
{
// Calculate distance from last point to first point of this edge
double dx = edgePoints[0].X - points[^1].X;
double dy = edgePoints[0].Y - points[^1].Y;
double connectionDistance = Math.Sqrt(dx * dx + dy * dy);
cumulativeDistance = points[^1].DistanceFromStart + connectionDistance;
}
else
{
cumulativeDistance = 0.0;
}
// Add points from this edge
for (int i = 0; i < edgePoints.Count; i++)
{
if (i == 0 && points.Count > 0)
{
// Skip first point if it's the same as last point (edge connection)
double dx = edgePoints[i].X - points[^1].X;
double dy = edgePoints[i].Y - points[^1].Y;
if (Math.Sqrt(dx * dx + dy * dy) < 0.001)
continue;
}
if (i > 0)
{
// Calculate distance from previous point
double dx = edgePoints[i].X - edgePoints[i - 1].X;
double dy = edgePoints[i].Y - edgePoints[i - 1].Y;
double segmentDistance = Math.Sqrt(dx * dx + dy * dy);
cumulativeDistance += segmentDistance;
}
// Use direction from edge
RobotDirection direction = edge.Direction;
points.Add(new PathPoint
{
X = edgePoints[i].X,
Y = edgePoints[i].Y,
Direction = direction,
DistanceFromStart = cumulativeDistance
});
}
}
return points;
}
/// <summary>
/// Split an edge into points based on resolution
/// </summary>
private List<(double X, double Y, double Theta)> SplitEdge(PathEdge edge, double resolution)
{
var points = new List<(double X, double Y, double Theta)>();
// Calculate edge length
double edgeLength = CalculateEdgeLength(edge);
if (edgeLength <= 0)
{
// Single point at start
double theta = CalculateThetaAt(edge, 0.0);
points.Add((edge.StartX, edge.StartY, theta));
return points;
}
// Calculate number of points based on resolution
int numPoints = Math.Max(1, (int)(edgeLength / resolution));
for (int i = 0; i <= numPoints; i++)
{
double t = numPoints > 0 ? i / numPoints : 0.0;
var (x, y) = CalculatePointAt(edge, t);
double theta = CalculateThetaAt(edge, t);
points.Add((x, y, theta));
}
return points;
}
/// <summary>
/// Calculate point on edge at parameter t (0.0 to 1.0)
/// </summary>
private (double X, double Y) CalculatePointAt(PathEdge edge, double t)
{
t = Math.Clamp(t, 0.0, 1.0);
return edge.Degree switch
{
1 => CalculateLinearPoint(edge, t),
2 => CalculateQuadraticBezierPoint(edge, t),
3 => CalculateCubicBezierPoint(edge, t),
_ => CalculateLinearPoint(edge, t) // Default to linear
};
}
/// <summary>
/// Linear interpolation (Degree 1)
/// </summary>
private (double X, double Y) CalculateLinearPoint(PathEdge edge, double t)
{
double x = edge.StartX + t * (edge.EndX - edge.StartX);
double y = edge.StartY + t * (edge.EndY - edge.StartY);
return (x, y);
}
/// <summary>
/// Quadratic Bezier curve (Degree 2)
/// P(t) = (1-t)²P₀ + 2(1-t)tP₁ + t²P₂
/// </summary>
private (double X, double Y) CalculateQuadraticBezierPoint(PathEdge edge, double t)
{
if (!edge.ControlPoint1X.HasValue || !edge.ControlPoint1Y.HasValue)
{
// Fallback to linear if control point not provided
return CalculateLinearPoint(edge, t);
}
double oneMinusT = 1.0 - t;
double x = oneMinusT * oneMinusT * edge.StartX +
2 * oneMinusT * t * edge.ControlPoint1X.Value +
t * t * edge.EndX;
double y = oneMinusT * oneMinusT * edge.StartY +
2 * oneMinusT * t * edge.ControlPoint1Y.Value +
t * t * edge.EndY;
return (x, y);
}
/// <summary>
/// Cubic Bezier curve (Degree 3)
/// P(t) = (1-t)³P₀ + 3(1-t)²tP₁ + 3(1-t)t²P₂ + t³P₃
/// </summary>
private (double X, double Y) CalculateCubicBezierPoint(PathEdge edge, double t)
{
if (!edge.ControlPoint1X.HasValue || !edge.ControlPoint1Y.HasValue ||
!edge.ControlPoint2X.HasValue || !edge.ControlPoint2Y.HasValue)
{
// Fallback to quadratic or linear if control points not provided
if (edge.ControlPoint1X.HasValue && edge.ControlPoint1Y.HasValue)
return CalculateQuadraticBezierPoint(edge, t);
return CalculateLinearPoint(edge, t);
}
double oneMinusT = 1.0 - t;
double oneMinusT2 = oneMinusT * oneMinusT;
double oneMinusT3 = oneMinusT2 * oneMinusT;
double t2 = t * t;
double t3 = t2 * t;
double x = oneMinusT3 * edge.StartX +
3 * oneMinusT2 * t * edge.ControlPoint1X.Value +
3 * oneMinusT * t2 * edge.ControlPoint2X.Value +
t3 * edge.EndX;
double y = oneMinusT3 * edge.StartY +
3 * oneMinusT2 * t * edge.ControlPoint1Y.Value +
3 * oneMinusT * t2 * edge.ControlPoint2Y.Value +
t3 * edge.EndY;
return (x, y);
}
/// <summary>
/// Calculate tangent angle (theta) at parameter t
/// </summary>
private double CalculateThetaAt(PathEdge edge, double t)
{
const double epsilon = 0.001;
double t1 = Math.Clamp(t, 0.0, 1.0);
double t2 = Math.Clamp(t + epsilon, 0.0, 1.0);
var (x1, y1) = CalculatePointAt(edge, t1);
var (x2, y2) = CalculatePointAt(edge, t2);
double dx = x2 - x1;
double dy = y2 - y1;
double theta = Math.Atan2(dy, dx);
return NormalizeAngle(theta);
}
/// <summary>
/// Calculate approximate length of edge
/// </summary>
private double CalculateEdgeLength(PathEdge edge)
{
// For linear: direct distance
if (edge.Degree == 1)
{
double dx = edge.EndX - edge.StartX;
double dy = edge.EndY - edge.StartY;
return Math.Sqrt(dx * dx + dy * dy);
}
// For curves: approximate by sampling
const int samples = 20;
double length = 0.0;
var (prevX, prevY) = CalculatePointAt(edge, 0.0);
for (int i = 1; i <= samples; i++)
{
double t = i / samples;
var (x, y) = CalculatePointAt(edge, t);
double dx = x - prevX;
double dy = y - prevY;
length += Math.Sqrt(dx * dx + dy * dy);
prevX = x;
prevY = y;
}
return length;
}
public override bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f)
{
if (Edges.Count == 0)
return false;
var goal = GetGoalPose();
double distance = Pose2D.Distance(currentPose, goal);
return distance <= tolerance;
}
public override Pose2D GetGoalPose()
{
if (Edges.Count == 0)
return new Pose2D(0, 0, 0);
var lastEdge = Edges[^1];
double theta = CalculateThetaAt(lastEdge, 1.0);
return new Pose2D(lastEdge.EndX, lastEdge.EndY, theta);
}
private static double NormalizeAngle(double angle)
{
while (angle > Math.PI) angle -= 2 * Math.PI;
while (angle < -Math.PI) angle += 2 * Math.PI;
return angle;
}
}

736
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/NavigationParameterSet.cs Normal file
View File

@@ -0,0 +1,736 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// Path following controller type
/// </summary>
public enum PathFollowingController
{
PurePursuit = 1,
Stanley = 2
}
/// <summary>
/// Complete parameter set for navigation tuning
/// </summary>
public class NavigationParameterSet
{
public Guid Id { get; set; } = Guid.NewGuid();
public string Name { get; set; } = string.Empty;
public string Description { get; set; } = string.Empty;
public DateTime CreatedAt { get; set; } = DateTime.UtcNow;
public DateTime? UpdatedAt { get; set; }
public bool IsDefault { get; set; }
public int Version { get; set; } = 1;
// Controller Selection
public PathFollowingController ControllerType { get; set; } = PathFollowingController.PurePursuit;
// PID Configs
public PIDConfig MovePidConfig { get; set; } = new()
{
Kp = 1.0,
Ki = 0.0001,
Kd = 0.6
};
public PIDConfig RotatePidConfig { get; set; } = new()
{
Kp = 10.0,
Ki = 0.01,
Kd = 0.1
};
// Pure Pursuit Config
public PurePursuitConfig PurePursuitConfig { get; set; } = new();
// Stanley Controller Config
public StanleyConfig StanleyConfig { get; set; } = new();
// Velocity Estimator Config
public VelocityEstimatorConfig EstimatorConfig { get; set; } = new();
public VelocitySignalProcessingConfig SignalConfig { get; set; } = new();
public MotorDynamicsConfig MotorDynamicsConfig { get; set; } = new();
// Navigation Limits
public NavigationConfig NavigationConfig { get; set; } = new();
}
/// <summary>
/// Pure Pursuit path tracking configuration
/// Controls how the robot follows planned paths
/// </summary>
public class PurePursuitConfig
{
#region Basic Lookahead Parameters
/// <summary>
/// Minimum lookahead distance (meters)
/// Default: 0.3m
///
/// Meaning: Closest point ahead on path that robot aims for
///
/// ↑ Increase (0.4-0.6m):
/// ✓ Smoother tracking on straight paths
/// ✓ More predictive, less reactive
/// ✗ May cut corners on sharp curves
/// ✗ Less precise at low speeds
///
/// ↓ Decrease (0.2-0.25m):
/// ✓ Tighter tracking on curves
/// ✓ Better precision at low speeds
/// ✗ More jittery/oscillation
/// ✗ Sensitive to noise
///
/// Tuning Tips:
/// - Start: 0.3m for general use
/// - Warehouse AGV: 0.4-0.5m (smooth, wide corridors)
/// - Tight spaces: 0.25-0.3m (precision needed)
/// </summary>
public double LookaheadMin { get; set; } = 0.3;
/// <summary>
/// Lookahead velocity gain (seconds)
/// Default: 1.0s
///
/// Meaning: How much lookahead increases per m/s of velocity
/// Formula: lookahead = LookaheadMin + Kdd × |velocity|
///
/// ↑ Increase (1.2-1.5s):
/// ✓ Look further ahead at high speed → smoother
/// ✓ Better for fast robots (>1.5 m/s)
/// ✗ May be too predictive (overshoot)
///
/// ↓ Decrease (0.7-0.9s):
/// ✓ More reactive control
/// ✓ Better for slow, precise robots
/// ✗ May be jittery at high speed
///
/// Tuning Tips:
/// - Formula check: At 1.0 m/s → lookahead = 0.3 + 1.0×1.0 = 1.3m
/// - Slow robot (<0.5 m/s): Kdd = 0.8-1.0
/// - Fast robot (>1.5 m/s): Kdd = 1.2-1.5
/// </summary>
public double Kdd { get; set; } = 1.0;
/// <summary>
/// Maximum lookahead distance (meters)
/// Default: 2.0m
///
/// Meaning: Upper limit for lookahead distance
///
/// ↑ Increase (2.5-3.0m):
/// ✓ Very smooth at high speed
/// ✓ Good for long straight paths
/// ✗ May cut corners aggressively
/// ✗ Slower reaction to path changes
///
/// ↓ Decrease (1.5-1.8m):
/// ✓ Tighter path following
/// ✓ Better for complex paths
/// ✗ Less smooth at high speed
///
/// Tuning Tips:
/// - Should be > LookaheadMin + Kdd × MaxVelocity
/// - Example: MaxVel=1.5m/s → need LookaheadMax ≥ 0.3+1.0×1.5 = 1.8m
/// </summary>
public double LookaheadMax { get; set; } = 2.0;
/// <summary>
/// Maximum angular velocity during tracking (rad/s)
/// Default: 1.5 rad/s (≈86°/s)
///
/// Meaning: Limit on how fast robot can turn while tracking
///
/// ↑ Increase (2.0-2.5 rad/s):
/// ✓ Faster turning on sharp curves
/// ✓ Better for agile robots
/// ✗ May cause wheel slip
/// ✗ Less stable, jerky motion
///
/// ↓ Decrease (1.0-1.2 rad/s):
/// ✓ Smoother, more stable
/// ✓ Better for heavy/slow robots
/// ✗ Slower on sharp turns
/// ✗ May not track sharp curves well
///
/// Tuning Tips:
/// - Check robot physical limits first
/// - Warehouse AGV: 1.0-1.5 rad/s
/// - Fast AMR: 2.0+ rad/s
/// - Safety-critical: 0.8-1.0 rad/s
/// </summary>
public double MaxAngularVelocity { get; set; } = 1.5;
/// <summary>
/// Path waypoint spacing resolution (meters)
/// Default: 0.05m (5cm)
///
/// Meaning: How densely path is sampled into waypoints
///
/// ↑ Increase (0.08-0.1m):
/// ✓ Less memory usage
/// ✓ Faster path processing
/// ✗ Coarser path, may lose detail on curves
///
/// ↓ Decrease (0.02-0.03m):
/// ✓ More accurate curve representation
/// ✓ Smoother tracking
/// ✗ More memory usage
/// ✗ Slower processing
///
/// Tuning Tips:
/// - Long paths (>50m): Use 0.08-0.1m
/// - Complex curves: Use 0.03-0.05m
/// - Memory constrained: Increase
/// </summary>
public double ResolutionSplit { get; set; } = 0.05f;
#endregion
#region Final Approach Parameters
/// <summary>
/// Distance to activate final approach mode (meters)
/// Default: 0.2m (20cm)
///
/// Meaning: When robot is this close to goal, switch to precision mode
/// Final approach uses Stanley controller for precise CTE-based tracking
///
/// ↑ Increase (0.3-0.5m):
/// ✓ Earlier slow down → smoother
/// ✓ More gentle approach
/// ✗ Takes longer to reach goal
///
/// ↓ Decrease (0.1-0.15m):
/// ✓ Faster approach
/// ✗ May be abrupt
/// ✗ Risk of overshoot
///
/// Tuning Tips:
/// - High precision: 0.3-0.5m
/// - Speed priority: 0.15-0.2m
/// </summary>
public double FinalApproachThreshold { get; set; } = 0.2;
/// <summary>
/// Final heading tolerance (degrees)
/// Default: 2.0° (0.035 rad)
///
/// Meaning: How aligned robot heading must be with goal
///
/// ↑ Increase (5-10°):
/// ✓ Faster completion
/// ✓ Less strict
/// ✗ Robot may face wrong direction
///
/// ↓ Decrease (1-2°):
/// ✓ Very precise alignment
/// ✗ Takes much longer
/// ✗ May oscillate
///
/// Tuning Tips:
/// - Docking/charging: 2-3° (precision critical)
/// - General navigation: 5-8° (acceptable)
/// - No heading requirement: 10-15° (fast)
/// </summary>
public double HeadingTolerance { get; set; } = 2.0;
#endregion
#region Adaptive Lookahead Parameters
/// <summary>
/// Goal region distance for lookahead reduction (meters)
/// Default: 1.5m
///
/// Meaning: Start reducing lookahead when within this distance of goal
/// Reduction: Linear from 100% at this distance → 50% at goal
///
/// ↑ Increase (2.0-3.0m):
/// ✓ Earlier precision mode
/// ✓ Smoother deceleration
/// ✗ Slower overall
///
/// ↓ Decrease (0.8-1.2m):
/// ✓ Faster approach
/// ✗ More abrupt near goal
///
/// Tuning Tips:
/// - Long paths: 2.0-2.5m
/// - Short paths: 1.0-1.5m
/// - Fast robot: Increase (more brake distance)
/// </summary>
public double GoalRegionDistance { get; set; } = 1.5;
/// <summary>
/// Curvature sensitivity factor
/// Default: 2.0
///
/// Meaning: How much to reduce lookahead on curves
/// Formula: curvatureFactor = 1 / (1 + KCurvature × curvature)
///
/// ↑ Increase (3.0-5.0):
/// ✓ Tighter tracking on curves
/// ✓ Less corner cutting
/// ✗ May be too reactive
/// ✗ More oscillation on curves
///
/// ↓ Decrease (1.0-1.5):
/// ✓ Smoother on curves
/// ✗ May cut corners more
/// ✗ Less precise tracking
///
/// Tuning Tips:
/// - Warehouse (gentle curves): 1.5-2.0
/// - Tight spaces (sharp curves): 3.0-4.0
/// - High speed: Increase (need tighter control)
/// </summary>
public double KCurvature { get; set; } = 2.0;
/// <summary>
/// Minimum lookahead time ratio (seconds)
/// Default: 0.3s
///
/// Meaning: Look ahead at least this many seconds
/// Formula: minLookahead = max(LookaheadMin, velocity × 0.3s)
///
/// ↑ Increase (0.4-0.5s):
/// ✓ More predictive at all speeds
/// ✓ Smoother
/// ✗ May be too far ahead at low speed
///
/// ↓ Decrease (0.2-0.25s):
/// ✓ More reactive
/// ✗ May be too short at high speed
///
/// Tuning Tips:
/// - Human reaction time: ~0.25s
/// - Safe: 0.3-0.4s (reasonable preview)
/// - Very predictive: 0.5s+
/// </summary>
public double MinLookaheadTimeRatio { get; set; } = 0.3;
/// <summary>
/// Maximum lookahead time ratio (seconds)
/// Default: 2.0s
///
/// Meaning: Look ahead at most this many seconds
/// Formula: maxLookahead = min(LookaheadMax, velocity × 2.0s)
///
/// ↑ Increase (2.5-3.0s):
/// ✓ Very smooth at high speed
/// ✗ May be excessively far ahead
/// ✗ Cuts corners
///
/// ↓ Decrease (1.5-1.8s):
/// ✓ Tighter control
/// ✗ Less smooth at high speed
///
/// Tuning Tips:
/// - Should give comfortable preview distance
/// - At 1.5m/s: 2.0s → 3.0m ahead (reasonable)
/// - At 1.5m/s: 3.0s → 4.5m ahead (too far)
/// </summary>
public double MaxLookaheadTimeRatio { get; set; } = 2.0;
#endregion
}
/// <summary>
/// Hybrid Velocity Estimator configuration
/// Blends motor model prediction with encoder feedback
/// </summary>
public class VelocityEstimatorConfig
{
/// <summary>
/// Minimum blend ratio (model weight)
/// Default: 0.15 (15% model, 85% encoder)
///
/// Meaning: Lower bound for how much to trust motor model
///
/// ↑ Increase (0.2-0.3):
/// ✓ More model influence even when tracking poor
/// ✗ May diverge from actual velocity
///
/// ↓ Decrease (0.05-0.1):
/// ✓ More encoder influence
/// ✗ More susceptible to encoder noise
///
/// Tuning Tips:
/// - Good encoders: 0.1-0.15
/// - Noisy encoders: 0.2-0.25
/// </summary>
public double MinBlendRatio { get; set; } = 0.15f;
/// <summary>
/// Maximum blend ratio (model weight)
/// Default: 0.8 (80% model, 20% encoder)
///
/// Meaning: Upper bound for model trust
///
/// ↑ Increase (0.85-0.9):
/// ✓ More predictive
/// ✗ May ignore actual wheel behavior
///
/// ↓ Decrease (0.7-0.75):
/// ✓ More grounded in reality
/// ✗ Less predictive
///
/// Tuning Tips:
/// - Accurate motor model: 0.8-0.85
/// - Uncertain dynamics: 0.7-0.75
/// </summary>
public double MaxBlendRatio { get; set; } = 0.8f;
/// <summary>
/// Default blend ratio (startup)
/// Default: 0.6 (60% model, 40% encoder)
///
/// Meaning: Initial blend before adaptation kicks in
///
/// Tuning Tips:
/// - Should be between Min and Max
/// - Balanced: 0.5-0.6
/// - Trust model more: 0.65-0.7
/// </summary>
public double DefaultBlendRatio { get; set; } = 0.6;
/// <summary>
/// Good tracking error threshold
/// Default: 0.12 (12% error)
///
/// Meaning: If |predicted - actual| / actual < 12% → tracking is "good"
///
/// ↑ Increase (0.15-0.2):
/// ✓ Easier to achieve "good" status
/// ✗ May accept mediocre tracking
///
/// ↓ Decrease (0.08-0.1):
/// ✓ Stricter quality requirement
/// ✗ May rarely achieve "good"
///
/// Tuning Tips:
/// - Well-tuned system: 0.1-0.12
/// - Noisy system: 0.15-0.2
/// </summary>
public double GoodTrackingThreshold { get; set; } = 0.12f;
/// <summary>
/// Moderate tracking error threshold
/// Default: 0.3 (30% error)
///
/// Meaning: If error 12-30% → "moderate", >30% → "poor"
///
/// Tuning Tips:
/// - Should be > GoodTrackingThreshold
/// - Typical: 2-3× good threshold
/// </summary>
public double ModerateTrackingThreshold { get; set; } = 0.3;
/// <summary>
/// Blend ratio for good tracking
/// Default: 0.7 (70% model)
///
/// Meaning: When tracking well, trust model more
///
/// Tuning Tips:
/// - Reward good tracking: 0.7-0.75
/// - Conservative: 0.6-0.65
/// </summary>
public double GoodTrackingBlend { get; set; } = 0.7;
/// <summary>
/// Blend ratio for moderate tracking
/// Default: 0.5 (50% model, 50% encoder)
///
/// Meaning: Balanced when tracking is OK
/// </summary>
public double ModerateTrackingBlend { get; set; } = 0.5;
/// <summary>
/// Blend ratio for poor tracking
/// Default: 0.25 (25% model, 75% encoder)
///
/// Meaning: Trust encoder more when model is wrong
///
/// Tuning Tips:
/// - Very noisy encoders: 0.3-0.35
/// - Good encoders: 0.2-0.25
/// </summary>
public double PoorTrackingBlend { get; set; } = 0.25f;
/// <summary>
/// Confidence exponential decay rate
/// Default: 0.95 (5% decay per sample)
///
/// Meaning: How fast confidence updates
/// Formula: confidence = 0.95 × old + 0.05 × new
///
/// ↑ Increase (0.97-0.99):
/// ✓ Slower, smoother updates
/// ✗ Slow to detect changes
///
/// ↓ Decrease (0.9-0.93):
/// ✓ Faster adaptation
/// ✗ May be jittery
///
/// Tuning Tips:
/// - Stable system: 0.95-0.97
/// - Dynamic system: 0.92-0.94
/// </summary>
public double ConfidenceDecayRate { get; set; } = 0.95f;
/// <summary>
/// Minimum confidence floor
/// Default: 0.3 (30%)
///
/// Meaning: Never go below this confidence level
///
/// Tuning Tips:
/// - Safety-critical: 0.4-0.5 (cautious)
/// - Performance-focused: 0.2-0.3 (aggressive)
/// </summary>
public double MinConfidence { get; set; } = 0.3;
}
/// <summary>
/// Velocity signal processing configuration
/// Filters encoder velocity noise
/// </summary>
public class VelocitySignalProcessingConfig
{
/// <summary>
/// EMA (Exponential Moving Average) filter alpha
/// Default: 0.3
///
/// Meaning: Weight for new sample in filter
/// Formula: filtered = alpha × new + (1-alpha) × old
///
/// ↑ Increase (0.4-0.6):
/// ✓ More responsive to changes
/// ✗ Less noise filtering
/// ✗ May be jittery
///
/// ↓ Decrease (0.1-0.2):
/// ✓ More noise filtering
/// ✓ Smoother signal
/// ✗ Slower response
/// ✗ May lag actual velocity
///
/// Tuning Tips:
/// - Noisy encoders: 0.2-0.3 (more filtering)
/// - Clean encoders: 0.4-0.5 (more responsive)
/// - High-frequency control: 0.3-0.4
/// </summary>
public double AlphaFilter { get; set; } = 0.3;
/// <summary>
/// Noise detection threshold (m/s)
/// Default: 0.5 m/s
///
/// Meaning: Velocity changes > this are considered noise spikes
///
/// ↑ Increase (0.8-1.0):
/// ✓ Allow larger velocity changes
/// ✗ May not filter big spikes
///
/// ↓ Decrease (0.3-0.4):
/// ✓ Filter smaller spikes
/// ✗ May filter legitimate changes
///
/// Tuning Tips:
/// - Check max acceleration: threshold > max_accel × sample_time
/// - Example: 2m/s² accel, 30Hz → 0.067 m/s change/sample
/// - Set threshold ~5-10× expected change: 0.3-0.5 m/s
/// </summary>
public double NoiseThreshold { get; set; } = 0.5;
}
/// <summary>
/// Navigation system limits configuration
/// Physical and safety constraints
/// </summary>
public class NavigationConfig
{
/// <summary>
/// Maximum linear velocity (m/s)
/// Default: 1.5 m/s
///
/// Meaning: Top speed for robot during navigation
///
/// ↑ Increase (2.0-3.0 m/s):
/// ✓ Faster navigation
/// ✗ Requires more braking distance
/// ✗ May lose traction/stability
/// ✗ Safety concerns
///
/// ↓ Decrease (0.8-1.2 m/s):
/// ✓ Safer operation
/// ✓ More precise control
/// ✗ Slower task completion
///
/// Tuning Tips:
/// - MUST match motor controller limits
/// - Warehouse AGV: 1.0-1.5 m/s
/// - Outdoor robot: 2.0-3.0 m/s
/// - Crowded areas: 0.5-0.8 m/s
/// - Check: Braking distance = v²/(2×decel) < safety margin
/// </summary>
public double MaxLinearVelocity { get; set; } = 1.5;
/// <summary>
/// Maximum angular velocity (rad/s)
/// Default: 6.0 rad/s (≈344°/s)
///
/// Meaning: Fastest rotation speed (for in-place rotation)
///
/// ↑ Increase (8.0-10.0 rad/s):
/// ✓ Faster orientation changes
/// ✗ May be unsafe
/// ✗ High stress on motors
///
/// ↓ Decrease (4.0-5.0 rad/s):
/// ✓ Safer, gentler
/// ✗ Slower rotations
///
/// Tuning Tips:
/// - MUST match motor limits
/// - This is for in-place rotation (not tracking)
/// - Typical: 4-8 rad/s
/// - Heavy robot: 3-5 rad/s
/// </summary>
public double MaxAngularVelocity { get; set; } = 6.0;
/// <summary>
/// Minimum linear velocity (m/s)
/// Default: 0.1 m/s
///
/// Meaning: Slowest speed before considering "stopped"
///
/// ↑ Increase (0.15-0.2 m/s):
/// ✓ Avoid very slow creeping
/// ✗ Less precision at low speed
///
/// ↓ Decrease (0.05-0.08 m/s):
/// ✓ More precise low-speed control
/// ✗ May be too slow/jerky
///
/// Tuning Tips:
/// - Should be > encoder resolution
/// - Typical: 0.08-0.15 m/s
/// </summary>
public double MinLinearVelocity { get; set; } = 0.1;
/// <summary>
/// Angular velocity for in-place rotation (rad/s)
/// Default: 1.0 rad/s (≈57°/s)
///
/// Meaning: Speed when robot rotates without moving forward
///
/// ↑ Increase (1.5-2.0 rad/s):
/// ✓ Faster reorientation
/// ✗ Less smooth
///
/// ↓ Decrease (0.5-0.8 rad/s):
/// ✓ Gentle rotation
/// ✗ Slower
///
/// Tuning Tips:
/// - Should be < MaxAngularVelocity
/// - Gentle: 0.5-1.0 rad/s
/// - Fast: 1.5-2.0 rad/s
/// </summary>
public double RotateAngularVelocity { get; set; } = 1.0;
/// <summary>
/// Goal reached radius (meters)
/// Default: 0.015m (1.5cm)
///
/// Meaning: Distance to consider navigation complete
///
/// ↑ Increase (0.03-0.05m):
/// ✓ Easier to "reach" goal
/// ✓ Faster completion
/// ✗ Lower precision
///
/// ↓ Decrease (0.01m):
/// ✓ Higher precision
/// ✗ May never reach (localization error)
///
/// Tuning Tips:
/// - Must be ≥ localization RMS error
/// - Conservative: 0.02-0.03m
/// - High precision: 0.01-0.015m (if localization allows)
/// </summary>
public double ReachedRadius { get; set; } = 0.015;
/// <summary>
/// Initial rotation threshold (degrees)
/// Default: 20.0°
///
/// Meaning: If heading error to first lookahead point exceeds this, rotate in place first
///
/// ↑ Increase (30-45°):
/// ✓ Start moving sooner (less initial rotation)
/// ✗ May approach path from poor angle
///
/// ↓ Decrease (10-15°):
/// ✓ Better initial alignment
/// ✗ More time spent rotating before moving
///
/// Tuning Tips:
/// - Tight spaces: 10-15° (precision critical)
/// - Open areas: 25-35° (faster start)
/// - Balance: 20-25°
/// </summary>
public double InitialRotationThreshold { get; set; } = 5.0;
/// <summary>
/// Linear acceleration (m/s²)
/// Default: 0.5 m/s²
///
/// Meaning: How quickly the robot is allowed to reach target linear speed
///
/// ↑ Increase (1.0-2.0 m/s²):
/// ✓ Faster response to speed commands
/// ✓ Shorter ramp-up time
/// ✗ May cause slip or load spike
/// ✗ Less smooth start
///
/// ↓ Decrease (0.2-0.4 m/s²):
/// ✓ Smoother, gentler start
/// ✓ Better traction
/// ✗ Slower to reach target speed
///
/// Tuning Tips:
/// - Must not exceed motor/drive limits
/// - Heavy load or slippery floor: use lower (0.3-0.5)
/// - Empty AGV on good floor: 0.8-1.5 typical
/// - Match to Deceleration for symmetric feel
/// </summary>
public double Acceleration { get; set; } = 0.5;
/// <summary>
/// Linear deceleration (m/s²)
/// Default: 0.5 m/s²
///
/// Meaning: How quickly the robot is allowed to slow down / stop
///
/// ↑ Increase (1.0-2.0 m/s²):
/// ✓ Faster stopping
/// ✓ Shorter braking distance
/// ✗ May cause slip or cargo shift
/// ✗ Less smooth stop
///
/// ↓ Decrease (0.2-0.4 m/s²):
/// ✓ Smoother stop
/// ✓ Safer for fragile load
/// ✗ Longer braking distance
///
/// Tuning Tips:
/// - Often set equal to or slightly higher than Acceleration for safe stop
/// - Safety: ensure Deceleration allows stop within ReachedRadius
/// - Slippery surface: use lower value
/// </summary>
public double Deceleration { get; set; } = 0.5;
}

10
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/PagedResult.cs Normal file
View File

@@ -0,0 +1,10 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// Paged API result
/// </summary>
public class PagedResult<T>
{
public int TotalCount { get; set; }
public List<T> Items { get; set; } = new();
}

58
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/PathEdge.cs Normal file
View File

@@ -0,0 +1,58 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// Edge in a custom path
/// Represents a segment of the path with start, end, and optional control points for curves
/// </summary>
public class PathEdge
{
/// <summary>
/// Start point X coordinate (meters)
/// </summary>
public double StartX { get; set; }
/// <summary>
/// Start point Y coordinate (meters)
/// </summary>
public double StartY { get; set; }
/// <summary>
/// End point X coordinate (meters)
/// </summary>
public double EndX { get; set; }
/// <summary>
/// End point Y coordinate (meters)
/// </summary>
public double EndY { get; set; }
/// <summary>
/// Degree of the curve (1 = linear, 2 = quadratic Bezier, 3 = cubic Bezier)
/// </summary>
public int Degree { get; set; } = 1;
/// <summary>
/// First control point X (for Degree 2 and 3)
/// </summary>
public double? ControlPoint1X { get; set; }
/// <summary>
/// First control point Y (for Degree 2 and 3)
/// </summary>
public double? ControlPoint1Y { get; set; }
/// <summary>
/// Second control point X (for Degree 3 only)
/// </summary>
public double? ControlPoint2X { get; set; }
/// <summary>
/// Second control point Y (for Degree 3 only)
/// </summary>
public double? ControlPoint2Y { get; set; }
/// <summary>
/// Direction of movement along this edge
/// </summary>
public RobotDirection Direction { get; set; } = RobotDirection.FORWARD;
}

40
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/Pose2D.cs Normal file
View File

@@ -0,0 +1,40 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// 2D Pose (position + heading)
/// </summary>
public struct Pose2D
{
public double X { get; set; }
public double Y { get; set; }
public double Theta { get; set; } // radians
public Pose2D(double x, double y, double theta)
{
X = x;
Y = y;
Theta = theta;
}
public static double Distance(Pose2D a, Pose2D b)
{
double dx = b.X - a.X;
double dy = b.Y - a.Y;
return Math.Sqrt(dx * dx + dy * dy);
}
}
/// <summary>
/// 2D Twist (linear + angular velocity)
/// </summary>
public struct Twist2D
{
public double Linear { get; set; } // m/s
public double Angular { get; set; } // rad/s
public Twist2D(double linear, double angular)
{
Linear = linear;
Angular = angular;
}
}

99
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/StraightLineScenario.cs Normal file
View File

@@ -0,0 +1,99 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// Straight line test scenario (client-side version)
/// </summary>
public class StraightLineScenario : TestScenario
{
public double Length { get; set; } = 10.0; // meters
public double StartX { get; set; } = 0.0;
public double StartY { get; set; } = 0.0;
public double StartTheta { get; set; } = 0.0; // radians
public double Resolution { get; set; } = 0.05; // meters between points
public StraightLineScenario()
{
Name = "Straight Line 10m";
Description = "Robot moves in a straight line for 10 meters";
Type = TrajectoryType.StraightLine;
}
public override List<PathPoint> GenerateReferencePath()
{
var points = new List<PathPoint>();
double absLength = Math.Abs(Length);
bool isBackward = Length < 0;
// For backward movement, reverse the direction
double directionTheta = isBackward ? StartTheta + Math.PI : StartTheta;
// Normalize directionTheta to [-π, π]
while (directionTheta > Math.PI) directionTheta -= 2 * Math.PI;
while (directionTheta < -Math.PI) directionTheta += 2 * Math.PI;
var direction = isBackward ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
// Start point
points.Add(new PathPoint
{
X = StartX,
Y = StartY,
Direction = direction,
DistanceFromStart = 0.0
});
// Generate intermediate points
int numPoints = (int)(absLength / Resolution);
for (int i = 1; i <= numPoints; i++)
{
double distance = i * Resolution;
if (distance > absLength) distance = absLength;
points.Add(new PathPoint
{
X = StartX + distance * Math.Cos(directionTheta),
Y = StartY + distance * Math.Sin(directionTheta),
Direction = direction,
DistanceFromStart = distance
});
}
// Ensure end point is exactly at absLength
if (points[^1].DistanceFromStart < absLength)
{
points.Add(new PathPoint
{
X = StartX + absLength * Math.Cos(directionTheta),
Y = StartY + absLength * Math.Sin(directionTheta),
Direction = direction,
DistanceFromStart = absLength
});
}
return points;
}
public override bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f)
{
var goal = GetGoalPose();
double distance = Pose2D.Distance(currentPose, goal);
return distance <= tolerance;
}
public override Pose2D GetGoalPose()
{
double absLength = Math.Abs(Length);
bool isBackward = Length < 0;
double directionTheta = isBackward ? StartTheta + Math.PI : StartTheta;
// Normalize directionTheta to [-π, π]
while (directionTheta > Math.PI) directionTheta -= 2 * Math.PI;
while (directionTheta < -Math.PI) directionTheta += 2 * Math.PI;
return new Pose2D(
StartX + absLength * Math.Cos(directionTheta),
StartY + absLength * Math.Sin(directionTheta),
directionTheta
);
}
}

31
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/TelemetryData.cs Normal file
View File

@@ -0,0 +1,31 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// Navigation phase for telemetry segmentation.
/// </summary>
public enum TelemetryPhase
{
InitialRotation = 0,
PathFollowing = 1,
FinalApproach = 2,
FinalRotation = 3,
Completed = 4
}
/// <summary>
/// Telemetry data collected during test execution
/// </summary>
public class TelemetryData
{
public long TimestampMs { get; set; }
public Pose2D RobotPose { get; set; } // X, Y, Theta
public Twist2D RobotTwist { get; set; } // Linear, Angular velocity (estimated)
public Twist2D CommandTwist { get; set; } // Linear, Angular velocity (commanded)
public Pose2D ReferencePose { get; set; } // Closest point on reference path
public double CrossTrackError { get; set; } // meters
public double HeadingError { get; set; } // radians
public double LookaheadDistance { get; set; } // meters
public double ModelConfidence { get; set; } // 0.0 - 1.0
public double DistanceToGoal { get; set; } // meters
public TelemetryPhase? Phase { get; set; } // Navigation phase (null for legacy data)
}

128
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/TestMetrics.cs Normal file
View File

@@ -0,0 +1,128 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// Complete test metrics calculated from telemetry data after a tuning test run.
/// Groups: Tracking Accuracy (bám đường), Smoothness (độ mượt), Efficiency (hiệu quả), and Scores (0–100).
/// </summary>
public class TestMetrics
{
/// <summary>Unique identifier for this metrics record.</summary>
public Guid Id { get; set; } = Guid.NewGuid();
/// <summary>Id of the test run this metrics belong to.</summary>
public Guid TestRunId { get; set; }
// --- Tracking Accuracy (độ chính xác bám đường) ---
/// <summary>Root-mean-square of cross-track error (CTE) in meters. Khoảng cách vuông góc từ robot tới reference path. Mục tiêu &lt; 0.10 m (10 cm).</summary>
public double CrossTrackErrorRMS { get; set; }
/// <summary>Peak (max) cross-track error in meters. Giá trị CTE lớn nhất trong test. Mục tiêu &lt; 0.20 m.</summary>
public double CrossTrackErrorPeak { get; set; }
/// <summary>Mean cross-track error in meters.</summary>
public double CrossTrackErrorMean { get; set; }
/// <summary>Standard deviation of cross-track error in meters. Độ phân tán của lỗi bám đường.</summary>
public double CrossTrackErrorStdDev { get; set; }
/// <summary>Root-mean-square of heading error in radians. Lỗi góc hướng (rad). Mục tiêu tương đương &lt; 10°.</summary>
public double HeadingErrorRMS { get; set; }
/// <summary>Peak (max) heading error in radians.</summary>
public double HeadingErrorPeak { get; set; }
/// <summary>Position error at goal in meters (average over last ~10% of trajectory). Lỗi vị trí tại điểm đích. Mục tiêu &lt; 0.05 m (5 cm).</summary>
public double GoalPositionError { get; set; }
/// <summary>Heading error at goal in radians. Lỗi góc tại điểm đích. Mục tiêu tương đương &lt; 5°.</summary>
public double GoalHeadingError { get; set; }
// --- Smoothness (độ mượt chuyển động) ---
/// <summary>Standard deviation of linear velocity (m/s). Biến động tốc độ dọc đường. Mục tiêu &lt; 0.1 m/s.</summary>
public double VelocityStdDev { get; set; }
/// <summary>Standard deviation of linear acceleration (m/s²). Biến động gia tốc. Mục tiêu &lt; 0.5 m/s².</summary>
public double AccelerationStdDev { get; set; }
// --- Efficiency (hiệu quả) ---
/// <summary>Ratio actual path length / reference path length. &gt; 1 = đi dài hơn đường chuẩn. Mục tiêu &lt; 1.15 (115%).</summary>
public double PathLengthRatio { get; set; }
/// <summary>Total time to complete the test in seconds.</summary>
public double CompletionTime { get; set; }
/// <summary>Average linear speed during test in m/s.</summary>
public double AverageSpeed { get; set; }
/// <summary>Maximum linear speed during test in m/s.</summary>
public double MaxSpeed { get; set; }
// --- Scores (0–100) ---
/// <summary>Overall score 0–100. Weighted: Tracking 50%, Smoothness 30%, Efficiency 20%.</summary>
public double OverallScore { get; set; }
/// <summary>Tracking accuracy score 0–100 (CTE, heading, goal error).</summary>
public double TrackingScore { get; set; }
/// <summary>Smoothness score 0–100 (velocity stddev, acceleration stddev).</summary>
public double SmoothnessScore { get; set; }
/// <summary>Efficiency score 0–100 (path length ratio).</summary>
public double EfficiencyScore { get; set; }
/// <summary>True if all acceptance criteria are met (CTE RMS/peak, heading, goal error, path length ratio).</summary>
public bool PassedCriteria { get; set; }
}
/// <summary>
/// Tracking accuracy metrics: độ chính xác bám đường (CTE, heading, goal error).
/// </summary>
public class TrackingAccuracyMetrics
{
/// <summary>RMS cross-track error (m).</summary>
public double CrossTrackErrorRMS { get; set; }
/// <summary>Peak cross-track error (m).</summary>
public double CrossTrackErrorPeak { get; set; }
/// <summary>Mean cross-track error (m).</summary>
public double CrossTrackErrorMean { get; set; }
/// <summary>Std dev cross-track error (m).</summary>
public double CrossTrackErrorStdDev { get; set; }
/// <summary>RMS heading error (rad).</summary>
public double HeadingErrorRMS { get; set; }
/// <summary>Peak heading error (rad).</summary>
public double HeadingErrorPeak { get; set; }
/// <summary>Position error at goal (m).</summary>
public double GoalPositionError { get; set; }
/// <summary>Heading error at goal (rad).</summary>
public double GoalHeadingError { get; set; }
}
/// <summary>
/// Smoothness metrics: độ mượt chuyển động (biến động vận tốc, gia tốc).
/// </summary>
public class SmoothnessMetrics
{
/// <summary>Std dev of linear velocity (m/s).</summary>
public double VelocityStdDev { get; set; }
/// <summary>Std dev of linear acceleration (m/s²).</summary>
public double AccelerationStdDev { get; set; }
}
/// <summary>
/// Efficiency metrics: hiệu quả (quãng đường, thời gian, tốc độ).
/// </summary>
public class EfficiencyMetrics
{
/// <summary>Actual path length / reference path length.</summary>
public double PathLengthRatio { get; set; }
/// <summary>Completion time (s).</summary>
public double CompletionTime { get; set; }
/// <summary>Average speed (m/s).</summary>
public double AverageSpeed { get; set; }
/// <summary>Max speed (m/s).</summary>
public double MaxSpeed { get; set; }
}

97
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/TestRun.cs Normal file
View File

@@ -0,0 +1,97 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// Test run execution record
/// </summary>
public class TestRun
{
public Guid Id { get; set; } = Guid.NewGuid();
public Guid ScenarioId { get; set; }
public Guid ParameterSetId { get; set; }
public DateTime StartTime { get; set; }
public DateTime? EndTime { get; set; }
public TestStatus Status { get; set; }
public double Duration { get; set; } // seconds
public string? Notes { get; set; }
public string? ErrorMessage { get; set; }
// Navigation properties
public TestScenario? Scenario { get; set; }
public NavigationParameterSet? ParameterSet { get; set; }
public TestMetrics? Metrics { get; set; }
public List<SafetyViolation> SafetyViolations { get; set; } = new();
}
/// <summary>
/// DTO for TestRun API responses. Use this to avoid deserializing abstract TestScenario on the client.
/// </summary>
public class TestRunDto
{
public Guid Id { get; set; }
public Guid ScenarioId { get; set; }
public string ScenarioName { get; set; } = string.Empty;
public Guid ParameterSetId { get; set; }
public string ParameterSetName { get; set; } = string.Empty;
public DateTime StartTime { get; set; }
public DateTime? EndTime { get; set; }
public TestStatus Status { get; set; }
public double Duration { get; set; }
public string? Notes { get; set; }
public string? ErrorMessage { get; set; }
public TestMetrics? Metrics { get; set; }
public List<SafetyViolation> SafetyViolations { get; set; } = new();
}
/// <summary>
/// Request body for delete-batch API
/// </summary>
public class DeleteBatchRequest
{
public List<Guid> Ids { get; set; } = new();
}
/// <summary>
/// Test execution status
/// </summary>
public enum TestStatus
{
Preparing = 0,
Running = 1,
Paused = 2,
Completed = 3,
Aborted = 4,
Error = 5,
EmergencyStopped = 6
}
/// <summary>
/// Safety violation during test
/// </summary>
public class SafetyViolation
{
public Guid Id { get; set; } = Guid.NewGuid();
public Guid TestRunId { get; set; }
public DateTime Timestamp { get; set; }
public ViolationType Type { get; set; }
public ViolationSeverity Severity { get; set; }
public double Value { get; set; }
public double Threshold { get; set; }
public string Message { get; set; } = string.Empty;
}
public enum ViolationType
{
CrossTrackError = 1,
HeadingError = 2,
VelocityLimit = 3,
AccelerationLimit = 4,
SustainedTrackingError = 5,
ObstacleProximity = 6
}
public enum ViolationSeverity
{
Info = 0,
Warning = 1,
Critical = 2
}

121
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/TestScenario.cs Normal file
View File

@@ -0,0 +1,121 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// Base class for test scenarios
/// </summary>
public abstract class TestScenario
{
public Guid Id { get; set; } = Guid.NewGuid();
public string Name { get; set; } = string.Empty;
public string Description { get; set; } = string.Empty;
public TrajectoryType Type { get; set; }
public DateTime CreatedAt { get; set; } = DateTime.UtcNow;
public bool IsDefault { get; set; }
/// <summary>
/// Generate reference path for this scenario
/// </summary>
public abstract List<PathPoint> GenerateReferencePath();
/// <summary>
/// Check if goal is reached
/// </summary>
public abstract bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f);
/// <summary>
/// Get goal pose
/// </summary>
public abstract Pose2D GetGoalPose();
}
/// <summary>
/// Trajectory type
/// </summary>
public enum TrajectoryType
{
StraightLine = 1,
Circle = 2,
Square = 3,
SCurve = 4,
Custom = 99
}
/// <summary>
/// Direction for robot movement
/// </summary>
public enum RobotDirection
{
FORWARD,
BACKWARD
}
/// <summary>
/// Point on reference path
/// </summary>
public class PathPoint
{
public double X { get; set; }
public double Y { get; set; }
public RobotDirection Direction { get; set; } = RobotDirection.FORWARD;
public double DistanceFromStart { get; set; } // cumulative distance
}
/// <summary>
/// Reference path
/// </summary>
public class ReferencePath
{
public List<PathPoint> Points { get; set; } = new();
public double TotalLength { get; set; }
public PathPoint? GetClosestPoint(Pose2D pose)
{
if (Points.Count == 0) return null;
double minDistance = double.MaxValue;
PathPoint? closest = null;
foreach (var point in Points)
{
double dx = pose.X - point.X;
double dy = pose.Y - point.Y;
double distance = Math.Sqrt(dx * dx + dy * dy);
if (distance < minDistance)
{
minDistance = distance;
closest = point;
}
}
return closest;
}
public PathPoint? GetPointAtDistance(double distance)
{
if (Points.Count == 0) return null;
if (distance <= 0) return Points[0];
if (distance >= TotalLength) return Points[^1];
// Find segment containing this distance
for (int i = 0; i < Points.Count - 1; i++)
{
if (distance >= Points[i].DistanceFromStart && distance <= Points[i + 1].DistanceFromStart)
{
// Interpolate
double segmentLength = Points[i + 1].DistanceFromStart - Points[i].DistanceFromStart;
double t = (distance - Points[i].DistanceFromStart) / segmentLength;
return new PathPoint
{
X = Points[i].X + t * (Points[i + 1].X - Points[i].X),
Y = Points[i].Y + t * (Points[i + 1].Y - Points[i].Y),
Direction = Points[i].Direction, // Use direction from start point
DistanceFromStart = distance
};
}
}
return Points[^1];
}
}

156
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/TuningAdvisorModels.cs Normal file
View File

@@ -0,0 +1,156 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// Priority level for tuning suggestions.
/// </summary>
public enum SuggestionPriority
{
Low = 0,
Medium = 1,
High = 2,
Critical = 3
}
/// <summary>
/// Category of the diagnostic pattern detected from telemetry analysis.
/// </summary>
public enum DiagnosticCategory
{
Oscillation,
LargeCTE,
CornerCutting,
GoalOvershoot,
SluggishResponse,
JerkyMotion,
VelocityEstimation,
GoalHeadingError,
PathEfficiency
}
/// <summary>
/// A detected behavioral pattern from telemetry analysis.
/// </summary>
public class DiagnosticPattern
{
public DiagnosticCategory Category { get; set; }
/// <summary>Human-readable description of what was detected.</summary>
public string Description { get; set; } = string.Empty;
/// <summary>Severity of the pattern (0.0 = negligible, 1.0 = critical).</summary>
public double Severity { get; set; }
/// <summary>Evidence values that led to the detection (e.g., oscillation frequency, CTE values).</summary>
public Dictionary<string, double> Evidence { get; set; } = new();
/// <summary>Which navigation phase this pattern was detected in (null = all phases combined).</summary>
public TelemetryPhase? DetectedInPhase { get; set; }
}
/// <summary>
/// A single parameter adjustment suggestion.
/// </summary>
public class TuningSuggestion
{
public Guid Id { get; set; } = Guid.NewGuid();
/// <summary>
/// Dot-path to the parameter, e.g., "PurePursuitConfig.LookaheadMin" or "StanleyConfig.K".
/// </summary>
public string ParameterPath { get; set; } = string.Empty;
/// <summary>Display name for the parameter.</summary>
public string ParameterDisplayName { get; set; } = string.Empty;
/// <summary>Current value of the parameter.</summary>
public double CurrentValue { get; set; }
/// <summary>Suggested new value.</summary>
public double SuggestedValue { get; set; }
/// <summary>Human-readable reason for the suggestion.</summary>
public string Reason { get; set; } = string.Empty;
/// <summary>Priority of this suggestion.</summary>
public SuggestionPriority Priority { get; set; }
/// <summary>Confidence in this suggestion (0.0 - 1.0).</summary>
public double Confidence { get; set; }
/// <summary>Which diagnostic pattern(s) triggered this suggestion.</summary>
public List<DiagnosticCategory> RelatedPatterns { get; set; } = new();
/// <summary>Expected impact description.</summary>
public string ExpectedImpact { get; set; } = string.Empty;
/// <summary>Which navigation phase this suggestion targets (null = general).</summary>
public TelemetryPhase? TargetPhase { get; set; }
/// <summary>Percentage change from current to suggested.</summary>
public double ChangePercent => CurrentValue != 0
? ((SuggestedValue - CurrentValue) / Math.Abs(CurrentValue)) * 100.0
: 0;
}
/// <summary>
/// Per-phase metrics computed from telemetry for phase-aware tuning analysis.
/// </summary>
public class PhaseMetrics
{
public TelemetryPhase Phase { get; set; }
public int SampleCount { get; set; }
public double DurationMs { get; set; }
// Tracking
public double CrossTrackErrorRMS { get; set; }
public double CrossTrackErrorPeak { get; set; }
public double CrossTrackErrorMean { get; set; }
public double HeadingErrorRMS { get; set; }
public double HeadingErrorPeak { get; set; }
// Smoothness
public double AngularVelocityStdDev { get; set; }
public double VelocityStdDev { get; set; }
public double AccelerationStdDev { get; set; }
// Goal (FinalApproach/FinalRotation only)
public double GoalPositionError { get; set; }
public double GoalHeadingErrorDeg { get; set; }
}
/// <summary>
/// Complete tuning analysis report for a test run.
/// </summary>
public class TuningReport
{
public Guid Id { get; set; } = Guid.NewGuid();
public Guid TestRunId { get; set; }
public DateTime GeneratedAt { get; set; } = DateTime.UtcNow;
/// <summary>Which controller was active during the test.</summary>
public PathFollowingController ControllerType { get; set; }
/// <summary>Overall assessment of the test quality.</summary>
public string OverallAssessment { get; set; } = string.Empty;
/// <summary>Detected diagnostic patterns from telemetry analysis.</summary>
public List<DiagnosticPattern> DetectedPatterns { get; set; } = new();
/// <summary>Ordered list of suggestions (highest priority first).</summary>
public List<TuningSuggestion> Suggestions { get; set; } = new();
/// <summary>Number of telemetry samples analyzed.</summary>
public int TelemetrySamplesAnalyzed { get; set; }
/// <summary>Sample count per navigation phase.</summary>
public Dictionary<TelemetryPhase, int> PhaseSampleCounts { get; set; } = new();
/// <summary>Per-phase metrics computed from telemetry.</summary>
public Dictionary<TelemetryPhase, PhaseMetrics> PhaseMetricsMap { get; set; } = new();
/// <summary>Whether any critical issues were detected.</summary>
public bool HasCriticalIssues => Suggestions.Any(s => s.Priority == SuggestionPriority.Critical);
/// <summary>Count of high+ priority suggestions.</summary>
public int HighPriorityCount => Suggestions.Count(s => s.Priority >= SuggestionPriority.High);
}