diff --git a/MedullaAdapter/DiverCartDefinition.cs b/MedullaAdapter/DiverCartDefinition.cs index 2f88f05..d8e3847 100644 --- a/MedullaAdapter/DiverCartDefinition.cs +++ b/MedullaAdapter/DiverCartDefinition.cs @@ -283,8 +283,6 @@ namespace MedullaAdapter Math.Sign(x); var steeringDegrees = -normalizedSteering * MaxManualTheta; - var frontTh = steeringDegrees; - var rearTh = -steeringDegrees; ManualMode = (int)mode; switch (mode) @@ -292,16 +290,24 @@ namespace MedullaAdapter case ManualControlMode.Normal: // 普通模式统一使用车体速度命令： // X向前，行驶中连续改变角速度时舵轮边转、车辆边走。 - // SendBodyCommand( - // vx: speed, - // vy: 0.0, - // omegaRadiansPerSecond: omega, - // interval); - Chassis.SendMotion( + var normalOmegaRadiansPerSecond = + speed * + Math.Tan( + AngleMath.DegreesToRadians( + steeringDegrees)) / + adapter.ControlPointRadiusMeters; + if (!adapter.SendBodyTwist( + new Twist2D( speed, - frontTh, - rearTh, - interval); + 0.0, + normalOmegaRadiansPerSecond), + interval)) + { + adapter.StopImmediately(); + Console.WriteLine( + "Normal SendMotion decomposition failed: " + + adapter.LastFailureReason); + } break; case ManualControlMode.Crab: // 舵轮机械范围为[-120°,120°]。 @@ -332,13 +338,15 @@ namespace MedullaAdapter // 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中 // 复用与普通模式相同的SendMotion前后控制点解算。 - if (!adapter.SendVirtualAckermannMotion( - motionDirectionRadians: - Math.PI / 2.0, - speedMetersPerSecond: + var crabOmegaRadiansPerSecond = + speed * + Math.Tan(crabSteeringRadians) / + adapter.ControlPointRadiusMeters; + if (!adapter.SendBodyTwist( + new Twist2D( + 0.0, speed, - steeringRadians: - crabSteeringRadians, + crabOmegaRadiansPerSecond), interval)) { adapter.StopImmediately(); @@ -380,13 +388,11 @@ namespace MedullaAdapter // 普通安全版SendXYThSpeed只下发角速度， // 四轮实际舵角未到位时不会开放驱动速度。 - if (!adapter.Send( - new ChassisCommand( - CarNum, - new Twist2D( - 0.0, - 0.0, - spinOmegaRadiansPerSecond)), + if (!adapter.SendBodyTwist( + new Twist2D( + 0.0, + 0.0, + spinOmegaRadiansPerSecond), interval)) { adapter.StopImmediately(); diff --git a/MedullaAdapter/MedullaAdapter.csproj b/MedullaAdapter/MedullaAdapter.csproj index c73779d..8d9d7e5 100644 --- a/MedullaAdapter/MedullaAdapter.csproj +++ b/MedullaAdapter/MedullaAdapter.csproj @@ -42,8 +42,8 @@ - + diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll index 48d9fcb..71ef99f 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll and b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll index c6e3d74..0602883 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb index 0409e29..517fda7 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ diff --git a/MultiWheelC/Control/Abstractions/PathTrackingContext.cs b/MultiWheelC/Control/Abstractions/PathTrackingContext.cs index 0f5ef4f..5e2af4d 100644 --- a/MultiWheelC/Control/Abstractions/PathTrackingContext.cs +++ b/MultiWheelC/Control/Abstractions/PathTrackingContext.cs @@ -1,6 +1,7 @@ using System; using MultiWheelC.StateEstimation; using MultiWheelC.Trajectory; +using MyParking.Shared; namespace MultiWheelC.Control.Abstractions { @@ -17,7 +18,8 @@ namespace MultiWheelC.Control.Abstractions TrajectoryProjection projection, double controlReferenceSpeedMetersPerSecond, double feedforwardCurvaturePerMeter, - double deltaTimeSeconds) + double deltaTimeSeconds, + double motionDirectionInBodyRadians = 0.0) { EnsureFinite( controlReferenceSpeedMetersPerSecond, @@ -28,6 +30,9 @@ namespace MultiWheelC.Control.Abstractions EnsureFinitePositive( deltaTimeSeconds, nameof(deltaTimeSeconds)); + EnsureFinite( + motionDirectionInBodyRadians, + nameof(motionDirectionInBodyRadians)); VehicleState = vehicleState; Projection = projection; @@ -36,6 +41,9 @@ namespace MultiWheelC.Control.Abstractions FeedforwardCurvaturePerMeter = feedforwardCurvaturePerMeter; DeltaTimeSeconds = deltaTimeSeconds; + MotionDirectionInBodyRadians = + AngleMath.NormalizeRadians( + motionDirectionInBodyRadians); } /// @@ -59,11 +67,18 @@ namespace MultiWheelC.Control.Abstractions public double ControlReferenceSpeedMetersPerSecond { get; } /// - /// 获取车辆在车体X轴方向上的实际纵向速度，单位为m/s。 + /// 获取车辆沿当前运动坐标系X轴方向的实际纵向速度，单位为m/s。 /// public double ActualLongitudinalSpeedMetersPerSecond => - VehicleState.TwistInBody - .VxMetersPerSecond; + Math.Cos(MotionDirectionInBodyRadians) * + VehicleState.TwistInBody.VxMetersPerSecond + + Math.Sin(MotionDirectionInBodyRadians) * + VehicleState.TwistInBody.VyMetersPerSecond; + + /// + /// 获取当前运动坐标系X轴在车体系中的方向，单位为rad。 + /// + public double MotionDirectionInBodyRadians { get; } /// /// 获取沿轨迹执行点序定义的参考曲率，单位为1/m，左弯为正。 diff --git a/MultiWheelC/Control/Execution/GcpCommandExecutor.cs b/MultiWheelC/Control/Execution/GcpCommandExecutor.cs index ee21a84..255f8fd 100644 --- a/MultiWheelC/Control/Execution/GcpCommandExecutor.cs +++ b/MultiWheelC/Control/Execution/GcpCommandExecutor.cs @@ -13,6 +13,7 @@ namespace MultiWheelC.Control.Execution 1e-6; private readonly MultiWheelChassisAdapter _chassisAdapter; + private readonly double _motionDirectionInBodyRadians; private double _lastFrontAngleRadians; private double _lastRearAngleRadians; @@ -22,7 +23,8 @@ namespace MultiWheelC.Control.Execution public GcpCommandExecutor( MultiWheelChassisAdapter chassisAdapter, double maximumGcpAngleRateRadiansPerSecond = - 10.0 * Math.PI / 180.0) + 10.0 * Math.PI / 180.0, + double motionDirectionInBodyRadians = 0.0) { _chassisAdapter = chassisAdapter ?? throw new ArgumentNullException( @@ -30,9 +32,15 @@ namespace MultiWheelC.Control.Execution EnsureFinitePositive( maximumGcpAngleRateRadiansPerSecond, nameof(maximumGcpAngleRateRadiansPerSecond)); + NumericGuard.EnsureFinite( + motionDirectionInBodyRadians, + nameof(motionDirectionInBodyRadians)); MaximumGcpAngleRateRadiansPerSecond = maximumGcpAngleRateRadiansPerSecond; + _motionDirectionInBodyRadians = + AngleMath.NormalizeRadians( + motionDirectionInBodyRadians); } /// @@ -103,10 +111,19 @@ namespace MultiWheelC.Control.Execution _lastRearAngleRadians); LastSentCommand = limitedCommand; - var success = _chassisAdapter.SendGcpMotion( - limitedCommand.SpeedMetersPerSecond, - limitedCommand.FrontAngleRadians, - limitedCommand.RearAngleRadians, + var motionFrameTwist = + GcpKinematics.ToBodyTwist( + limitedCommand, + _chassisAdapter.ControlPointRadiusMeters); + var bodyTwist = + FrameTransform2D.TransformTwistAtSamePoint( + new Pose2D( + 0.0, + 0.0, + _motionDirectionInBodyRadians), + motionFrameTwist); + var success = _chassisAdapter.SendBodyTwist( + bodyTwist, TimeSpan.FromSeconds(deltaTimeSeconds)); LastFailureReason = success diff --git a/MultiWheelC/Control/Execution/ParkingGeometricController.cs b/MultiWheelC/Control/Execution/ParkingGeometricController.cs index 0e3573a..e5a2a3e 100644 --- a/MultiWheelC/Control/Execution/ParkingGeometricController.cs +++ b/MultiWheelC/Control/Execution/ParkingGeometricController.cs @@ -86,6 +86,7 @@ namespace MultiWheelC.Control.Execution private readonly ILongitudinalController _longitudinalController; private readonly GcpCommandAllocator _gcpAllocator; private readonly GcpCommandExecutor _commandExecutor; + private readonly double _motionDirectionInBodyRadians; private Trajectory2D _trajectory; private double _terminalTravelDirection = 1.0; @@ -112,7 +113,8 @@ namespace MultiWheelC.Control.Execution double terminalApproachGainPerSecond = 0.8, double maximumTerminalApproachSpeedMetersPerSecond = 0.05, double curvaturePreviewSeconds = 0.20, - double maximumCurvaturePreviewMeters = 0.12) + double maximumCurvaturePreviewMeters = 0.12, + double motionDirectionInBodyRadians = 0.0) { _stateProvider = stateProvider ?? throw new ArgumentNullException( @@ -160,6 +162,9 @@ namespace MultiWheelC.Control.Execution EnsureFiniteNonNegative( maximumCurvaturePreviewMeters, nameof(maximumCurvaturePreviewMeters)); + NumericGuard.EnsureFinite( + motionDirectionInBodyRadians, + nameof(motionDirectionInBodyRadians)); if (terminalApproachDistanceMeters <= finishDistanceMeters) @@ -187,6 +192,9 @@ namespace MultiWheelC.Control.Execution CurvaturePreviewSeconds = curvaturePreviewSeconds; MaximumCurvaturePreviewMeters = maximumCurvaturePreviewMeters; + _motionDirectionInBodyRadians = + AngleMath.NormalizeRadians( + motionDirectionInBodyRadians); } /// @@ -484,7 +492,8 @@ namespace MultiWheelC.Control.Execution projection, controlReferenceSpeedMetersPerSecond, feedforwardCurvaturePerMeter, - deltaTimeSeconds); + deltaTimeSeconds, + _motionDirectionInBodyRadians); var lateralCommand = _lateralController.Compute(context); var commandSpeedMetersPerSecond = @@ -617,9 +626,8 @@ namespace MultiWheelC.Control.Execution var previewSpeedMetersPerSecond = vehicleState.HasValidVelocityEstimate - ? Math.Abs( - vehicleState.TwistInBody - .VxMetersPerSecond) + ? CalculateActualLongitudinalSpeedMetersPerSecond( + vehicleState) : Math.Abs( controlReferenceSpeedMetersPerSecond); @@ -931,13 +939,16 @@ namespace MultiWheelC.Control.Execution } /// - /// 计算车体坐标系实际纵向速度的绝对值，单位为m/s。 + /// 计算车辆沿当前运动坐标系X轴实际速度的绝对值，单位为m/s。 /// - private static double CalculateActualLongitudinalSpeedMetersPerSecond( + private double CalculateActualLongitudinalSpeedMetersPerSecond( VehicleState vehicleState) { return Math.Abs( - vehicleState.TwistInBody.VxMetersPerSecond); + Math.Cos(_motionDirectionInBodyRadians) * + vehicleState.TwistInBody.VxMetersPerSecond + + Math.Sin(_motionDirectionInBodyRadians) * + vehicleState.TwistInBody.VyMetersPerSecond); } /// diff --git a/MultiWheelC/Experiments/NewControllerTrackingTests.cs b/MultiWheelC/Experiments/NewControllerTrackingTests.cs index ad06f9a..a5fa694 100644 --- a/MultiWheelC/Experiments/NewControllerTrackingTests.cs +++ b/MultiWheelC/Experiments/NewControllerTrackingTests.cs @@ -137,6 +137,18 @@ namespace MultiWheelC protected virtual string ExperimentTrajectoryBaseName => "ProfiledStraight4m"; + /// + /// 获取直线主运动方向相对车头的夹角，单位为rad。 + /// + protected virtual double MotionDirectionInBodyRadians => + 0.0; + + /// + /// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。 + /// + protected virtual bool ReturnWheelsForwardAfterCompletion => + false; + /// /// 读取当前位姿、绘制离散轨迹并启动新版轨迹跟踪动作。 /// @@ -190,7 +202,8 @@ namespace MultiWheelC CruiseSpeedMetersPerSecond, AccelerationMetersPerSecondSquared, DecelerationMetersPerSecondSquared, - PointSpacingMeters); + PointSpacingMeters, + MotionDirectionInBodyRadians); DrawTrajectory(trajectory); @@ -226,6 +239,10 @@ namespace MultiWheelC { Trajectory = trajectory, StateProvider = _stateProvider, + MotionDirectionInBodyRadians = + MotionDirectionInBodyRadians, + ReturnWheelsForwardAfterCompletion = + ReturnWheelsForwardAfterCompletion, CycleObserver = controller => RecordControlCycle( recorder, @@ -467,6 +484,32 @@ namespace MultiWheelC "ProfiledReverseStraight4m"; } + /// + /// 将舵轮准备到车体左前45°，以0.4m/s跟踪4m直线，停车后再恢复车头方向。 + /// + [MovementTest(name = "新版控制器：45°蟹行4m直线轨迹跟踪")] + public sealed class NewControllerCrab45Straight4mTest + : NewControllerStraight4mTest + { + /// + /// 使用车体左前45°作为本次直线轨迹的固定运动方向。 + /// + protected override double MotionDirectionInBodyRadians => + Math.PI / 4.0; + + /// + /// 蟹行轨迹正常完成后主动将四个舵轮恢复到车头方向。 + /// + protected override bool ReturnWheelsForwardAfterCompletion => + true; + + /// + /// 将45°蟹行实验与普通前进和倒车实验的CSV名称明确区分。 + /// + protected override string ExperimentTrajectoryBaseName => + "ProfiledCrab45Straight4m"; + } + /// /// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。 /// diff --git a/MultiWheelC/Experiments/TestTrajectoryFactory.cs b/MultiWheelC/Experiments/TestTrajectoryFactory.cs index aad3f1b..06c6f6d 100644 --- a/MultiWheelC/Experiments/TestTrajectoryFactory.cs +++ b/MultiWheelC/Experiments/TestTrajectoryFactory.cs @@ -20,7 +20,8 @@ namespace MultiWheelC double cruiseSpeedMetersPerSecond = 0.30, double accelerationMetersPerSecondSquared = 0.20, double decelerationMetersPerSecondSquared = 0.20, - double pointSpacingMeters = 0.02) + double pointSpacingMeters = 0.02, + double motionDirectionInBodyRadians = 0.0) { return CreateStraight( startPoseInWorld, @@ -28,7 +29,8 @@ namespace MultiWheelC cruiseSpeedMetersPerSecond, accelerationMetersPerSecondSquared, decelerationMetersPerSecondSquared, - pointSpacingMeters); + pointSpacingMeters, + motionDirectionInBodyRadians); } /// @@ -40,7 +42,8 @@ namespace MultiWheelC double cruiseSpeedMetersPerSecond = 0.30, double accelerationMetersPerSecondSquared = 0.20, double decelerationMetersPerSecondSquared = 0.20, - double pointSpacingMeters = 0.02) + double pointSpacingMeters = 0.02, + double motionDirectionInBodyRadians = 0.0) { NumericGuard.EnsureFinite( startPoseInWorld, @@ -60,6 +63,9 @@ namespace MultiWheelC NumericGuard.EnsureFinitePositive( pointSpacingMeters, nameof(pointSpacingMeters)); + NumericGuard.EnsureFinite( + motionDirectionInBodyRadians, + nameof(motionDirectionInBodyRadians)); if (pointSpacingMeters > lengthMeters) { @@ -73,10 +79,13 @@ namespace MultiWheelC pointSpacingMeters); var points = new List( segmentCount + 1); + var worldMotionYawRadians = + startPoseInWorld.YawRadians + + motionDirectionInBodyRadians; var directionX = travelDirection * - Math.Cos(startPoseInWorld.YawRadians); + Math.Cos(worldMotionYawRadians); var directionY = travelDirection * - Math.Sin(startPoseInWorld.YawRadians); + Math.Sin(worldMotionYawRadians); for (var index = 0; index <= segmentCount; diff --git a/MultiWheelC/Movements/PrepareWheelsForward.cs b/MultiWheelC/Movements/PrepareWheelsForward.cs index 6beb03a..a6425b5 100644 --- a/MultiWheelC/Movements/PrepareWheelsForward.cs +++ b/MultiWheelC/Movements/PrepareWheelsForward.cs @@ -14,6 +14,11 @@ namespace MultiWheelC /// public class PrepareWheelsForward : MovementDefinition { + /// + /// 获取或设置舵轮需要对准的车体方向，单位为rad；0表示车头方向。 + /// + public double DirectionRadians; + /// /// 获取或设置本次动作的回正到位容差覆盖值，单位为deg；为空时读取车辆配置。 /// @@ -36,6 +41,10 @@ namespace MultiWheelC /// public override IEnumerable Get() { + NumericGuard.EnsureFinite( + DirectionRadians, + nameof(DirectionRadians)); + var config = PilotDefinition.Conf; var toleranceDegrees = ToleranceDegrees ?? @@ -75,10 +84,11 @@ namespace MultiWheelC DateTime? alignedSince = null; Completed = false; - if (!adapter.PrepareParallelDirection(0.0)) + if (!adapter.PrepareParallelDirection( + DirectionRadians)) { throw new InvalidOperationException( - "无法将所有舵轮下发到车体前向0°。"); + "无法将所有舵轮下发到指定运动方向。"); } try @@ -87,7 +97,7 @@ namespace MultiWheelC { var aligned = adapter.AreParallelWheelsAligned( - 0.0, + DirectionRadians, toleranceRadians); if (aligned) @@ -122,7 +132,7 @@ namespace MultiWheelC } finally { - // 只清零驱动速度，保留已经下发的0°舵角。 + // 只清零驱动速度，保留已经下发的目标舵角。 adapter.StopImmediately(); } } diff --git a/MultiWheelC/Movements/RotateInPlace.cs b/MultiWheelC/Movements/RotateInPlace.cs index 64b4836..c9b221c 100644 --- a/MultiWheelC/Movements/RotateInPlace.cs +++ b/MultiWheelC/Movements/RotateInPlace.cs @@ -233,17 +233,14 @@ namespace MultiWheelC var interval = now - lastCommandTime; lastCommandTime = now; - // PID输出s为deg/s，Shared命令统一使用rad/s。 - // adapter.Send最终调用普通安全版SendXYThSpeed。 + // PID输出s为deg/s，Shared统一使用车体坐标系Twist2D和rad/s。 var omegaRadiansPerSecond = (float)AngleMath.DegreesToRadians(s); - if (!adapter.Send( - new ChassisCommand( - PilotDefinition.Self.CarNum, - new Twist2D( - 0.0, - 0.0, - omegaRadiansPerSecond)), + if (!adapter.SendBodyTwist( + new Twist2D( + 0.0, + 0.0, + omegaRadiansPerSecond), interval)) { throw new InvalidOperationException( diff --git a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs index a9e40fe..4787916 100644 --- a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs +++ b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs @@ -42,6 +42,16 @@ namespace MultiWheelC /// public Action CycleObserver; + /// + /// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；0表示车头方向。 + /// + public double MotionDirectionInBodyRadians; + + /// + /// 获取或设置轨迹正常完成后是否停车并将舵轮主动恢复到车头方向。 + /// + public bool ReturnWheelsForwardAfterCompletion; + /// /// 获取或设置本次动作的Stanley横向误差增益覆盖值，单位为1/s；为空时读取车辆配置。 /// @@ -270,7 +280,11 @@ namespace MultiWheelC } var wheelPreparation = - new PrepareWheelsForward(); + new PrepareWheelsForward + { + DirectionRadians = + MotionDirectionInBodyRadians + }; foreach (var keepRunning in wheelPreparation.Get()) { if (!keepRunning) @@ -284,15 +298,15 @@ namespace MultiWheelC if (!wheelPreparation.Completed) { throw new InvalidOperationException( - "轨迹跟踪开始前舵轮未能稳定回到车头方向。"); + "轨迹跟踪开始前舵轮未能稳定到达目标运动方向。"); } var adapter = new MultiWheelChassisAdapter( chassis, PilotDefinition.Self.CarNum); - // 新版GCP控制统一以真实车头为车体X正方向，避免继承上一次蟹行偏置。 - adapter.ResetToBodyFrame(); + adapter.ActivateMotionFrame( + MotionDirectionInBodyRadians); var stateProvider = StateProvider ?? @@ -333,7 +347,8 @@ namespace MultiWheelC var commandExecutor = new GcpCommandExecutor( adapter, - maximumGcpAngleRateRadiansPerSecond); + maximumGcpAngleRateRadiansPerSecond, + MotionDirectionInBodyRadians); Controller = new ParkingGeometricController( stateProvider, @@ -350,7 +365,8 @@ namespace MultiWheelC terminalApproachGainPerSecond, maximumTerminalApproachSpeedMetersPerSecond, stanleyCurvaturePreviewSeconds, - stanleyMaximumCurvaturePreviewMeters); + stanleyMaximumCurvaturePreviewMeters, + MotionDirectionInBodyRadians); var clock = Stopwatch.StartNew(); var previousCycleSeconds = @@ -425,6 +441,28 @@ namespace MultiWheelC Controller.Cancel(); } + if (ReturnWheelsForwardAfterCompletion) + { + var forwardPreparation = + new PrepareWheelsForward(); + foreach (var keepRunning in + forwardPreparation.Get()) + { + if (!keepRunning) + { + break; + } + + yield return true; + } + + if (!forwardPreparation.Completed) + { + throw new InvalidOperationException( + "轨迹完成后舵轮未能稳定回到车头方向。"); + } + } + yield return false; } @@ -440,6 +478,10 @@ namespace MultiWheelC "新版轨迹跟踪动作没有设置Trajectory。"); } + NumericGuard.EnsureFinite( + MotionDirectionInBodyRadians, + nameof(MotionDirectionInBodyRadians)); + if (double.IsNaN(executionTimeoutSeconds) || double.IsInfinity(executionTimeoutSeconds) || executionTimeoutSeconds <= 0.0) diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll index 48d9fcb..71ef99f 100644 Binary files a/MultiWheelC/build/Clumsy/CommonUsage.dll and b/MultiWheelC/build/Clumsy/CommonUsage.dll differ diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.dll b/MultiWheelC/build/Clumsy/MultiWheelC.dll index dc9516c..ed0cd34 100644 Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.dll and b/MultiWheelC/build/Clumsy/MultiWheelC.dll differ diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.pdb b/MultiWheelC/build/Clumsy/MultiWheelC.pdb index f905320..8412bc4 100644 Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ diff --git a/Shared/Chassis/MultiWheelChassisAdapter.cs b/Shared/Chassis/MultiWheelChassisAdapter.cs index b4f3ab9..1c17d3b 100644 --- a/Shared/Chassis/MultiWheelChassisAdapter.cs +++ b/Shared/Chassis/MultiWheelChassisAdapter.cs @@ -12,7 +12,9 @@ namespace MyParking.Shared { #region 辅助内容 private const float BiasTolerance = 0.001f; + private const double MotionDeadband = 1e-6; private readonly MultiWheelChassis _chassis; + private double _activeMotionDirectionRadians; /// /// 当前适配器对应的车辆编号。 /// @@ -31,10 +33,22 @@ namespace MyParking.Shared /// /// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。 - /// 对称四舵轮底盘中，它也是蟹行虚拟阿克曼模型的半轴距。 + /// 对称四舵轮底盘中，它通常等于物理轮距的一半。 /// public double HalfTrackWidthMeters { get; } + /// + /// 获取旧版SendMotion使用的对称前后GCP半径，单位为m。 + /// + public double ControlPointRadiusMeters => + _chassis.ControlPointRadius / 1000.0; + + /// + /// 获取当前已经准备并激活的滚动运动系X轴在真实车体系中的方向，单位为rad。 + /// + public double ActiveMotionDirectionRadians => + _activeMotionDirectionRadians; + /// /// Width of the steering-alignment speed gate, in degrees. /// @@ -168,6 +182,7 @@ namespace MyParking.Shared /// /// 激活指定运动方向对应的SendMotion坐标系。 /// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。 + /// 调用方必须先停车，并确认舵轮已经按该方向完成预对齐。 /// public void ActivateMotionFrame( double motionDirectionRadians) @@ -176,10 +191,13 @@ namespace MyParking.Shared motionDirectionRadians, nameof(motionDirectionRadians)); + var normalizedDirectionRadians = + AngleMath.NormalizeRadians( + motionDirectionRadians); + var biasDegrees = ConvertRadiansToSingleDegrees( - -AngleMath.NormalizeRadians( - motionDirectionRadians), + -normalizedDirectionRadians, nameof(motionDirectionRadians)); var currentBias = _chassis.GetOriginBias(); @@ -194,6 +212,8 @@ namespace MyParking.Shared biasDegrees)) <= BiasTolerance) { + SetActiveMotionDirection( + normalizedDirectionRadians); return; } @@ -201,6 +221,19 @@ namespace MyParking.Shared x: 0.0f, y: 0.0f, th: biasDegrees); + SetActiveMotionDirection( + normalizedDirectionRadians); + } + + /// + /// 缓存当前运动坐标系方向，供控制周期内转换车体速度。 + /// + private void SetActiveMotionDirection( + double motionDirectionRadians) + { + _activeMotionDirectionRadians = + AngleMath.NormalizeRadians( + motionDirectionRadians); } public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId) { @@ -251,94 +284,146 @@ namespace MyParking.Shared if (MaximumWheelRadiusMeters <= 0.0 || HalfWheelBaseMeters <= 0.0 || - HalfTrackWidthMeters <= 0.0) + HalfTrackWidthMeters <= 0.0 || + ControlPointRadiusMeters <= 0.0) { throw new InvalidOperationException( - "Wheel positions cannot produce valid chassis dimensions."); + "Wheel positions and ControlPointRadius must produce valid chassis dimensions."); } + var initialBias = _chassis.GetOriginBias(); + SetActiveMotionDirection( + -AngleMath.DegreesToRadians( + initialBias.Z)); + // 通过反转轮速表达反向运动，避免蟹行正反切换时舵轮无意义地旋转180°。 _chassis.PreferMinimumSteeringTravel = true; } /// - /// 将车体坐标系速度命令发送给多舵轮底盘。 + /// 将车体坐标系刚体速度统一转换为滚动SendMotion、原地自转或停车命令。 + /// 非零平移命令使用调用方在运动段开始前已经准备并激活的β运动坐标系。 /// - public bool Send(ChassisCommand command, TimeSpan? interval = null) - { - if (command.VehicleId != VehicleId) - { - throw new InvalidOperationException( - $"命令车辆编号{command.VehicleId}与适配器车辆编号" + - $"{VehicleId}不一致。"); - } - ValidateTwist(command.BodyTwist); - // 防止其他旧逻辑再次调用DirectionAngle或 - // SetOriginBias改变底盘坐标语义。 - EnsureBodyFrameIsActive(); - var vxMetersPerSecond = - (float)command.BodyTwist.VxMetersPerSecond; - var vyMetersPerSecond = - (float)command.BodyTwist.VyMetersPerSecond; - var omegaDegreesPerSecond = - ConvertRadiansToSingleDegrees( - command.BodyTwist.OmegaRadiansPerSecond, - nameof(command.BodyTwist.OmegaRadiansPerSecond)); - var success = _chassis.SendXYThSpeed( - vxMetersPerSecond, - vyMetersPerSecond, - omegaDegreesPerSecond, - interval, - enableDifferentialSteerFeedforward: true); - if (!success) - { - // 防止分解失败后继续执行上一条运动命令。 - _chassis.PredefinedDriveStop(); - } - return success; - } - - - /// - /// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。 - /// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。 - /// - public bool SendVirtualAckermannMotion( - double motionDirectionRadians, - double speedMetersPerSecond, - double steeringRadians, + public bool SendBodyTwist( + Twist2D bodyTwist, TimeSpan? interval = null) { - NumericGuard.EnsureFinite( - motionDirectionRadians, - nameof(motionDirectionRadians)); - EnsureRepresentableAsSingle( - speedMetersPerSecond, - nameof(speedMetersPerSecond)); - NumericGuard.EnsureFinite( - steeringRadians, - nameof(steeringRadians)); - EnsureMotionFrameIsActive( - motionDirectionRadians); + ValidateTwist(bodyTwist); - if (Math.Abs(steeringRadians) >= - Math.PI / 2.0) + var linearSpeedMetersPerSecond = + Math.Sqrt( + bodyTwist.VxMetersPerSecond * + bodyTwist.VxMetersPerSecond + + bodyTwist.VyMetersPerSecond * + bodyTwist.VyMetersPerSecond); + + if (linearSpeedMetersPerSecond <= MotionDeadband) { - throw new ArgumentOutOfRangeException( - nameof(steeringRadians), - "虚拟阿克曼转向角必须位于正负90度以内。"); + if (Math.Abs( + bodyTwist.OmegaRadiansPerSecond) <= + MotionDeadband) + { + StopImmediately(); + return true; + } + + return SendPureRotation( + bodyTwist.OmegaRadiansPerSecond, + interval); } - var steeringDegrees = + return SendRollingTwistInActiveMotionFrame( + bodyTwist, + linearSpeedMetersPerSecond, + interval); + } + + /// + /// 将车体刚体速度转换到已准备的运动坐标系，并生成该坐标系中的前后GCP方向。 + /// + private bool SendRollingTwistInActiveMotionFrame( + Twist2D bodyTwist, + double linearSpeedMetersPerSecond, + TimeSpan? interval) + { + EnsureMotionFrameIsActive( + _activeMotionDirectionRadians); + + var bodyPoseInMotionFrame = + new Pose2D( + 0.0, + 0.0, + -_activeMotionDirectionRadians); + var motionTwist = + FrameTransform2D.TransformTwistAtSamePoint( + bodyPoseInMotionFrame, + bodyTwist); + var motionVxMetersPerSecond = + motionTwist.VxMetersPerSecond; + var motionVyMetersPerSecond = + motionTwist.VyMetersPerSecond; + + if (Math.Abs(motionVxMetersPerSecond) <= + MotionDeadband) + { + StopImmediately(); + throw new InvalidOperationException( + "当前车体速度几乎垂直于已经准备的运动坐标系，" + + "无法由方向型前后GCP稳定表示。请停车后按目标主运动方向重新准备并激活β。"); + } + + var travelDirection = + Math.Sign( + motionVxMetersPerSecond); + var signedCenterSpeedMetersPerSecond = + travelDirection * + linearSpeedMetersPerSecond; + var frontVelocityYMetersPerSecond = + motionVyMetersPerSecond + + bodyTwist.OmegaRadiansPerSecond * + ControlPointRadiusMeters; + var rearVelocityYMetersPerSecond = + motionVyMetersPerSecond - + bodyTwist.OmegaRadiansPerSecond * + ControlPointRadiusMeters; + var directedVxMetersPerSecond = + travelDirection * + motionVxMetersPerSecond; + var frontAngleRadians = + Math.Atan2( + travelDirection * + frontVelocityYMetersPerSecond, + directedVxMetersPerSecond); + var rearAngleRadians = + Math.Atan2( + travelDirection * + rearVelocityYMetersPerSecond, + directedVxMetersPerSecond); + + return SendGcpMotionInActiveFrame( + signedCenterSpeedMetersPerSecond, + frontAngleRadians, + rearAngleRadians, + interval); + } + + /// + /// 将已经完成自转舵轮准备的纯角速度命令交给XYTh底盘解算。 + /// + private bool SendPureRotation( + double omegaRadiansPerSecond, + TimeSpan? interval) + { + EnsureBodyFrameIsActive(); + + var success = _chassis.SendXYThSpeed( + 0.0f, + 0.0f, ConvertRadiansToSingleDegrees( - steeringRadians, - nameof(steeringRadians)); - var success = - _chassis.SendMotion( - (float)speedMetersPerSecond, - steeringDegrees, - -steeringDegrees, - interval); + omegaRadiansPerSecond, + nameof(omegaRadiansPerSecond)), + interval, + enableDifferentialSteerFeedforward: true); if (!success) { @@ -347,11 +432,10 @@ namespace MyParking.Shared return success; } - /// - /// 在真实车体坐标系中将有符号速度和独立前后GCP角度发送给旧版SendMotion。 + /// 在当前已激活的运动坐标系中将有符号速度和前后GCP角度发送给旧版SendMotion。 /// - public bool SendGcpMotion( + private bool SendGcpMotionInActiveFrame( double speedMetersPerSecond, double frontAngleRadians, double rearAngleRadians, @@ -366,7 +450,8 @@ namespace MyParking.Shared NumericGuard.EnsureFinite( rearAngleRadians, nameof(rearAngleRadians)); - EnsureBodyFrameIsActive(); + EnsureMotionFrameIsActive( + _activeMotionDirectionRadians); if (Math.Abs(frontAngleRadians) >= Math.PI / 2.0 || diff --git a/Shared/Models/ChassisCommand.cs b/Shared/Models/ChassisCommand.cs deleted file mode 100644 index 79671d5..0000000 --- a/Shared/Models/ChassisCommand.cs +++ /dev/null @@ -1,153 +0,0 @@ -// 纯数据层：只描述坐标、速度和命令 -// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。 -// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。 -// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。 -// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。 - -namespace MyParking.Shared -{ - /// - /// 二维坐标点，X、Y单位均为米。 - /// - public readonly struct Point2D - { - public Point2D(double xMeters, double yMeters) - { - XMeters = xMeters; - YMeters = yMeters; - } - - public double XMeters { get; } - - public double YMeters { get; } - - public static Point2D Zero => new Point2D(0.0, 0.0); - } - - /// - /// 二维局部坐标系在父坐标系中的位姿。 - /// 位置单位为米，朝向单位为弧度，逆时针为正。 - /// 具体父子关系由变量名称说明，例如RadarPoseInBody。 - /// - public readonly struct Pose2D - { - public Pose2D( - double xMeters, - double yMeters, - double yawRadians) - { - XMeters = xMeters; - YMeters = yMeters; - YawRadians = yawRadians; - } - - public double XMeters { get; } - - public double YMeters { get; } - - public double YawRadians { get; } - - public Point2D Position => - new Point2D(XMeters, YMeters); - - public static Pose2D Identity => - new Pose2D(0.0, 0.0, 0.0); - } - - /// - /// 二维刚体速度。 - /// 线速度单位为m/s，角速度单位为rad/s。 - /// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。 - /// - public readonly struct Twist2D - { - public Twist2D( - double vxMetersPerSecond, - double vyMetersPerSecond, - double omegaRadiansPerSecond) - { - VxMetersPerSecond = vxMetersPerSecond; - VyMetersPerSecond = vyMetersPerSecond; - OmegaRadiansPerSecond = omegaRadiansPerSecond; - } - - public double VxMetersPerSecond { get; } - - public double VyMetersPerSecond { get; } - - public double OmegaRadiansPerSecond { get; } - - public static Twist2D Zero => - new Twist2D(0.0, 0.0, 0.0); - } - - - /// - /// 单辆车的车体坐标系在车队坐标系中的位姿。 - /// - public readonly struct VehicleLayout - { - public VehicleLayout( - int vehicleId, - Pose2D poseInFleet) - { - VehicleId = vehicleId; - PoseInFleet = poseInFleet; - } - - public int VehicleId { get; } - - public Pose2D PoseInFleet { get; } - } - - /// - /// 车队整体运动命令，速度分量均在车队坐标系中表达。 - /// - public readonly struct FleetMotionCommand - { - public FleetMotionCommand( - Point2D referencePointInFleet, - Twist2D twistAtReferencePoint) - { - ReferencePointInFleet = referencePointInFleet; - TwistAtReferencePoint = twistAtReferencePoint; - } - - /// - /// 速度命令对应的参考点，也可作为自定义旋转中心。 - /// - public Point2D ReferencePointInFleet { get; } - - /// - /// 参考点处的车队速度。 - /// - public Twist2D TwistAtReferencePoint { get; } - - /// - /// 创建绕指定中心原地旋转的车队命令。 - /// - public static FleetMotionCommand RotateAround( - Point2D rotationCenterInFleet, - double omegaRadiansPerSecond) - { - return new FleetMotionCommand( - rotationCenterInFleet, - new Twist2D( - 0.0, - 0.0, - omegaRadiansPerSecond)); - } - - /// - /// 创建车队停止命令。 - /// - public static FleetMotionCommand Stop() - { - return new FleetMotionCommand( - Point2D.Zero, - Twist2D.Zero); - } - } - - -} \ No newline at end of file diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll index 48d9fcb..71ef99f 100644 Binary files a/output/C/CommonUsage.dll and b/output/C/CommonUsage.dll differ diff --git a/output/C/MultiWheelC.dll b/output/C/MultiWheelC.dll index dc9516c..ed0cd34 100644 Binary files a/output/C/MultiWheelC.dll and b/output/C/MultiWheelC.dll differ diff --git a/output/C/MultiWheelC.pdb b/output/C/MultiWheelC.pdb index f905320..8412bc4 100644 Binary files a/output/C/MultiWheelC.pdb and b/output/C/MultiWheelC.pdb differ diff --git a/output/M/CommonUsage.dll b/output/M/CommonUsage.dll index 48d9fcb..71ef99f 100644 Binary files a/output/M/CommonUsage.dll and b/output/M/CommonUsage.dll differ diff --git a/output/M/MedullaAdapter.dll b/output/M/MedullaAdapter.dll index c6e3d74..0602883 100644 Binary files a/output/M/MedullaAdapter.dll and b/output/M/MedullaAdapter.dll differ diff --git a/output/M/MedullaAdapter.pdb b/output/M/MedullaAdapter.pdb index 0409e29..517fda7 100644 Binary files a/output/M/MedullaAdapter.pdb and b/output/M/MedullaAdapter.pdb differ diff --git a/ref/CommonUsage.dll b/ref/CommonUsage.dll index 48d9fcb..71ef99f 100644 Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ