v2.4.0

road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/paths/BezierCurves.kt

@@ -1,3 +1,4 @@
+@file:JvmName("BezierCurves")
 package com.acmerobotics.roadrunner.paths
 
 import com.acmerobotics.roadrunner.geometry.DualNum
@@ -56,7 +57,14 @@ data class BezierCurve1d(
         }
 
         fun fromRRSpline(spline: QuinticSpline1d): BezierCurve1d =
-            BezierCurve1d(spline.coefficients)
+            BezierCurve1d(doubleArrayOf(
+                spline.f,
+                spline.f + 0.2 * spline.e,
+                spline.f + 0.4 * spline.e + 0.1 * spline.d,
+                spline.f + 0.6 * spline.e + 0.3 * spline.d + 0.1 * spline.c,
+                spline.f + 0.8 * spline.e + 0.6 * spline.d + 0.4 * spline.c + 0.2 * spline.b,
+                spline.coefficients.sum()
+                ))
     }
 }
 
@@ -81,9 +89,9 @@ class BezierCurve2dInternal(val x: BezierCurve1d, val y: BezierCurve1d) : Positi
     }
 }
 
-fun generateCurveFromPoints(vararg points: Vector2d) = generateCurveFromPoints(points.toList())
+fun fromPoints(vararg points: Vector2d) = fromPoints(points.toList())
 
-fun generateCurveFromPoints(points: List<Vector2d>) = ArclengthReparamCurve2d(
+fun fromPoints(points: List<Vector2d>) = ArclengthReparamCurve2d(
     BezierCurve2dInternal.fromPoints(points),
     1e-6
 )

road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/paths/PosePaths.kt

@@ -30,6 +30,8 @@ interface PosePath {
     }
 
     fun map(map: PoseMap) = MappedPosePath(this, map)
+
+    operator fun plus(other: PosePath) = CompositePosePath(listOf(this, other))
 }
 
 data class TangentPath(
@@ -64,7 +66,7 @@ data class HeadingPosePath(
     override fun length() = posPath.length()
 }
 
-data class CompositePosePath(
+data class CompositePosePath @JvmOverloads constructor(
     @JvmField
     val paths: List<PosePath>,
     @JvmField
@@ -76,8 +78,6 @@ data class CompositePosePath(
         }
     }
 
-    constructor(paths: List<CompositePosePath>) : this(paths.flatMap { it.paths })
-
     @JvmField
     val length = offsets.last()
 
@@ -96,6 +96,11 @@ data class CompositePosePath(
     }
 
     override fun length() = length
+
+    override fun plus(other: PosePath) = when (other) {
+        is CompositePosePath -> CompositePosePath(this.paths + other.paths)
+        else -> CompositePosePath(paths + other)
+    }
 }
 
 class OffsetPosePath(

road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/paths/PositionPaths.kt

@@ -6,6 +6,8 @@ import com.acmerobotics.roadrunner.geometry.Arclength
 import com.acmerobotics.roadrunner.geometry.DualNum
 import com.acmerobotics.roadrunner.geometry.IntegralScanResult
 import com.acmerobotics.roadrunner.geometry.Internal
+import com.acmerobotics.roadrunner.geometry.Rotation2d
+import com.acmerobotics.roadrunner.geometry.Rotation2dDual
 import com.acmerobotics.roadrunner.geometry.Vector2d
 import com.acmerobotics.roadrunner.geometry.Vector2dDual
 import com.acmerobotics.roadrunner.geometry.clamp
@@ -38,8 +40,56 @@ interface PositionPath<Param> {
         val ds = (query - guess.value()) dot guess.drop(1).value()
         clamp(s + ds, 0.0, this.length())
     }
+
+    /**
+     * Creates a PosePath using this path for position and its tangent for heading.
+     */
+    fun withTangentHeading() = TangentPath(this.wrtArclength(), 0.0)
+
+    /**
+     * Creates a PosePath using this path for position
+     * and a linear interpolation between [startHeading] and [endHeading] for heading.
+     */
+    fun withLinearHeading(startHeading: Rotation2d, endHeading: Rotation2d) = this.wrtArclength().let {
+        HeadingPosePath(
+            it,
+            LinearHeadingPath(startHeading, endHeading - startHeading, it.length())
+            )
+    }
+
+    /**
+     * Creates a PosePath using this path for position
+     * and a linear interpolation between [startHeading] and [endHeading] for heading.
+     */
+    fun withLinearHeading(startHeading: Double, endHeading: Double) =
+        withLinearHeading(startHeading.toRotation(), endHeading.toRotation())
+
+    /**
+     * Creates a PosePath using this path for position
+     * and a linear interpolation between [startHeading] and [endHeading] for heading.
+     */
+    fun withSplineHeading(startHeading: Rotation2d, endHeading: Rotation2d) = this.wrtArclength().let {
+        HeadingPosePath(
+            it,
+            SplineHeadingPath(
+                startHeading.dual(),
+                (endHeading-startHeading).toRotation().dual(),
+                it.length()
+            )
+        )
+    }
+
+    /**
+     * Creates a PosePath using this path for position
+     * and a linear interpolation between [startHeading] and [endHeading] for heading.
+     */
+    fun withSplineHeading(startHeading: Double, endHeading: Double) =
+        withSplineHeading(startHeading.toRotation(), endHeading.toRotation())
 }
 
+fun Double.toRotation() = Rotation2d.exp(this)
+fun Rotation2d.dual() = Rotation2dDual.Companion.constant<Arclength>(this, 1)
+
 /**
  * Project position [query] onto position path [path] starting with initial guess [init].
  */
@@ -136,6 +186,12 @@ data class ArclengthReparamCurve2d(
     override fun length() = length
 }
 
+private fun <Param> PositionPath<Param>.wrtArclength(): PositionPath<Arclength> = when(this) {
+    is Line -> this
+    is ArclengthReparamCurve2d -> this
+    else -> ArclengthReparamCurve2d(this, 1e-6)
+}
+
 data class CompositePositionPath<Param> @JvmOverloads constructor(
     @JvmField
     val paths: List<PositionPath<Param>>,