From 16b42c7c0d9836a2e37fd99148dd40a765355ed7 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Tue, 27 Feb 2024 02:45:28 -0500 Subject: [PATCH 01/16] add chassisspeeds --- src/wpimath/Kinematics/ChassisSpeeds.cs | 145 ++++++++++++++++++++++++ 1 file changed, 145 insertions(+) create mode 100644 src/wpimath/Kinematics/ChassisSpeeds.cs diff --git a/src/wpimath/Kinematics/ChassisSpeeds.cs b/src/wpimath/Kinematics/ChassisSpeeds.cs new file mode 100644 index 00000000..6006713f --- /dev/null +++ b/src/wpimath/Kinematics/ChassisSpeeds.cs @@ -0,0 +1,145 @@ +using System.Numerics; +using Google.Protobuf.Reflection; +using UnitsNet; +using WPIMath.Geometry; +using WPIMath.Proto; +using WPIUtil.Serialization.Protobuf; +using WPIUtil.Serialization.Struct; + +namespace WPIMath; + +public class ChassisSpeeds : IAdditionOperators, + ISubtractionOperators, + IMultiplyOperators, + IDivisionOperators, + IUnaryNegationOperators +{ + /// + /// Chassis velocity in the X-axis (Forward is positive). + /// + public Speed Vx { get; } = Speed.FromMetersPerSecond(0); + + /// + /// Chassis velocity in the Y-axis (Left is positive). + /// + public Speed Vy { get; } = Speed.FromMetersPerSecond(0); + + /// + /// Chassis Angular velocity (Z-axis or theta, Counter-Clockwise is positive). + /// + public RotationalSpeed Omega { get; } = RotationalSpeed.FromRadiansPerSecond(0); + + public static IProtobuf Proto { get; } = new ChassisSpeedsProto(); + public static IStruct Struct { get; } = new ChassisSpeedsStruct(); + + /// + /// Constructs a new ChassisSpeeds object with zero velocity on all axes. + /// + public ChassisSpeeds() {} + + /// + /// Constructs a new ChassisSpeeds object with the given velocities. + /// + /// Chassis velocity in the X-axis (Forward is positive). + /// Chassis velocity in the Y-axis (Left is positive). + /// Chassis Angular velocity (Z-axis or theta, Counter-Clockwise is positive). + public ChassisSpeeds(Speed Vx, Speed Vy, RotationalSpeed Omega) + { + this.Vx = Vx; + this.Vy = Vy; + this.Omega = Omega; + } + + public static ChassisSpeeds Discretize(Speed Vx, Speed Vy, RotationalSpeed Omega, TimeSpan Dt) + { + Pose2d desiredDeltaPose = new Pose2d(Vx * Dt, Vy * Dt, new Rotation2d(Omega * Dt)); + var twist = new Pose2d().Log(desiredDeltaPose); + return new(twist.Dx / Dt, twist.Dy / Dt, twist.Dtheta / Dt); + } + + public static ChassisSpeeds Discretize(ChassisSpeeds ContinuousSpeeds, TimeSpan Dt) => + Discretize(ContinuousSpeeds.Vx, ContinuousSpeeds.Vy, ContinuousSpeeds.Omega, Dt); + + public static ChassisSpeeds FromFieldRelativeSpeeds(Speed Vx, Speed Vy, RotationalSpeed Omega, Rotation2d RobotAngle) + { + var oneSecond = TimeSpan.FromSeconds(1); + // clockwise rotation into robot frame, so negate the angle + var chassisFrameVelocity = new Translation2d(Vx * oneSecond, Vy * oneSecond).RotateBy(-RobotAngle); + return new(chassisFrameVelocity.X / oneSecond, chassisFrameVelocity.Y / oneSecond, Omega); + } + + public static ChassisSpeeds FromFieldRelativeSpeeds(ChassisSpeeds FieldRelativeSpeeds, Rotation2d RobotAngle) => + FromFieldRelativeSpeeds(FieldRelativeSpeeds.Vx, FieldRelativeSpeeds.Vy, FieldRelativeSpeeds.Omega, RobotAngle); + + public static ChassisSpeeds FromRobotRelativeSpeeds(Speed Vx, Speed Vy, RotationalSpeed Omega, Rotation2d RobotAngle) + { + var oneSecond = TimeSpan.FromSeconds(1); + // counter-clockwise rotation out of robot frame + var chassisFrameVelocity = new Translation2d(Vx * oneSecond, Vy * oneSecond).RotateBy(RobotAngle); + return new(chassisFrameVelocity.X / oneSecond, chassisFrameVelocity.Y / oneSecond, Omega); + } + + public static ChassisSpeeds FromRobotRelativeSpeeds(ChassisSpeeds RobotRelativeSpeeds, Rotation2d RobotAngle) => + FromRobotRelativeSpeeds(RobotRelativeSpeeds.Vx, RobotRelativeSpeeds.Vy, RobotRelativeSpeeds.Omega, RobotAngle); + + public static ChassisSpeeds operator +(ChassisSpeeds left, ChassisSpeeds right) => + new(left.Vx + right.Vx, left.Vy + right.Vy, left.Omega + right.Omega); + + public static ChassisSpeeds operator -(ChassisSpeeds left, ChassisSpeeds right) => + new(left.Vx - right.Vx, left.Vy - right.Vy, left.Omega - right.Omega); + + public static ChassisSpeeds operator -(ChassisSpeeds value) => + new(-value.Vx, -value.Vy, -value.Omega); + + public static ChassisSpeeds operator *(ChassisSpeeds speed, double scalar) => + new(speed.Vx * scalar, speed.Vy * scalar, speed.Omega * scalar); + + public static ChassisSpeeds operator /(ChassisSpeeds speeds, double scalar) => + new(speeds.Vx / scalar, speeds.Vy / scalar, speeds.Omega / scalar); + + public override string ToString() => + $"ChassisSpeeds(Vx: {Vx:F2} {Vx.Unit}, Vy: {Vy:F2} {Vy.Unit}, Omega: {Omega:F2} {Omega.Unit})"; +} + +public class ChassisSpeedsProto : IProtobuf +{ + public MessageDescriptor Descriptor => ProtobufChassisSpeeds.Descriptor; + + public ProtobufChassisSpeeds CreateMessage() => new(); + + public void Pack(ProtobufChassisSpeeds msg, ChassisSpeeds value) + { + msg.Vx = value.Vx.MetersPerSecond; + msg.Vy = value.Vy.MetersPerSecond; + msg.Omega = value.Omega.RadiansPerSecond; + } + + public ChassisSpeeds Unpack(ProtobufChassisSpeeds msg) + { + return new(Speed.FromMetersPerSecond(msg.Vx), Speed.FromMetersPerSecond(msg.Vy), RotationalSpeed.FromRadiansPerSecond(msg.Omega)); + } +} + +public class ChassisSpeedsStruct : IStruct +{ + public string TypeString => "struct:ChassisSpeeds"; + + public int Size => sizeof(double) * 3; + + public string Schema => "double vx;double vy;double omega"; + + public void Pack(ref StructPacker buffer, ChassisSpeeds value) + { + buffer.WriteDouble(value.Vx.MetersPerSecond); + buffer.WriteDouble(value.Vy.MetersPerSecond); + buffer.WriteDouble(value.Omega.RadiansPerSecond); + } + + public ChassisSpeeds Unpack(ref StructUnpacker buffer) + { + var vx = Speed.FromMetersPerSecond(buffer.ReadDouble()); + var vy = Speed.FromMetersPerSecond(buffer.ReadDouble()); + var omega = RotationalSpeed.FromRadiansPerSecond(buffer.ReadDouble()); + return new(vx, vy, omega); + } +} From 7a91390d8304f2af1680e97c6f2f741af70eebd1 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Tue, 27 Feb 2024 04:18:52 -0500 Subject: [PATCH 02/16] add docs --- src/wpimath/Kinematics/ChassisSpeeds.cs | 77 +++++++++++++++++++++++++ 1 file changed, 77 insertions(+) diff --git a/src/wpimath/Kinematics/ChassisSpeeds.cs b/src/wpimath/Kinematics/ChassisSpeeds.cs index 6006713f..84e502e1 100644 --- a/src/wpimath/Kinematics/ChassisSpeeds.cs +++ b/src/wpimath/Kinematics/ChassisSpeeds.cs @@ -50,6 +50,25 @@ public ChassisSpeeds(Speed Vx, Speed Vy, RotationalSpeed Omega) this.Omega = Omega; } + /// + /// Discretizes a continuous-time chassis speed. + /// + /// + /// This function converts a continuous-time chassis speed into a discrete-time one such that + /// when the discrete-time chassis speed is applied for one timestep, the robot moves as if the + /// velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt + /// along the y-axis, and omega * dt around the z-axis). + /// + /// + /// This is useful for compensating for translational skew when translating and rotating a + /// swerve drivetrain. + /// + /// + /// Forward velocity. + /// Sideways velocity (left is positive). + /// Angular velocity (couter-clockwise is positive). + /// Timestep the speed should be applied for (this should be the same as the robot loop duration for most users). + /// Discretized ChassisSpeeds. public static ChassisSpeeds Discretize(Speed Vx, Speed Vy, RotationalSpeed Omega, TimeSpan Dt) { Pose2d desiredDeltaPose = new Pose2d(Vx * Dt, Vy * Dt, new Rotation2d(Omega * Dt)); @@ -57,9 +76,38 @@ public static ChassisSpeeds Discretize(Speed Vx, Speed Vy, RotationalSpeed Omega return new(twist.Dx / Dt, twist.Dy / Dt, twist.Dtheta / Dt); } + /// + /// Discretizes a continuous-time chassis speed. + /// + /// + /// This function converts a continuous-time chassis speed into a discrete-time one such that + /// when the discrete-time chassis speed is applied for one timestep, the robot moves as if the + /// velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt + /// along the y-axis, and omega * dt around the z-axis). + /// + /// + /// This is useful for compensating for translational skew when translating and rotating a + /// swerve drivetrain. + /// + /// + /// ChassisSpeeds to discretize. + /// Timestep the speed should be applied for (this should be the same as the robot loop duration for most users). + /// Discretized ChassisSpeeds. public static ChassisSpeeds Discretize(ChassisSpeeds ContinuousSpeeds, TimeSpan Dt) => Discretize(ContinuousSpeeds.Vx, ContinuousSpeeds.Vy, ContinuousSpeeds.Omega, Dt); + /// + /// Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds object. + /// + /// The component of speed in the x direction relative to the field. Positive x is away + /// from your alliance wall. + /// The component of speed in the y direction relative to the field. Positive y is to + /// your left when standing behind the alliance wall. + /// The angular velocity of the robot. + /// The angle of the robot as measured by a gyroscope. The robot's angle is + /// considered to be zero when it is facing directly away from your alliance station wall. + /// Remember that this should be CCW positive. + /// ChassisSpeeds object representing the speeds in the robot's frame of reference. public static ChassisSpeeds FromFieldRelativeSpeeds(Speed Vx, Speed Vy, RotationalSpeed Omega, Rotation2d RobotAngle) { var oneSecond = TimeSpan.FromSeconds(1); @@ -68,9 +116,29 @@ public static ChassisSpeeds FromFieldRelativeSpeeds(Speed Vx, Speed Vy, Rotation return new(chassisFrameVelocity.X / oneSecond, chassisFrameVelocity.Y / oneSecond, Omega); } + /// + /// Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds object. + /// + /// Field-relative ChassisSpeeds. + /// The angle of the robot as measured by a gyroscope. The robot's angle is + /// considered to be zero when it is facing directly away from your alliance station wall. + /// Remember that this should be CCW positive. + /// ChassisSpeeds object representing the speeds in the robot's frame of reference. public static ChassisSpeeds FromFieldRelativeSpeeds(ChassisSpeeds FieldRelativeSpeeds, Rotation2d RobotAngle) => FromFieldRelativeSpeeds(FieldRelativeSpeeds.Vx, FieldRelativeSpeeds.Vy, FieldRelativeSpeeds.Omega, RobotAngle); + /// + /// Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds object. + /// + /// The component of speed in the x direction relative to the robot. Positive x is towards the + /// robot's front. + /// The component of speed in the y direction relative to the robot. Positive y is towards the + /// robot's left. + /// The angular velocity of the robot. + /// The angle of the robot as measured by a gyroscope. The robot's angle is + /// considered to be zero when it is facing directly away from your alliance station wall. + /// Remember that this should be CCW positive. + /// ChassisSpeeds object representing the speeds in the field's frame of reference. public static ChassisSpeeds FromRobotRelativeSpeeds(Speed Vx, Speed Vy, RotationalSpeed Omega, Rotation2d RobotAngle) { var oneSecond = TimeSpan.FromSeconds(1); @@ -79,6 +147,15 @@ public static ChassisSpeeds FromRobotRelativeSpeeds(Speed Vx, Speed Vy, Rotation return new(chassisFrameVelocity.X / oneSecond, chassisFrameVelocity.Y / oneSecond, Omega); } + /// + /// Converts a user provided robot-relative ChassisSpeeds object into a field-relative ChassisSpeeds object. + /// + /// The ChassisSpeeds object representing the speeds in the robot frame + /// of reference. Positive x is towards the robot's front. Positive y is towards the robot's left. + /// The angle of the robot as measured by a gyroscope. The robot's angle is + /// considered to be zero when it is facing directly away from your alliance station wall. + /// Remember that this should be CCW positive. + /// ChassisSpeeds object representing the speeds in the field's frame of reference. public static ChassisSpeeds FromRobotRelativeSpeeds(ChassisSpeeds RobotRelativeSpeeds, Rotation2d RobotAngle) => FromRobotRelativeSpeeds(RobotRelativeSpeeds.Vx, RobotRelativeSpeeds.Vy, RobotRelativeSpeeds.Omega, RobotAngle); From 268fa85b4dec34ef8d62a68cf8d7762a9dac45bf Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Tue, 27 Feb 2024 05:14:44 -0500 Subject: [PATCH 03/16] start tests --- .../Kinematics/ChassisSpeedsTest.cs | 44 +++++++++++++++++++ test/wpimath.test/wpimath.test.csproj | 3 -- 2 files changed, 44 insertions(+), 3 deletions(-) create mode 100644 test/wpimath.test/Kinematics/ChassisSpeedsTest.cs diff --git a/test/wpimath.test/Kinematics/ChassisSpeedsTest.cs b/test/wpimath.test/Kinematics/ChassisSpeedsTest.cs new file mode 100644 index 00000000..17655032 --- /dev/null +++ b/test/wpimath.test/Kinematics/ChassisSpeedsTest.cs @@ -0,0 +1,44 @@ +using UnitsNet; +using UnitsNet.NumberExtensions.NumberToFrequency; +using UnitsNet.NumberExtensions.NumberToSpeed; +using WPIMath; +using Xunit; + +namespace wpimath.test; + +public class ChassisSpeedsTest +{ + // test: + // all operators + // robot relative + // field relative + // serialize + // deserialize + // tostring + + private static readonly double Epsilon = 1e-9; + + [Fact] + public void TestAddition() + { + // Given + ChassisSpeeds first = new(1.MetersPerSecond(), 2.MetersPerSecond(), RotationalSpeed.FromRadiansPerSecond(3)); + ChassisSpeeds second = new(4.MetersPerSecond(), 5.MetersPerSecond(), RotationalSpeed.FromRadiansPerSecond(6)); + // When + ChassisSpeeds sum = first + second; + // Then + Assert.Equal(first.Vx.MetersPerSecond + second.Vx.MetersPerSecond, sum.Vx.MetersPerSecond, Epsilon); + Assert.Equal(first.Vy.MetersPerSecond + second.Vy.MetersPerSecond, sum.Vy.MetersPerSecond, Epsilon); + Assert.Equal(first.Omega.RadiansPerSecond + second.Omega.RadiansPerSecond, sum.Omega.RadiansPerSecond, Epsilon); + } + + [Fact] + public void TestMinus() + { + // Given + + // When + + // Then + } +} diff --git a/test/wpimath.test/wpimath.test.csproj b/test/wpimath.test/wpimath.test.csproj index 4309094e..6c4078aa 100644 --- a/test/wpimath.test/wpimath.test.csproj +++ b/test/wpimath.test/wpimath.test.csproj @@ -5,7 +5,4 @@ - - - From 524023f860bb61e659f20b00bcd8460ba994abef Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Wed, 28 Feb 2024 01:54:26 -0500 Subject: [PATCH 04/16] Add ToString overrides for Pose2d and its components --- src/wpimath/Geometry/Pose2d.cs | 2 ++ src/wpimath/Geometry/Rotation2d.cs | 1 + src/wpimath/Geometry/Translation2d.cs | 2 ++ 3 files changed, 5 insertions(+) diff --git a/src/wpimath/Geometry/Pose2d.cs b/src/wpimath/Geometry/Pose2d.cs index 15d555c2..89000c7c 100644 --- a/src/wpimath/Geometry/Pose2d.cs +++ b/src/wpimath/Geometry/Pose2d.cs @@ -236,4 +236,6 @@ public override int GetHashCode() { return HashCode.Combine(Translation, Rotation); } + + public override string ToString() => $"Pose2d({Translation}, {Rotation})"; } diff --git a/src/wpimath/Geometry/Rotation2d.cs b/src/wpimath/Geometry/Rotation2d.cs index 847fc587..fda02828 100644 --- a/src/wpimath/Geometry/Rotation2d.cs +++ b/src/wpimath/Geometry/Rotation2d.cs @@ -176,4 +176,5 @@ public Rotation2d Interpolate(Rotation2d endValue, double t) return MathExtras.Lerp(this, endValue, t); } + public override string ToString() => $"Rotation2d(Angle: {Angle})"; } diff --git a/src/wpimath/Geometry/Translation2d.cs b/src/wpimath/Geometry/Translation2d.cs index aa707757..027bb5b0 100644 --- a/src/wpimath/Geometry/Translation2d.cs +++ b/src/wpimath/Geometry/Translation2d.cs @@ -179,4 +179,6 @@ public override int GetHashCode() { return HashCode.Combine(X, Y); } + + public override string ToString() => $"Translation2d(X: {X}, Y: {Y})"; } From 402daedb6cae57c81e3ae9143d5bfa3ae2752221 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Wed, 28 Feb 2024 01:55:30 -0500 Subject: [PATCH 05/16] add chassisspeed double constructor with defaulted units --- src/wpimath/Kinematics/ChassisSpeeds.cs | 35 ++++++++++++++++++------- 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/src/wpimath/Kinematics/ChassisSpeeds.cs b/src/wpimath/Kinematics/ChassisSpeeds.cs index 84e502e1..4a6c7750 100644 --- a/src/wpimath/Kinematics/ChassisSpeeds.cs +++ b/src/wpimath/Kinematics/ChassisSpeeds.cs @@ -1,6 +1,8 @@ -using System.Numerics; +using System.Numerics; using Google.Protobuf.Reflection; using UnitsNet; +using UnitsNet.NumberExtensions.NumberToRotationalSpeed; +using UnitsNet.NumberExtensions.NumberToSpeed; using WPIMath.Geometry; using WPIMath.Proto; using WPIUtil.Serialization.Protobuf; @@ -8,7 +10,7 @@ namespace WPIMath; -public class ChassisSpeeds : IAdditionOperators, +public class ChassisSpeeds : IAdditionOperators, ISubtractionOperators, IMultiplyOperators, IDivisionOperators, @@ -35,7 +37,7 @@ public class ChassisSpeeds : IAdditionOperators /// Constructs a new ChassisSpeeds object with zero velocity on all axes. /// - public ChassisSpeeds() {} + public ChassisSpeeds() { } /// /// Constructs a new ChassisSpeeds object with the given velocities. @@ -43,13 +45,26 @@ public ChassisSpeeds() {} /// Chassis velocity in the X-axis (Forward is positive). /// Chassis velocity in the Y-axis (Left is positive). /// Chassis Angular velocity (Z-axis or theta, Counter-Clockwise is positive). - public ChassisSpeeds(Speed Vx, Speed Vy, RotationalSpeed Omega) + public ChassisSpeeds(Speed Vx, Speed Vy, RotationalSpeed Omega) { this.Vx = Vx; this.Vy = Vy; this.Omega = Omega; } + /// + /// Constructs a new ChassisSpeeds object with the given velocities. + /// + /// Chassis velocity in the X-axis (Forward is positive) in meters per second. + /// Chassis velocity in the Y-axis (Left is positive) in meters per second. + /// Chassis Angular velocity (Z-axis or theta, Counter-Clockwise is positive) in radians per second. + public ChassisSpeeds(double vx, double vy, double omega) + { + Vx = vx.MetersPerSecond(); + Vy = vy.MetersPerSecond(); + Omega = omega.RadiansPerSecond(); + } + /// /// Discretizes a continuous-time chassis speed. /// @@ -77,7 +92,7 @@ public static ChassisSpeeds Discretize(Speed Vx, Speed Vy, RotationalSpeed Omega } /// - /// Discretizes a continuous-time chassis speed. + /// Discretizes a continuous-time chassis speed. /// /// /// This function converts a continuous-time chassis speed into a discrete-time one such that @@ -159,10 +174,10 @@ public static ChassisSpeeds FromRobotRelativeSpeeds(Speed Vx, Speed Vy, Rotation public static ChassisSpeeds FromRobotRelativeSpeeds(ChassisSpeeds RobotRelativeSpeeds, Rotation2d RobotAngle) => FromRobotRelativeSpeeds(RobotRelativeSpeeds.Vx, RobotRelativeSpeeds.Vy, RobotRelativeSpeeds.Omega, RobotAngle); - public static ChassisSpeeds operator +(ChassisSpeeds left, ChassisSpeeds right) => + public static ChassisSpeeds operator +(ChassisSpeeds left, ChassisSpeeds right) => new(left.Vx + right.Vx, left.Vy + right.Vy, left.Omega + right.Omega); - - public static ChassisSpeeds operator -(ChassisSpeeds left, ChassisSpeeds right) => + + public static ChassisSpeeds operator -(ChassisSpeeds left, ChassisSpeeds right) => new(left.Vx - right.Vx, left.Vy - right.Vy, left.Omega - right.Omega); public static ChassisSpeeds operator -(ChassisSpeeds value) => @@ -174,7 +189,7 @@ public static ChassisSpeeds FromRobotRelativeSpeeds(ChassisSpeeds RobotRelativeS public static ChassisSpeeds operator /(ChassisSpeeds speeds, double scalar) => new(speeds.Vx / scalar, speeds.Vy / scalar, speeds.Omega / scalar); - public override string ToString() => + public override string ToString() => $"ChassisSpeeds(Vx: {Vx:F2} {Vx.Unit}, Vy: {Vy:F2} {Vy.Unit}, Omega: {Omega:F2} {Omega.Unit})"; } @@ -184,7 +199,7 @@ public class ChassisSpeedsProto : IProtobuf new(); - public void Pack(ProtobufChassisSpeeds msg, ChassisSpeeds value) + public void Pack(ProtobufChassisSpeeds msg, ChassisSpeeds value) { msg.Vx = value.Vx.MetersPerSecond; msg.Vy = value.Vy.MetersPerSecond; From b1d3dd815eb4cfeab1be20ac21400a13b6f63c11 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Wed, 28 Feb 2024 01:56:36 -0500 Subject: [PATCH 06/16] complete chassisspeed tests (depends on pose2d fixes to pass) --- .../Kinematics/ChassisSpeedsTest.cs | 105 +++++++++++++++--- 1 file changed, 89 insertions(+), 16 deletions(-) diff --git a/test/wpimath.test/Kinematics/ChassisSpeedsTest.cs b/test/wpimath.test/Kinematics/ChassisSpeedsTest.cs index 17655032..7993a114 100644 --- a/test/wpimath.test/Kinematics/ChassisSpeedsTest.cs +++ b/test/wpimath.test/Kinematics/ChassisSpeedsTest.cs @@ -1,20 +1,13 @@ -using UnitsNet; -using UnitsNet.NumberExtensions.NumberToFrequency; +using UnitsNet; +using UnitsNet.NumberExtensions.NumberToAngle; using UnitsNet.NumberExtensions.NumberToSpeed; -using WPIMath; +using WPIMath.Geometry; using Xunit; -namespace wpimath.test; +namespace WPIMath.Test.Kinematics; public class ChassisSpeedsTest { - // test: - // all operators - // robot relative - // field relative - // serialize - // deserialize - // tostring private static readonly double Epsilon = 1e-9; @@ -27,18 +20,98 @@ public void TestAddition() // When ChassisSpeeds sum = first + second; // Then - Assert.Equal(first.Vx.MetersPerSecond + second.Vx.MetersPerSecond, sum.Vx.MetersPerSecond, Epsilon); - Assert.Equal(first.Vy.MetersPerSecond + second.Vy.MetersPerSecond, sum.Vy.MetersPerSecond, Epsilon); - Assert.Equal(first.Omega.RadiansPerSecond + second.Omega.RadiansPerSecond, sum.Omega.RadiansPerSecond, Epsilon); + Assert.Equal(5.0, sum.Vx.MetersPerSecond, Epsilon); + Assert.Equal(7.0, sum.Vy.MetersPerSecond, Epsilon); + Assert.Equal(9.0, sum.Omega.RadiansPerSecond, Epsilon); } [Fact] public void TestMinus() { // Given - + ChassisSpeeds first = new(1.MetersPerSecond(), 2.MetersPerSecond(), RotationalSpeed.FromRadiansPerSecond(3)); + ChassisSpeeds second = new(4.MetersPerSecond(), 5.MetersPerSecond(), RotationalSpeed.FromRadiansPerSecond(6)); + // When + ChassisSpeeds difference = first - second; + // Then + Assert.Equal(-3.0, difference.Vx.MetersPerSecond, Epsilon); + Assert.Equal(-3.0, difference.Vy.MetersPerSecond, Epsilon); + Assert.Equal(-3.0, difference.Omega.RadiansPerSecond, Epsilon); + } + + [Fact] + public void TestMultiply() + { + // Given + ChassisSpeeds speed = new(1.MetersPerSecond(), 2.MetersPerSecond(), RotationalSpeed.FromRadiansPerSecond(3)); + double scalar = 3.0; // When - + ChassisSpeeds product = speed * scalar; // Then + Assert.Equal(3.0, product.Vx.MetersPerSecond, Epsilon); + Assert.Equal(6.0, product.Vy.MetersPerSecond, Epsilon); + Assert.Equal(9.0, product.Omega.RadiansPerSecond, Epsilon); + } + + [Fact] + public void TestDivide() + { + // Given + ChassisSpeeds speed = new(1.MetersPerSecond(), 2.MetersPerSecond(), RotationalSpeed.FromRadiansPerSecond(3)); + double scalar = 5.0; + // When + ChassisSpeeds quotient = speed / scalar; + // Then + Assert.Equal(0.2, quotient.Vx.MetersPerSecond, Epsilon); + Assert.Equal(0.4, quotient.Vy.MetersPerSecond, Epsilon); + Assert.Equal(0.6, quotient.Omega.RadiansPerSecond, Epsilon); + } + + [Fact] + public void TestFromFieldRelative() + { + + ChassisSpeeds fieldSpeeds = new(1.0, 0.0, 0.5); + ChassisSpeeds robotSpeeds = ChassisSpeeds.FromFieldRelativeSpeeds(fieldSpeeds, new Rotation2d(-90.Degrees())); + + Assert.Equal(0.0, robotSpeeds.Vx.MetersPerSecond, Epsilon); + Assert.Equal(1.0, robotSpeeds.Vy.MetersPerSecond, Epsilon); + Assert.Equal(0.5, robotSpeeds.Omega.RadiansPerSecond, Epsilon); + } + + [Fact] + public void TestFromRobotRelative() + { + ChassisSpeeds robotSpeeds = new(1.0, 0.0, 0.5); + ChassisSpeeds fieldSpeeds = ChassisSpeeds.FromRobotRelativeSpeeds(robotSpeeds, new Rotation2d(45.Degrees())); + + Assert.Equal(1.0 / Math.Sqrt(2.0), fieldSpeeds.Vx.MetersPerSecond, Epsilon); + Assert.Equal(1.0 / Math.Sqrt(2.0), fieldSpeeds.Vy.MetersPerSecond, Epsilon); + Assert.Equal(0.5, fieldSpeeds.Omega.RadiansPerSecond, Epsilon); + } + + [Fact] + public void TestDiscretize() + { + ChassisSpeeds target = new(1, 0.0, 0.5); + TimeSpan duration = TimeSpan.FromSeconds(1); + TimeSpan dt = TimeSpan.FromSeconds(0.01); + + ChassisSpeeds speeds = ChassisSpeeds.Discretize(target, duration); + Twist2d twist = new( + speeds.Vx * dt, + speeds.Vy * dt, + speeds.Omega * dt + ); + + Pose2d pose = new(); + for (double time = 0; time < duration.TotalSeconds; time += dt.TotalSeconds) + { + pose = pose.Exp(twist); + } + + Assert.Equal((target.Vx * duration).Meters, pose.X.Meters, Epsilon); + Assert.Equal((target.Vy * duration).Meters, pose.Y.Meters, Epsilon); + Assert.Equal((target.Omega * duration).Radians, pose.Rotation.Angle.Radians, Epsilon); } } From 935800fd235dc5850b6ef951cda40a6b9ad42765 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Wed, 28 Feb 2024 01:58:28 -0500 Subject: [PATCH 07/16] Exp/Transform investigation tests --- test/wpimath.test/Geometry/Twist2dTest.cs | 49 +++++++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 test/wpimath.test/Geometry/Twist2dTest.cs diff --git a/test/wpimath.test/Geometry/Twist2dTest.cs b/test/wpimath.test/Geometry/Twist2dTest.cs new file mode 100644 index 00000000..b0e6364a --- /dev/null +++ b/test/wpimath.test/Geometry/Twist2dTest.cs @@ -0,0 +1,49 @@ +using UnitsNet.NumberExtensions.NumberToAngle; +using UnitsNet.NumberExtensions.NumberToLength; +using WPIMath.Geometry; +using Xunit; + +namespace WPIMath.Test.Geometry +{ + public class Twist2dTest + { + [Fact] + public void TestPose2dLog() + { + Pose2d start = new(); + Pose2d end = new(new Translation2d(5.Meters(), 5.Meters()), new Rotation2d(90.Degrees())); + + Twist2d twist = start.Log(end); + + Twist2d expected = new((5.0 / 2.0 * Math.PI).Meters(), 0.Meters(), (Math.PI / 2.0).Radians()); + + Assert.Equal(expected, twist); + + // ensure the twist gives us the real end pose + Pose2d applied = start.Exp(twist); + Assert.Equal(end, applied); + } + // TODO: Move tests to own classes to match with their relevant geometry types + [Fact] + public void TestPose2dTransformBy() + { + Pose2d start = new(); + + Transform2d transform = new(new Translation2d(5.Meters(), 5.Meters()), new Rotation2d(90.Degrees())); + + Pose2d end = new(new Translation2d(5.Meters(), 5.Meters()), new Rotation2d(90.Degrees())); + + Assert.Equal(end, start.TransformBy(transform)); + } + + [Fact] + public void TestTranslation2dRotateBy() + { + Translation2d start = new(5.Meters(), 0.Meters()); + Rotation2d rotation = new(90.Degrees()); + Translation2d end = start.RotateBy(rotation); + + Assert.Equal(new Translation2d(0.Meters(), 5.Meters()), end); + } + } +} From 8f52b8dd01e5c6200f191970e63b668f6c0dae2b Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Wed, 28 Feb 2024 16:25:38 -0500 Subject: [PATCH 08/16] chassisspeeds to struct --- src/wpimath/Kinematics/ChassisSpeeds.cs | 2 +- test/wpimath.test/wpimath.test.csproj | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/wpimath/Kinematics/ChassisSpeeds.cs b/src/wpimath/Kinematics/ChassisSpeeds.cs index 4a6c7750..3ff74925 100644 --- a/src/wpimath/Kinematics/ChassisSpeeds.cs +++ b/src/wpimath/Kinematics/ChassisSpeeds.cs @@ -10,7 +10,7 @@ namespace WPIMath; -public class ChassisSpeeds : IAdditionOperators, +public readonly struct ChassisSpeeds : IAdditionOperators, ISubtractionOperators, IMultiplyOperators, IDivisionOperators, diff --git a/test/wpimath.test/wpimath.test.csproj b/test/wpimath.test/wpimath.test.csproj index 6c4078aa..dbbbdd68 100644 --- a/test/wpimath.test/wpimath.test.csproj +++ b/test/wpimath.test/wpimath.test.csproj @@ -5,4 +5,8 @@ + + + + From ddd7bc199db4d27317c4d60e3b389d8c6c73309a Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Wed, 28 Feb 2024 17:07:40 -0500 Subject: [PATCH 09/16] pose init weirdness --- src/wpimath/Geometry/Pose2d.cs | 8 ++++++-- src/wpimath/Geometry/Rotation2d.cs | 7 +++++++ test/wpimath.test/Geometry/Rotation2dTest.cs | 9 +++++++++ test/wpimath.test/Geometry/Twist2dTest.cs | 10 ++++++++++ test/wpimath.test/wpimath.test.csproj | 4 ---- 5 files changed, 32 insertions(+), 6 deletions(-) diff --git a/src/wpimath/Geometry/Pose2d.cs b/src/wpimath/Geometry/Pose2d.cs index 89000c7c..58cb8f47 100644 --- a/src/wpimath/Geometry/Pose2d.cs +++ b/src/wpimath/Geometry/Pose2d.cs @@ -75,7 +75,7 @@ public partial class Pose2dJsonContext : JsonSerializerContext public Translation2d Translation { get; init; } [JsonInclude] [JsonPropertyName("rotation")] - public Rotation2d Rotation { get; init; } + public Rotation2d Rotation { get; init; } = new Rotation2d(); [JsonIgnore] public Length X => Translation.X; @@ -98,7 +98,11 @@ public Pose2d(Length x, Length y, Rotation2d rotation) public Pose2d TransformBy(Transform2d other) { - return new Pose2d(Translation + other.Translation.RotateBy(Rotation), other.Rotation + Rotation); + Translation2d rotated = other.Translation.RotateBy(Rotation); + Translation2d newTranslation = Translation + rotated; + Rotation2d newRotation = other.Rotation + Rotation; + Pose2d newPose = new Pose2d(newTranslation, newRotation); + return newPose; } public Pose2d RotateBy(Rotation2d other) diff --git a/src/wpimath/Geometry/Rotation2d.cs b/src/wpimath/Geometry/Rotation2d.cs index fda02828..724dd6ad 100644 --- a/src/wpimath/Geometry/Rotation2d.cs +++ b/src/wpimath/Geometry/Rotation2d.cs @@ -69,6 +69,13 @@ public partial class Rotation2dJsonContext : JsonSerializerContext public static IStruct Struct { get; } = new Rotation2dStruct(); public static IProtobuf Proto { get; } = new Rotation2dProto(); + public Rotation2d() + { + Angle = 0.Radians(); + Cos = 1; + Sin = 0; + } + public Rotation2d(Angle angle) { Angle = angle; diff --git a/test/wpimath.test/Geometry/Rotation2dTest.cs b/test/wpimath.test/Geometry/Rotation2dTest.cs index ecd8cbff..b59ca665 100644 --- a/test/wpimath.test/Geometry/Rotation2dTest.cs +++ b/test/wpimath.test/Geometry/Rotation2dTest.cs @@ -66,6 +66,15 @@ public void TestMinus() Assert.Equal(40.0, (rot1 - rot2).Angle.Degrees, Epsilon); } + [Fact] + public void TestPlus() + { + Rotation2d rot1 = 70.Degrees(); + Rotation2d rot2 = 30.Degrees(); + + Assert.Equal(100.0, (rot1 + rot2).Angle.Degrees, Epsilon); + } + [Fact] public void TestUrnaryMinus() { diff --git a/test/wpimath.test/Geometry/Twist2dTest.cs b/test/wpimath.test/Geometry/Twist2dTest.cs index b0e6364a..3c5bf8cb 100644 --- a/test/wpimath.test/Geometry/Twist2dTest.cs +++ b/test/wpimath.test/Geometry/Twist2dTest.cs @@ -45,5 +45,15 @@ public void TestTranslation2dRotateBy() Assert.Equal(new Translation2d(0.Meters(), 5.Meters()), end); } + + [Fact] + public void TestTranslation2dAddition() + { + Translation2d start = new(5.Meters(), 0.Meters()); + Translation2d offset = new(2.Meters(), 3.Meters()); + Translation2d end = new(7.Meters(), 3.Meters()); + + Assert.Equal(end, start + offset); + } } } diff --git a/test/wpimath.test/wpimath.test.csproj b/test/wpimath.test/wpimath.test.csproj index dbbbdd68..6c4078aa 100644 --- a/test/wpimath.test/wpimath.test.csproj +++ b/test/wpimath.test/wpimath.test.csproj @@ -5,8 +5,4 @@ - - - - From 1824e3f1a3946981ce848d8ad31ab197bee4fea2 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Wed, 28 Feb 2024 17:22:41 -0500 Subject: [PATCH 10/16] add pose2d default constructor --- src/wpimath/Geometry/Pose2d.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/wpimath/Geometry/Pose2d.cs b/src/wpimath/Geometry/Pose2d.cs index 58cb8f47..bda5ec00 100644 --- a/src/wpimath/Geometry/Pose2d.cs +++ b/src/wpimath/Geometry/Pose2d.cs @@ -82,6 +82,7 @@ public partial class Pose2dJsonContext : JsonSerializerContext [JsonIgnore] public Length Y => Translation.Y; + public Pose2d() { } [JsonConstructor] public Pose2d(Translation2d translation, Rotation2d rotation) From 5cff10c6b510d9629abf462aec54158ad54d5da1 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Wed, 28 Feb 2024 18:09:45 -0500 Subject: [PATCH 11/16] add equality and hashcode --- src/wpimath/Kinematics/ChassisSpeeds.cs | 28 ++++++++++++++++++- .../Kinematics/Proto/ChassisSpeedProtoTest.cs | 22 +++++++++++++++ .../Struct/ChassisSpeedsStructTest.cs | 22 +++++++++++++++ 3 files changed, 71 insertions(+), 1 deletion(-) create mode 100644 test/wpimath.test/Kinematics/Proto/ChassisSpeedProtoTest.cs create mode 100644 test/wpimath.test/Kinematics/Struct/ChassisSpeedsStructTest.cs diff --git a/src/wpimath/Kinematics/ChassisSpeeds.cs b/src/wpimath/Kinematics/ChassisSpeeds.cs index 3ff74925..aa4e5b4b 100644 --- a/src/wpimath/Kinematics/ChassisSpeeds.cs +++ b/src/wpimath/Kinematics/ChassisSpeeds.cs @@ -14,7 +14,8 @@ namespace WPIMath; ISubtractionOperators, IMultiplyOperators, IDivisionOperators, - IUnaryNegationOperators + IUnaryNegationOperators, + IEquatable { /// /// Chassis velocity in the X-axis (Forward is positive). @@ -191,6 +192,31 @@ public static ChassisSpeeds FromRobotRelativeSpeeds(ChassisSpeeds RobotRelativeS public override string ToString() => $"ChassisSpeeds(Vx: {Vx:F2} {Vx.Unit}, Vy: {Vy:F2} {Vy.Unit}, Omega: {Omega:F2} {Omega.Unit})"; + + public bool Equals(ChassisSpeeds other) + { + return + Math.Abs(Vx.MetersPerSecond - other.Vx.MetersPerSecond) < 1E-9 && + Math.Abs(Vy.MetersPerSecond - other.Vy.MetersPerSecond) < 1E-9 && + Math.Abs(Omega.RadiansPerSecond - other.Omega.RadiansPerSecond) < 1E-9; + } + + public override bool Equals(object? obj) + { + return obj is ChassisSpeeds speeds && Equals(speeds); + } + + public static bool operator ==(ChassisSpeeds left, ChassisSpeeds right) + { + return left.Equals(right); + } + + public static bool operator !=(ChassisSpeeds left, ChassisSpeeds right) + { + return !(left == right); + } + + public override int GetHashCode() => HashCode.Combine(Vx, Vy, Omega); } public class ChassisSpeedsProto : IProtobuf diff --git a/test/wpimath.test/Kinematics/Proto/ChassisSpeedProtoTest.cs b/test/wpimath.test/Kinematics/Proto/ChassisSpeedProtoTest.cs new file mode 100644 index 00000000..38153bb8 --- /dev/null +++ b/test/wpimath.test/Kinematics/Proto/ChassisSpeedProtoTest.cs @@ -0,0 +1,22 @@ +using WPIMath.Proto; +using Xunit; + +namespace WPIMath.Test.Kinematics.Proto +{ + public class ChassisSpeedsProtoTest + { + readonly ChassisSpeeds data = new(1.0, 2.0, 3.0); + + [Fact] + public void TestRoundtrip() + { + ProtobufChassisSpeeds proto = ChassisSpeeds.Proto.CreateMessage(); + ChassisSpeeds.Proto.Pack(proto, data); + ChassisSpeeds unpacked = ChassisSpeeds.Proto.Unpack(proto); + + Assert.Equal(data.Vx, unpacked.Vx); + Assert.Equal(data.Vy, unpacked.Vy); + Assert.Equal(data.Omega, unpacked.Omega); + } + } +} diff --git a/test/wpimath.test/Kinematics/Struct/ChassisSpeedsStructTest.cs b/test/wpimath.test/Kinematics/Struct/ChassisSpeedsStructTest.cs new file mode 100644 index 00000000..b66f4451 --- /dev/null +++ b/test/wpimath.test/Kinematics/Struct/ChassisSpeedsStructTest.cs @@ -0,0 +1,22 @@ +using WPIUtil.Serialization.Struct; +using Xunit; + +namespace WPIMath.Test.Kinematics.Struct +{ + public class ChassisSpeedsStructTest + { + readonly ChassisSpeeds data = new(1.0, 2.0, 3.0); + + [Fact] + public void TestRoundtrip() + { + StructPacker packer = new(new byte[ChassisSpeeds.Struct.Size]); + ChassisSpeeds.Struct.Pack(ref packer, data); + + StructUnpacker unpacker = new StructUnpacker(packer.Filled); + ChassisSpeeds unpackedData = ChassisSpeeds.Struct.Unpack(ref unpacker); + + Assert.Equal(data, unpackedData); + } + } +} From 6a43144524bde742ef27303ea67872b7d3c3b68e Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Wed, 28 Feb 2024 22:23:46 -0500 Subject: [PATCH 12/16] split tests into correct files --- test/wpimath.test/Geometry/Pose2dTest.cs | 22 +++++++++++++ .../Geometry/Translation2dTest.cs | 30 +++++++++++++++++ test/wpimath.test/Geometry/Twist2dTest.cs | 32 ------------------- 3 files changed, 52 insertions(+), 32 deletions(-) create mode 100644 test/wpimath.test/Geometry/Pose2dTest.cs create mode 100644 test/wpimath.test/Geometry/Translation2dTest.cs diff --git a/test/wpimath.test/Geometry/Pose2dTest.cs b/test/wpimath.test/Geometry/Pose2dTest.cs new file mode 100644 index 00000000..dfbe88e2 --- /dev/null +++ b/test/wpimath.test/Geometry/Pose2dTest.cs @@ -0,0 +1,22 @@ +using UnitsNet.NumberExtensions.NumberToAngle; +using UnitsNet.NumberExtensions.NumberToLength; +using WPIMath.Geometry; +using Xunit; + +namespace WPIMath.Test.Geometry +{ + public class Pose2dTest + { + [Fact] + public void TestPose2dTransformBy() + { + Pose2d start = new(); + + Transform2d transform = new(new Translation2d(5.Meters(), 5.Meters()), new Rotation2d(90.Degrees())); + + Pose2d end = new(new Translation2d(5.Meters(), 5.Meters()), new Rotation2d(90.Degrees())); + + Assert.Equal(end, start.TransformBy(transform)); + } + } +} diff --git a/test/wpimath.test/Geometry/Translation2dTest.cs b/test/wpimath.test/Geometry/Translation2dTest.cs new file mode 100644 index 00000000..963a80a2 --- /dev/null +++ b/test/wpimath.test/Geometry/Translation2dTest.cs @@ -0,0 +1,30 @@ +using UnitsNet.NumberExtensions.NumberToAngle; +using UnitsNet.NumberExtensions.NumberToLength; +using WPIMath.Geometry; +using Xunit; + +namespace WPIMath.Test.Geometry +{ + public class Translation2dTest + { + [Fact] + public void TestTranslation2dRotateBy() + { + Translation2d start = new(5.Meters(), 0.Meters()); + Rotation2d rotation = new(90.Degrees()); + var end = start.RotateBy(rotation); + + Assert.Equal(new Translation2d(0.Meters(), 5.Meters()), end); + } + + [Fact] + public void TestTranslation2dAddition() + { + Translation2d start = new(5.Meters(), 0.Meters()); + Translation2d offset = new(2.Meters(), 3.Meters()); + Translation2d end = new(7.Meters(), 3.Meters()); + + Assert.Equal(end, start + offset); + } + } +} diff --git a/test/wpimath.test/Geometry/Twist2dTest.cs b/test/wpimath.test/Geometry/Twist2dTest.cs index 3c5bf8cb..7f046b4b 100644 --- a/test/wpimath.test/Geometry/Twist2dTest.cs +++ b/test/wpimath.test/Geometry/Twist2dTest.cs @@ -23,37 +23,5 @@ public void TestPose2dLog() Pose2d applied = start.Exp(twist); Assert.Equal(end, applied); } - // TODO: Move tests to own classes to match with their relevant geometry types - [Fact] - public void TestPose2dTransformBy() - { - Pose2d start = new(); - - Transform2d transform = new(new Translation2d(5.Meters(), 5.Meters()), new Rotation2d(90.Degrees())); - - Pose2d end = new(new Translation2d(5.Meters(), 5.Meters()), new Rotation2d(90.Degrees())); - - Assert.Equal(end, start.TransformBy(transform)); - } - - [Fact] - public void TestTranslation2dRotateBy() - { - Translation2d start = new(5.Meters(), 0.Meters()); - Rotation2d rotation = new(90.Degrees()); - Translation2d end = start.RotateBy(rotation); - - Assert.Equal(new Translation2d(0.Meters(), 5.Meters()), end); - } - - [Fact] - public void TestTranslation2dAddition() - { - Translation2d start = new(5.Meters(), 0.Meters()); - Translation2d offset = new(2.Meters(), 3.Meters()); - Translation2d end = new(7.Meters(), 3.Meters()); - - Assert.Equal(end, start + offset); - } } } From 04cb598e131c1daf7849677568cc560e90902a41 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Sat, 2 Mar 2024 20:40:51 -0500 Subject: [PATCH 13/16] final default ctor fix --- src/wpimath/Geometry/Pose2d.cs | 2 +- src/wpimath/Geometry/Rotation2d.cs | 11 ++--------- 2 files changed, 3 insertions(+), 10 deletions(-) diff --git a/src/wpimath/Geometry/Pose2d.cs b/src/wpimath/Geometry/Pose2d.cs index 7541d06e..d08c9cec 100644 --- a/src/wpimath/Geometry/Pose2d.cs +++ b/src/wpimath/Geometry/Pose2d.cs @@ -75,7 +75,7 @@ public partial class Pose2dJsonContext : JsonSerializerContext public Translation2d Translation { get; init; } [JsonInclude] [JsonPropertyName("rotation")] - public Rotation2d Rotation { get; init; } = new Rotation2d(); + public Rotation2d Rotation { get; init; } [JsonIgnore] public Length X => Translation.X; diff --git a/src/wpimath/Geometry/Rotation2d.cs b/src/wpimath/Geometry/Rotation2d.cs index 0cf52129..102a4924 100644 --- a/src/wpimath/Geometry/Rotation2d.cs +++ b/src/wpimath/Geometry/Rotation2d.cs @@ -69,11 +69,9 @@ public partial class Rotation2dJsonContext : JsonSerializerContext public static IStruct Struct { get; } = new Rotation2dStruct(); public static IProtobuf Proto { get; } = new Rotation2dProto(); - public Rotation2d() + public Rotation2d() : this(0.Radians()) { - Angle = 0.Radians(); - Cos = 1; - Sin = 0; + } public Rotation2d(Angle angle) @@ -107,11 +105,6 @@ internal Rotation2d(double radians) : this(radians.Radians()) [JsonPropertyName("radians")] internal double Radians => Angle.Radians; - public Rotation2d() : this(0.Radians()) - { - - } - [JsonIgnore] public Angle Angle { get; } [JsonIgnore] From 1aeddf389a24d3e54207d500417d2ba6f7b6d9ed Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Mon, 4 Mar 2024 04:06:26 -0500 Subject: [PATCH 14/16] remove property initializers --- src/wpimath/Kinematics/ChassisSpeeds.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/wpimath/Kinematics/ChassisSpeeds.cs b/src/wpimath/Kinematics/ChassisSpeeds.cs index aa4e5b4b..a5e5159e 100644 --- a/src/wpimath/Kinematics/ChassisSpeeds.cs +++ b/src/wpimath/Kinematics/ChassisSpeeds.cs @@ -20,17 +20,17 @@ namespace WPIMath; /// /// Chassis velocity in the X-axis (Forward is positive). /// - public Speed Vx { get; } = Speed.FromMetersPerSecond(0); + public Speed Vx { get; } /// /// Chassis velocity in the Y-axis (Left is positive). /// - public Speed Vy { get; } = Speed.FromMetersPerSecond(0); + public Speed Vy { get; } /// /// Chassis Angular velocity (Z-axis or theta, Counter-Clockwise is positive). /// - public RotationalSpeed Omega { get; } = RotationalSpeed.FromRadiansPerSecond(0); + public RotationalSpeed Omega { get; } public static IProtobuf Proto { get; } = new ChassisSpeedsProto(); public static IStruct Struct { get; } = new ChassisSpeedsStruct(); From a61946febd3e30153cf7fd192d60d569bebbcc76 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Tue, 5 Mar 2024 02:10:16 -0500 Subject: [PATCH 15/16] fix ctors in chassisspeed tests --- test/wpimath.test/Kinematics/Proto/ChassisSpeedProtoTest.cs | 3 ++- test/wpimath.test/Kinematics/Struct/ChassisSpeedsStructTest.cs | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/test/wpimath.test/Kinematics/Proto/ChassisSpeedProtoTest.cs b/test/wpimath.test/Kinematics/Proto/ChassisSpeedProtoTest.cs index 38153bb8..bc6f9fac 100644 --- a/test/wpimath.test/Kinematics/Proto/ChassisSpeedProtoTest.cs +++ b/test/wpimath.test/Kinematics/Proto/ChassisSpeedProtoTest.cs @@ -1,3 +1,4 @@ +using UnitsNet; using WPIMath.Proto; using Xunit; @@ -5,7 +6,7 @@ namespace WPIMath.Test.Kinematics.Proto { public class ChassisSpeedsProtoTest { - readonly ChassisSpeeds data = new(1.0, 2.0, 3.0); + readonly ChassisSpeeds data = new(Speed.FromMetersPerSecond(1.0), Speed.FromMetersPerSecond(2.0), RotationalSpeed.FromRadiansPerSecond(3.0)); [Fact] public void TestRoundtrip() diff --git a/test/wpimath.test/Kinematics/Struct/ChassisSpeedsStructTest.cs b/test/wpimath.test/Kinematics/Struct/ChassisSpeedsStructTest.cs index b66f4451..25871d86 100644 --- a/test/wpimath.test/Kinematics/Struct/ChassisSpeedsStructTest.cs +++ b/test/wpimath.test/Kinematics/Struct/ChassisSpeedsStructTest.cs @@ -1,3 +1,4 @@ +using UnitsNet; using WPIUtil.Serialization.Struct; using Xunit; @@ -5,7 +6,7 @@ namespace WPIMath.Test.Kinematics.Struct { public class ChassisSpeedsStructTest { - readonly ChassisSpeeds data = new(1.0, 2.0, 3.0); + readonly ChassisSpeeds data = new(Speed.FromMetersPerSecond(1.0), Speed.FromMetersPerSecond(2.0), RotationalSpeed.FromRadiansPerSecond(3.0)); [Fact] public void TestRoundtrip() From dd1451a7d5c606fbc10e1d4bac417371506402a9 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Sun, 10 Mar 2024 23:08:54 -0500 Subject: [PATCH 16/16] make parameters lowercase --- src/wpimath/Kinematics/ChassisSpeeds.cs | 80 ++++++++++++------------- 1 file changed, 40 insertions(+), 40 deletions(-) diff --git a/src/wpimath/Kinematics/ChassisSpeeds.cs b/src/wpimath/Kinematics/ChassisSpeeds.cs index a5e5159e..8350d7c2 100644 --- a/src/wpimath/Kinematics/ChassisSpeeds.cs +++ b/src/wpimath/Kinematics/ChassisSpeeds.cs @@ -43,14 +43,14 @@ public ChassisSpeeds() { } /// /// Constructs a new ChassisSpeeds object with the given velocities. /// - /// Chassis velocity in the X-axis (Forward is positive). - /// Chassis velocity in the Y-axis (Left is positive). - /// Chassis Angular velocity (Z-axis or theta, Counter-Clockwise is positive). - public ChassisSpeeds(Speed Vx, Speed Vy, RotationalSpeed Omega) + /// Chassis velocity in the X-axis (Forward is positive). + /// Chassis velocity in the Y-axis (Left is positive). + /// Chassis Angular velocity (Z-axis or theta, Counter-Clockwise is positive). + public ChassisSpeeds(Speed vx, Speed vy, RotationalSpeed omega) { - this.Vx = Vx; - this.Vy = Vy; - this.Omega = Omega; + Vx = vx; + Vy = vy; + Omega = omega; } /// @@ -80,16 +80,16 @@ public ChassisSpeeds(double vx, double vy, double omega) /// swerve drivetrain. /// /// - /// Forward velocity. - /// Sideways velocity (left is positive). - /// Angular velocity (couter-clockwise is positive). - /// Timestep the speed should be applied for (this should be the same as the robot loop duration for most users). + /// Forward velocity. + /// Sideways velocity (left is positive). + /// Angular velocity (couter-clockwise is positive). + /// Timestep the speed should be applied for (this should be the same as the robot loop duration for most users). /// Discretized ChassisSpeeds. - public static ChassisSpeeds Discretize(Speed Vx, Speed Vy, RotationalSpeed Omega, TimeSpan Dt) + public static ChassisSpeeds Discretize(Speed vx, Speed vy, RotationalSpeed omega, TimeSpan dt) { - Pose2d desiredDeltaPose = new Pose2d(Vx * Dt, Vy * Dt, new Rotation2d(Omega * Dt)); + Pose2d desiredDeltaPose = new Pose2d(vx * dt, vy * dt, new Rotation2d(omega * dt)); var twist = new Pose2d().Log(desiredDeltaPose); - return new(twist.Dx / Dt, twist.Dy / Dt, twist.Dtheta / Dt); + return new(twist.Dx / dt, twist.Dy / dt, twist.Dtheta / dt); } /// @@ -106,74 +106,74 @@ public static ChassisSpeeds Discretize(Speed Vx, Speed Vy, RotationalSpeed Omega /// swerve drivetrain. /// /// - /// ChassisSpeeds to discretize. - /// Timestep the speed should be applied for (this should be the same as the robot loop duration for most users). + /// ChassisSpeeds to discretize. + /// Timestep the speed should be applied for (this should be the same as the robot loop duration for most users). /// Discretized ChassisSpeeds. - public static ChassisSpeeds Discretize(ChassisSpeeds ContinuousSpeeds, TimeSpan Dt) => - Discretize(ContinuousSpeeds.Vx, ContinuousSpeeds.Vy, ContinuousSpeeds.Omega, Dt); + public static ChassisSpeeds Discretize(ChassisSpeeds continuousSpeeds, TimeSpan dt) => + Discretize(continuousSpeeds.Vx, continuousSpeeds.Vy, continuousSpeeds.Omega, dt); /// /// Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds object. /// - /// The component of speed in the x direction relative to the field. Positive x is away + /// The component of speed in the x direction relative to the field. Positive x is away /// from your alliance wall. - /// The component of speed in the y direction relative to the field. Positive y is to + /// The component of speed in the y direction relative to the field. Positive y is to /// your left when standing behind the alliance wall. - /// The angular velocity of the robot. - /// The angle of the robot as measured by a gyroscope. The robot's angle is + /// The angular velocity of the robot. + /// The angle of the robot as measured by a gyroscope. The robot's angle is /// considered to be zero when it is facing directly away from your alliance station wall. /// Remember that this should be CCW positive. /// ChassisSpeeds object representing the speeds in the robot's frame of reference. - public static ChassisSpeeds FromFieldRelativeSpeeds(Speed Vx, Speed Vy, RotationalSpeed Omega, Rotation2d RobotAngle) + public static ChassisSpeeds FromFieldRelativeSpeeds(Speed vx, Speed vy, RotationalSpeed omega, Rotation2d robotAngle) { var oneSecond = TimeSpan.FromSeconds(1); // clockwise rotation into robot frame, so negate the angle - var chassisFrameVelocity = new Translation2d(Vx * oneSecond, Vy * oneSecond).RotateBy(-RobotAngle); - return new(chassisFrameVelocity.X / oneSecond, chassisFrameVelocity.Y / oneSecond, Omega); + var chassisFrameVelocity = new Translation2d(vx * oneSecond, vy * oneSecond).RotateBy(-robotAngle); + return new(chassisFrameVelocity.X / oneSecond, chassisFrameVelocity.Y / oneSecond, omega); } /// /// Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds object. /// - /// Field-relative ChassisSpeeds. - /// The angle of the robot as measured by a gyroscope. The robot's angle is + /// Field-relative ChassisSpeeds. + /// The angle of the robot as measured by a gyroscope. The robot's angle is /// considered to be zero when it is facing directly away from your alliance station wall. /// Remember that this should be CCW positive. /// ChassisSpeeds object representing the speeds in the robot's frame of reference. - public static ChassisSpeeds FromFieldRelativeSpeeds(ChassisSpeeds FieldRelativeSpeeds, Rotation2d RobotAngle) => - FromFieldRelativeSpeeds(FieldRelativeSpeeds.Vx, FieldRelativeSpeeds.Vy, FieldRelativeSpeeds.Omega, RobotAngle); + public static ChassisSpeeds FromFieldRelativeSpeeds(ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) => + FromFieldRelativeSpeeds(fieldRelativeSpeeds.Vx, fieldRelativeSpeeds.Vy, fieldRelativeSpeeds.Omega, robotAngle); /// /// Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds object. /// - /// The component of speed in the x direction relative to the robot. Positive x is towards the + /// The component of speed in the x direction relative to the robot. Positive x is towards the /// robot's front. - /// The component of speed in the y direction relative to the robot. Positive y is towards the + /// The component of speed in the y direction relative to the robot. Positive y is towards the /// robot's left. - /// The angular velocity of the robot. - /// The angle of the robot as measured by a gyroscope. The robot's angle is + /// The angular velocity of the robot. + /// The angle of the robot as measured by a gyroscope. The robot's angle is /// considered to be zero when it is facing directly away from your alliance station wall. /// Remember that this should be CCW positive. /// ChassisSpeeds object representing the speeds in the field's frame of reference. - public static ChassisSpeeds FromRobotRelativeSpeeds(Speed Vx, Speed Vy, RotationalSpeed Omega, Rotation2d RobotAngle) + public static ChassisSpeeds FromRobotRelativeSpeeds(Speed vx, Speed vy, RotationalSpeed omega, Rotation2d robotAngle) { var oneSecond = TimeSpan.FromSeconds(1); // counter-clockwise rotation out of robot frame - var chassisFrameVelocity = new Translation2d(Vx * oneSecond, Vy * oneSecond).RotateBy(RobotAngle); - return new(chassisFrameVelocity.X / oneSecond, chassisFrameVelocity.Y / oneSecond, Omega); + var chassisFrameVelocity = new Translation2d(vx * oneSecond, vy * oneSecond).RotateBy(robotAngle); + return new(chassisFrameVelocity.X / oneSecond, chassisFrameVelocity.Y / oneSecond, omega); } /// /// Converts a user provided robot-relative ChassisSpeeds object into a field-relative ChassisSpeeds object. /// - /// The ChassisSpeeds object representing the speeds in the robot frame + /// The ChassisSpeeds object representing the speeds in the robot frame /// of reference. Positive x is towards the robot's front. Positive y is towards the robot's left. - /// The angle of the robot as measured by a gyroscope. The robot's angle is + /// The angle of the robot as measured by a gyroscope. The robot's angle is /// considered to be zero when it is facing directly away from your alliance station wall. /// Remember that this should be CCW positive. /// ChassisSpeeds object representing the speeds in the field's frame of reference. - public static ChassisSpeeds FromRobotRelativeSpeeds(ChassisSpeeds RobotRelativeSpeeds, Rotation2d RobotAngle) => - FromRobotRelativeSpeeds(RobotRelativeSpeeds.Vx, RobotRelativeSpeeds.Vy, RobotRelativeSpeeds.Omega, RobotAngle); + public static ChassisSpeeds FromRobotRelativeSpeeds(ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) => + FromRobotRelativeSpeeds(robotRelativeSpeeds.Vx, robotRelativeSpeeds.Vy, robotRelativeSpeeds.Omega, robotAngle); public static ChassisSpeeds operator +(ChassisSpeeds left, ChassisSpeeds right) => new(left.Vx + right.Vx, left.Vy + right.Vy, left.Omega + right.Omega);