测试后可正常执行

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);
         }
 
         /// <summary>
@@ -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