Add ChassisSpeeds

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;
@@ -102,7 +102,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)
@@ -240,4 +244,6 @@ public override int GetHashCode()
     {
         return HashCode.Combine(Translation, Rotation);
     }
+
+    public override string ToString() => $"Pose2d({Translation}, {Rotation})";
 }

src/wpimath/Geometry/Rotation2d.cs

@@ -69,6 +69,13 @@ public partial class Rotation2dJsonContext : JsonSerializerContext
     public static IStruct<Rotation2d> Struct { get; } = new Rotation2dStruct();
     public static IProtobuf<Rotation2d, ProtobufRotation2d> Proto { get; } = new Rotation2dProto();
 
+    public Rotation2d()
+    {
+        Angle = 0.Radians();
+        Cos = 1;
+        Sin = 0;
+    }
+
     public Rotation2d(Angle angle)
     {
         Angle = angle;
@@ -181,4 +188,5 @@ public Rotation2d Interpolate(Rotation2d endValue, double t)
         return MathExtras.Lerp(this, endValue, t);
     }
 
+    public override string ToString() => $"Rotation2d(Angle: {Angle})";
 }

src/wpimath/Geometry/Translation2d.cs

@@ -183,4 +183,6 @@ public override int GetHashCode()
     {
         return HashCode.Combine(X, Y);
     }
+
+    public override string ToString() => $"Translation2d(X: {X}, Y: {Y})";
 }

src/wpimath/Kinematics/ChassisSpeeds.cs

@@ -0,0 +1,263 @@
+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;
+using WPIUtil.Serialization.Struct;
+
+namespace WPIMath;
+
+public readonly struct ChassisSpeeds : IAdditionOperators<ChassisSpeeds, ChassisSpeeds, ChassisSpeeds>,
+                                ISubtractionOperators<ChassisSpeeds, ChassisSpeeds, ChassisSpeeds>,
+                                IMultiplyOperators<ChassisSpeeds, double, ChassisSpeeds>,
+                                IDivisionOperators<ChassisSpeeds, double, ChassisSpeeds>,
+                                IUnaryNegationOperators<ChassisSpeeds, ChassisSpeeds>,
+                                IEquatable<ChassisSpeeds>
+{
+    /// <summary>
+    /// Chassis velocity in the X-axis (Forward is positive).
+    /// </summary>
+    public Speed Vx { get; } = Speed.FromMetersPerSecond(0);
+
+    /// <summary>
+    /// Chassis velocity in the Y-axis (Left is positive).
+    /// </summary>
+    public Speed Vy { get; } = Speed.FromMetersPerSecond(0);
+
+    /// <summary>
+    /// Chassis Angular velocity (Z-axis or theta, Counter-Clockwise is positive).
+    /// </summary>
+    public RotationalSpeed Omega { get; } = RotationalSpeed.FromRadiansPerSecond(0);
+
+    public static IProtobuf<ChassisSpeeds, ProtobufChassisSpeeds> Proto { get; } = new ChassisSpeedsProto();
+    public static IStruct<ChassisSpeeds> Struct { get; } = new ChassisSpeedsStruct();
+
+    /// <summary>
+    /// Constructs a new ChassisSpeeds object with zero velocity on all axes.
+    /// </summary>
+    public ChassisSpeeds() { }
+
+    /// <summary>
+    /// Constructs a new ChassisSpeeds object with the given velocities.
+    /// </summary>
+    /// <param name="Vx">Chassis velocity in the X-axis (Forward is positive).</param>
+    /// <param name="Vy">Chassis velocity in the Y-axis (Left is positive).</param>
+    /// <param name="Omega">Chassis Angular velocity (Z-axis or theta, Counter-Clockwise is positive).</param>
+    public ChassisSpeeds(Speed Vx, Speed Vy, RotationalSpeed Omega)
+    {
+        this.Vx = Vx;
+        this.Vy = Vy;
+        this.Omega = Omega;
+    }
+
+    /// <summary>
+    /// Constructs a new ChassisSpeeds object with the given velocities.
+    /// </summary>
+    /// <param name="vx">Chassis velocity in the X-axis (Forward is positive) in meters per second.</param>
+    /// <param name="vy">Chassis velocity in the Y-axis (Left is positive) in meters per second.</param>
+    /// <param name="omega">Chassis Angular velocity (Z-axis or theta, Counter-Clockwise is positive) in radians per second.</param>
+    public ChassisSpeeds(double vx, double vy, double omega)
+    {
+        Vx = vx.MetersPerSecond();
+        Vy = vy.MetersPerSecond();
+        Omega = omega.RadiansPerSecond();
+    }
+
+    /// <summary>
+    /// Discretizes a continuous-time chassis speed.
+    /// 
+    /// <para>
+    /// 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).
+    /// </para>
+    /// <para>
+    /// This is useful for compensating for translational skew when translating and rotating a
+    /// swerve drivetrain.
+    /// </para>
+    /// </summary>
+    /// <param name="Vx">Forward velocity.</param>
+    /// <param name="Vy">Sideways velocity (left is positive).</param>
+    /// <param name="Omega">Angular velocity (couter-clockwise is positive).</param>
+    /// <param name="Dt">Timestep the speed should be applied for (this should be the same as the robot loop duration for most users).</param>
+    /// <returns>Discretized ChassisSpeeds.</returns>
+    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);
+    }
+
+    /// <summary>
+    /// Discretizes a  continuous-time chassis speed.
+    /// 
+    /// <para>
+    /// 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).
+    /// </para>
+    /// <para>
+    /// This is useful for compensating for translational skew when translating and rotating a
+    /// swerve drivetrain.
+    /// </para>
+    /// </summary>
+    /// <param name="ContinuousSpeeds">ChassisSpeeds to discretize.</param>
+    /// <param name="Dt">Timestep the speed should be applied for (this should be the same as the robot loop duration for most users).</param>
+    /// <returns>Discretized ChassisSpeeds.</returns>
+    public static ChassisSpeeds Discretize(ChassisSpeeds ContinuousSpeeds, TimeSpan Dt) =>
+        Discretize(ContinuousSpeeds.Vx, ContinuousSpeeds.Vy, ContinuousSpeeds.Omega, Dt);
+
+    /// <summary>
+    /// Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds object.
+    /// </summary>
+    /// <param name="Vx">The component of speed in the x direction relative to the field. Positive x is away 
+    /// from your alliance wall.</param>
+    /// <param name="Vy">The component of speed in the y direction relative to the field. Positive y is to 
+    /// your left when standing behind the alliance wall.</param>
+    /// <param name="Omega">The angular velocity of the robot.</param>
+    /// <param name="RobotAngle">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.</param>
+    /// <returns>ChassisSpeeds object representing the speeds in the robot's frame of reference.</returns>
+    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);
+    }
+
+    /// <summary>
+    /// Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds object.
+    /// </summary>
+    /// <param name="FieldRelativeSpeeds">Field-relative ChassisSpeeds.</param>
+    /// <param name="RobotAngle">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.</param>
+    /// <returns>ChassisSpeeds object representing the speeds in the robot's frame of reference.</returns>
+    public static ChassisSpeeds FromFieldRelativeSpeeds(ChassisSpeeds FieldRelativeSpeeds, Rotation2d RobotAngle) =>
+        FromFieldRelativeSpeeds(FieldRelativeSpeeds.Vx, FieldRelativeSpeeds.Vy, FieldRelativeSpeeds.Omega, RobotAngle);
+
+    /// <summary>
+    /// Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds object.
+    /// </summary>
+    /// <param name="Vx">The component of speed in the x direction relative to the robot. Positive x is towards the 
+    /// robot's front.</param>
+    /// <param name="Vy">The component of speed in the y direction relative to the robot. Positive y is towards the 
+    /// robot's left.</param>
+    /// <param name="Omega">The angular velocity of the robot.</param>
+    /// <param name="RobotAngle">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.</param>
+    /// <returns>ChassisSpeeds object representing the speeds in the field's frame of reference.</returns>
+    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);
+    }
+
+    /// <summary>
+    /// Converts a user provided robot-relative ChassisSpeeds object into a field-relative ChassisSpeeds object.
+    /// </summary>
+    /// <param name="RobotRelativeSpeeds"> 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.</param>
+    /// <param name="RobotAngle">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.</param>
+    /// <returns>ChassisSpeeds object representing the speeds in the field's frame of reference.</returns>
+    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 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<ChassisSpeeds, ProtobufChassisSpeeds>
+{
+    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<ChassisSpeeds>
+{
+    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);
+    }
+}

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));
+        }
+    }
+}

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()
     {

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);
+        }
+    }
+}