Merge branch 'master' of https://github.com/5190GreenHopeRobotics/Fal…

…conLibrary

src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/control/FeedForwardTracker.kt

@@ -2,7 +2,9 @@ package org.ghrobotics.lib.mathematics.twodim.control
 
 import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2d
 import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2dWithCurvature
-import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedIterator
+import org.ghrobotics.lib.mathematics.twodim.trajectory.TrajectoryIterator
+import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedEntry
+import org.ghrobotics.lib.mathematics.units.Time
 
 /**
  * Follows a path purely based on linear and angular velocities from the path without any external
@@ -11,7 +13,7 @@ import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedIterator
 class FeedForwardTracker : TrajectoryTracker() {
 
     override fun calculateState(
-        iterator: TimedIterator<Pose2dWithCurvature>,
+        iterator: TrajectoryIterator<Time, TimedEntry<Pose2dWithCurvature>>,
         robotPose: Pose2d
     ): TrajectoryTrackerVelocityOutput {
         val referenceState = iterator.currentState.state

src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/control/PurePursuitTracker.kt

@@ -3,7 +3,8 @@ package org.ghrobotics.lib.mathematics.twodim.control
 import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2d
 import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2dWithCurvature
 import org.ghrobotics.lib.mathematics.twodim.geometry.Translation2d
-import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedIterator
+import org.ghrobotics.lib.mathematics.twodim.trajectory.TrajectoryIterator
+import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedEntry
 import org.ghrobotics.lib.mathematics.units.Length
 import org.ghrobotics.lib.mathematics.units.Time
 import org.ghrobotics.lib.mathematics.units.meter
@@ -29,7 +30,7 @@ class PurePursuitTracker(
      * Calculate desired chassis velocity using pure pursuit.
      */
     override fun calculateState(
-        iterator: TimedIterator<Pose2dWithCurvature>,
+        iterator: TrajectoryIterator<Time, TimedEntry<Pose2dWithCurvature>>,
         robotPose: Pose2d
     ): TrajectoryTrackerVelocityOutput {
         val referencePoint = iterator.currentState
@@ -64,7 +65,7 @@ class PurePursuitTracker(
 
 
     private fun calculateLookaheadPose2d(
-        iterator: TimedIterator<Pose2dWithCurvature>,
+        iterator: TrajectoryIterator<Time, TimedEntry<Pose2dWithCurvature>>,
         robotPose: Pose2d
     ): Pose2d {
         val lookaheadPoseByTime = iterator.preview(kLookaheadTime).state.state.pose
@@ -97,7 +98,7 @@ class PurePursuitTracker(
 
         return lookaheadPoseByDistance.transformBy(
             Pose2d(
-                Translation2d(remaining * if (iterator.reversed) -1 else 1, 0.0)
+                Translation2d(remaining * if (iterator.trajectory.reversed) -1 else 1, 0.0)
             )
         )
     }

src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/control/RamseteTracker.kt

@@ -3,7 +3,9 @@ package org.ghrobotics.lib.mathematics.twodim.control
 import org.ghrobotics.lib.mathematics.epsilonEquals
 import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2d
 import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2dWithCurvature
-import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedIterator
+import org.ghrobotics.lib.mathematics.twodim.trajectory.TrajectoryIterator
+import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedEntry
+import org.ghrobotics.lib.mathematics.units.Time
 import kotlin.math.sin
 import kotlin.math.sqrt
 
@@ -23,7 +25,7 @@ class RamseteTracker(
      * Calculate desired chassis velocity using Ramsete.
      */
     override fun calculateState(
-        iterator: TimedIterator<Pose2dWithCurvature>,
+        iterator: TrajectoryIterator<Time, TimedEntry<Pose2dWithCurvature>>,
         robotPose: Pose2d
     ): TrajectoryTrackerVelocityOutput {
         val referenceState = iterator.currentState.state

src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/control/TrajectoryTracker.kt

@@ -2,8 +2,9 @@ package org.ghrobotics.lib.mathematics.twodim.control
 
 import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2d
 import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2dWithCurvature
-import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedIterator
-import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedTrajectory
+import org.ghrobotics.lib.mathematics.twodim.trajectory.TrajectoryIterator
+import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedEntry
+import org.ghrobotics.lib.mathematics.twodim.trajectory.types.Trajectory
 import org.ghrobotics.lib.mathematics.units.Time
 import org.ghrobotics.lib.mathematics.units.derivedunits.AngularVelocity
 import org.ghrobotics.lib.mathematics.units.derivedunits.LinearVelocity
@@ -16,14 +17,14 @@ import org.ghrobotics.lib.utils.DeltaTime
  */
 abstract class TrajectoryTracker {
 
-    private var trajectoryIterator: TimedIterator<Pose2dWithCurvature>? = null
+    private var trajectoryIterator: TrajectoryIterator<Time, TimedEntry<Pose2dWithCurvature>>? = null
     private var deltaTimeController = DeltaTime()
     private var previousVelocity: TrajectoryTrackerVelocityOutput? = null
 
     val referencePoint get() = trajectoryIterator?.currentState
     val isFinished get() = trajectoryIterator?.isDone ?: true
 
-    fun reset(trajectory: TimedTrajectory<Pose2dWithCurvature>) {
+    fun reset(trajectory: Trajectory<Time, TimedEntry<Pose2dWithCurvature>>) {
         trajectoryIterator = trajectory.iterator()
         deltaTimeController.reset()
         previousVelocity = null
@@ -63,7 +64,7 @@ abstract class TrajectoryTracker {
     }
 
     protected abstract fun calculateState(
-        iterator: TimedIterator<Pose2dWithCurvature>,
+        iterator: TrajectoryIterator<Time, TimedEntry<Pose2dWithCurvature>>,
         robotPose: Pose2d
     ): TrajectoryTrackerVelocityOutput
 

src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/trajectory/types/DistanceTrajectory.kt

@@ -7,8 +7,8 @@ import org.ghrobotics.lib.mathematics.units.meter
 import org.ghrobotics.lib.types.VaryInterpolatable
 
 class DistanceTrajectory<S : VaryInterpolatable<S>>(
-    points: List<S>
-) : Trajectory<Length, S>(points) {
+    override val points: List<S>
+) : Trajectory<Length, S> {
 
     private val distances: List<Double>
 

src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/trajectory/types/IndexedTrajectory.kt

@@ -4,8 +4,8 @@ import org.ghrobotics.lib.mathematics.twodim.trajectory.TrajectoryIterator
 import org.ghrobotics.lib.types.VaryInterpolatable
 
 class IndexedTrajectory<S : VaryInterpolatable<S>>(
-    points: List<S>
-) : Trajectory<Double, S>(points) {
+    override val points: List<S>
+) : Trajectory<Double, S> {
 
     override fun sample(interpolant: Double) = when {
         points.isEmpty() -> throw IndexOutOfBoundsException("Trajectory is empty!")

src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/trajectory/types/TimedTrajectory.kt

@@ -6,16 +6,18 @@ import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2d
 import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2dWithCurvature
 import org.ghrobotics.lib.mathematics.twodim.trajectory.TrajectoryIterator
 import org.ghrobotics.lib.mathematics.units.Time
+import org.ghrobotics.lib.mathematics.units.derivedunits.LinearAcceleration
+import org.ghrobotics.lib.mathematics.units.derivedunits.LinearVelocity
 import org.ghrobotics.lib.mathematics.units.derivedunits.acceleration
 import org.ghrobotics.lib.mathematics.units.derivedunits.velocity
 import org.ghrobotics.lib.mathematics.units.meter
 import org.ghrobotics.lib.mathematics.units.second
 import org.ghrobotics.lib.types.VaryInterpolatable
 
 class TimedTrajectory<S : VaryInterpolatable<S>>(
-    points: List<TimedEntry<S>>,
-    val reversed: Boolean
-) : Trajectory<Time, TimedEntry<S>>(points) {
+    override val points: List<TimedEntry<S>>,
+    override val reversed: Boolean
+) : Trajectory<Time, TimedEntry<S>> {
 
     override fun sample(interpolant: Time) = sample(interpolant.value)
 
@@ -63,6 +65,13 @@ data class TimedEntry<S : VaryInterpolatable<S>> internal constructor(
     val velocity get() = _velocity.meter.velocity
     val acceleration get() = _acceleration.meter.acceleration
 
+    constructor(
+        state: S,
+        t: Time,
+        velocity: LinearVelocity,
+        acceleration: LinearAcceleration
+    ) : this(state, t.value, velocity.value, acceleration.value)
+
     override fun interpolate(endValue: TimedEntry<S>, t: Double): TimedEntry<S> {
         val newT = _t.lerp(endValue._t, t)
         val deltaT = newT - this.t.value
@@ -87,14 +96,13 @@ data class TimedEntry<S : VaryInterpolatable<S>> internal constructor(
 class TimedIterator<S : VaryInterpolatable<S>>(
     trajectory: TimedTrajectory<S>
 ) : TrajectoryIterator<Time, TimedEntry<S>>(trajectory) {
-    val reversed = trajectory.reversed
     override fun addition(a: Time, b: Time) = a + b
 }
 
-fun TimedTrajectory<Pose2dWithCurvature>.mirror() =
+fun Trajectory<Time, TimedEntry<Pose2dWithCurvature>>.mirror() =
     TimedTrajectory(points.map { TimedEntry(it.state.mirror, it._t, it._velocity, it._acceleration) }, this.reversed)
 
-fun TimedTrajectory<Pose2dWithCurvature>.transform(transform: Pose2d) =
+fun Trajectory<Time, TimedEntry<Pose2dWithCurvature>>.transform(transform: Pose2d) =
     TimedTrajectory(
         points.map { TimedEntry(it.state + transform, it._t, it._velocity, it._acceleration) },
         this.reversed

src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/trajectory/types/Trajectory.kt

@@ -3,20 +3,22 @@ package org.ghrobotics.lib.mathematics.twodim.trajectory.types
 import org.ghrobotics.lib.mathematics.twodim.trajectory.TrajectoryIterator
 import org.ghrobotics.lib.types.VaryInterpolatable
 
-abstract class Trajectory<U : Comparable<U>, S : VaryInterpolatable<S>>(
+interface Trajectory<U : Comparable<U>, S : VaryInterpolatable<S>> {
     val points: List<S>
-) {
+    val reversed get() = false
+
+    @JvmDefault
     fun getPoint(index: Int) = TrajectoryPoint(index, points[index])
 
-    abstract fun sample(interpolant: U): TrajectorySamplePoint<S>
+    fun sample(interpolant: U): TrajectorySamplePoint<S>
 
-    abstract val firstInterpolant: U
-    abstract val lastInterpolant: U
+    val firstInterpolant: U
+    val lastInterpolant: U
 
-    abstract val firstState: S
-    abstract val lastState: S
+    val firstState: S
+    val lastState: S
 
-    abstract operator fun iterator(): TrajectoryIterator<U, S>
+    operator fun iterator(): TrajectoryIterator<U, S>
 }
 
 data class TrajectoryPoint<S>(

src/main/kotlin/org/ghrobotics/lib/subsystems/drive/TankDriveSubsystem.kt

@@ -10,7 +10,8 @@ import org.ghrobotics.lib.localization.Localization
 import org.ghrobotics.lib.mathematics.kEpsilon
 import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2dWithCurvature
 import org.ghrobotics.lib.mathematics.twodim.geometry.Rectangle2d
-import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedTrajectory
+import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedEntry
+import org.ghrobotics.lib.mathematics.twodim.trajectory.types.Trajectory
 import org.ghrobotics.lib.mathematics.twodim.trajectory.types.mirror
 import org.ghrobotics.lib.mathematics.units.Length
 import org.ghrobotics.lib.mathematics.units.Time
@@ -184,7 +185,7 @@ abstract class TankDriveSubsystem : FalconSubsystem("Drive Subsystem"), Differen
      * @param trajectory The trajectory to follow
      */
     fun followTrajectory(
-        trajectory: TimedTrajectory<Pose2dWithCurvature>,
+        trajectory: Trajectory<Time, TimedEntry<Pose2dWithCurvature>>,
         dt: Time = 20.millisecond
     ) = TrajectoryTrackerCommand(this, this, { trajectory }, dt = dt)
 
@@ -195,7 +196,7 @@ abstract class TankDriveSubsystem : FalconSubsystem("Drive Subsystem"), Differen
      * @param pathMirrored Whether to mirror the path or not
      */
     fun followTrajectory(
-        trajectory: TimedTrajectory<Pose2dWithCurvature>,
+        trajectory: Trajectory<Time, TimedEntry<Pose2dWithCurvature>>,
         pathMirrored: Boolean = false,
         dt: Time = 20.millisecond
     ) = followTrajectory(trajectory.let {
@@ -209,7 +210,7 @@ abstract class TankDriveSubsystem : FalconSubsystem("Drive Subsystem"), Differen
      * @param pathMirrored Whether to mirror the path or not
      */
     fun followTrajectory(
-        trajectory: Source<TimedTrajectory<Pose2dWithCurvature>>,
+        trajectory: Source<Trajectory<Time, TimedEntry<Pose2dWithCurvature>>>,
         pathMirrored: Boolean = false,
         dt: Time = 20.millisecond
     ) = TrajectoryTrackerCommand(this, this, trajectory.map {
@@ -223,7 +224,7 @@ abstract class TankDriveSubsystem : FalconSubsystem("Drive Subsystem"), Differen
      * @param pathMirrored Source with whether to mirror the path or not
      */
     fun followTrajectory(
-        trajectory: TimedTrajectory<Pose2dWithCurvature>,
+        trajectory: Trajectory<Time, TimedEntry<Pose2dWithCurvature>>,
         pathMirrored: BooleanSource,
         dt: Time = 20.millisecond
     ) = followTrajectory(pathMirrored.map(trajectory.mirror(), trajectory), dt = dt)
@@ -235,7 +236,7 @@ abstract class TankDriveSubsystem : FalconSubsystem("Drive Subsystem"), Differen
      * @param pathMirrored Source with whether to mirror the path or not
      */
     fun followTrajectory(
-        trajectory: Source<TimedTrajectory<Pose2dWithCurvature>>,
+        trajectory: Source<Trajectory<Time, TimedEntry<Pose2dWithCurvature>>>,
         pathMirrored: BooleanSource,
         dt: Time = 20.millisecond
     ) = followTrajectory(pathMirrored.map(trajectory.map { it.mirror() }, trajectory), dt = dt)

src/main/kotlin/org/ghrobotics/lib/subsystems/drive/TrajectoryTrackerCommand.kt

@@ -1,12 +1,12 @@
 package org.ghrobotics.lib.subsystems.drive
 
-import edu.wpi.first.wpilibj.Notifier
 import org.ghrobotics.lib.commands.FalconCommand
 import org.ghrobotics.lib.commands.FalconSubsystem
 import org.ghrobotics.lib.debug.LiveDashboard
 import org.ghrobotics.lib.mathematics.twodim.control.TrajectoryTracker
 import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2dWithCurvature
-import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedTrajectory
+import org.ghrobotics.lib.mathematics.twodim.trajectory.types.TimedEntry
+import org.ghrobotics.lib.mathematics.twodim.trajectory.types.Trajectory
 import org.ghrobotics.lib.mathematics.units.Time
 import org.ghrobotics.lib.mathematics.units.millisecond
 import org.ghrobotics.lib.utils.Source
@@ -20,7 +20,7 @@ import org.ghrobotics.lib.utils.Source
 class TrajectoryTrackerCommand(
     driveSubsystem: FalconSubsystem,
     private val driveBase: TrajectoryTrackerDriveBase,
-    val trajectorySource: Source<TimedTrajectory<Pose2dWithCurvature>>,
+    val trajectorySource: Source<Trajectory<Time, TimedEntry<Pose2dWithCurvature>>>,
     private val trajectoryTracker: TrajectoryTracker = driveBase.trajectoryTracker,
     val dt: Time = 20.millisecond
 ) : FalconCommand(driveSubsystem) {