Skip to content

Commit

Permalink
Reduce weirdness due to large t
Browse files Browse the repository at this point in the history
  • Loading branch information
@penguinencounter
penguinencounter committed Oct 31, 2024
1 parent e6a4032 commit 65b3e69
Show file tree
Hide file tree
Showing 2 changed files with 70 additions and 19 deletions.
49 changes: 30 additions & 19 deletions ...ode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/kinematics/CubicSplineSolver.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,20 +19,21 @@ private inline fun DoubleArray.mapInPlace(transform: (Double) -> Double) {
}
}

data class CubicSpline(
data class CubicSpline @JvmOverloads constructor(
val a: Double,
val b: Double,
val c: Double,
val d: Double,
val lowX: Double,
val highX: Double,
val offset: Double = 0.0
) {
companion object {
@JvmStatic
fun fromArrayAndBounds(arr: DoubleArray, lowX: Double, highX: Double): CubicSpline {
fun fromArrayAndBounds(arr: DoubleArray, lowX: Double, highX: Double, offset: Double): CubicSpline {
assert(arr.size == 4) { "Wrong size to import cubic spline from array : ${arr.size}" }
return CubicSpline(
arr[0], arr[1], arr[2], arr[3], lowX, highX
arr[0], arr[1], arr[2], arr[3], lowX, highX, offset
)
}
}
Expand All @@ -55,9 +56,10 @@ data class CubicSpline(
)
}

data class CubicSplinePair(val x: CubicSpline, val y: CubicSpline) {
data class PointData(val x: Double, val y: Double, val totalDistance: Double, val t: Double) {
override fun toString(): String = "(%.4f, %.4f <t:%.4f | d:%.4f>)".format(x, y, t, totalDistance)
data class CubicSplinePair(val id: Int, val x: CubicSpline, val y: CubicSpline) {

data class PointData(val basedOn: CubicSplinePair, val x: Double, val y: Double, val totalDistance: Double, val t: Double, val tReal: Double) {
override fun toString(): String = "(%.4f, %.4f <t:%.4f (real %.4f by %d) | d:%.4f>)".format(x, y, t, tReal, basedOn.id, totalDistance)
fun toDesmos(): String = "(%.${PRECISION}f, %.${PRECISION}f)".format(x, y)
}

Expand Down Expand Up @@ -86,13 +88,17 @@ data class CubicSplinePair(val x: CubicSpline, val y: CubicSpline) {
waypoints.add(currentAt)
}
val actualStep = totalDistance / fragments
println("fragments: $fragments (%.4f per fragment, target %.4f, %.2f%% error)"
.format(actualStep, step, (100 * (step - actualStep)) / step)
)
if (!CubicSplineSolver.silent)
println("fragments: $fragments (%.4f per fragment, target %.4f, %.2f%% error)"
.format(actualStep, step, (100 * (step - actualStep)) / step)
)
return waypoints
}

fun updateCache(resolution: Double = 0.01) {
assert(abs(x.offset - y.offset) < 0.001) {
"Mismatched offsets on splines (desync id %d)".format(id)
}
val newCache: MutableList<PointData> = mutableListOf()
val steps = floor((tTo - tFrom) / resolution).toInt()
var d = 0.0
Expand All @@ -107,20 +113,23 @@ data class CubicSplinePair(val x: CubicSpline, val y: CubicSpline) {
}
lastX = xP
lastY = yP
newCache.add(PointData(xP, yP, d, t))
newCache.add(PointData(this, xP, yP, d, t, t + x.offset))
}
pointCache = newCache
totalDistance = d

println(
"total %.4f units | $steps steps".format(
totalDistance
if (!CubicSplineSolver.silent)
println(
"total %.4f units | $steps steps".format(
totalDistance
)
)
)
}
}

object CubicSplineSolver {
var silent: Boolean = false

@JvmStatic
fun solveMat(t1: Number, x1: Number, t2: Number, x2: Number, s1: Number, s2: Number) =
solveMat(
Expand All @@ -146,6 +155,7 @@ object CubicSplineSolver {
x2: Double,
s1: Double,
s2: Double,
tOffset: Double = 0.0,
): CubicSpline {
var matrix: Array<DoubleArray> = arrayOf(
doubleArrayOf(t1 * t1 * t1, t1 * t1, t1, 1.0, x1),
Expand All @@ -155,7 +165,7 @@ object CubicSplineSolver {
)
val nCols = matrix[0].size
for ((rowN, row) in matrix.withIndex()) {
val pivotCol = row.indexOfFirst { abs(it) > 1e-8 }
val pivotCol = row.indexOfFirst { abs(it) > 1e-3 }
if (pivotCol == -1 || pivotCol == row.size - 1) throw IllegalStateException("No suitable pivot column found")
val coeff = row[pivotCol]
row.mapInPlace { it -> it / coeff }
Expand All @@ -174,8 +184,8 @@ object CubicSplineSolver {
abs(it[index] - 1.0) < 0.001
}.last()
}
val result = CubicSpline.fromArrayAndBounds(factors, min(t1, t2), max(t1, t2))
println(result.toString())
val result = CubicSpline.fromArrayAndBounds(factors, min(t1, t2), max(t1, t2), tOffset)
// println(result.toString())
return result
}

Expand Down Expand Up @@ -211,8 +221,8 @@ object CubicSplineSolver {
val results: MutableList<CubicSpline> = mutableListOf()
for (i in 0..<m.size - 1) results.add(
solveMat(
xArr[i], yArr[i], xArr[i + 1], yArr[i + 1],
m[i], m[i + 1]
0.0, yArr[i], xArr[i + 1] - xArr[i], yArr[i + 1],
m[i], m[i + 1], xArr[i]
)
)
return results
Expand Down Expand Up @@ -242,6 +252,7 @@ object CubicSplineSolver {
for (i in 0..<xSplines.size) {
result.add(
CubicSplinePair(
i,
xSplines[i],
ySplines[i]
)
Expand Down
40 changes: 40 additions & 0 deletions TeamCode/src/test/java/org/firstinspires/ftc/teamcode/test/SplineTest.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,14 +3,19 @@ package org.firstinspires.ftc.teamcode.test
import org.firstinspires.ftc.teamcode.mmooover.kinematics.CubicSplinePair
import org.firstinspires.ftc.teamcode.mmooover.kinematics.CubicSplineSolver
import org.junit.jupiter.api.Test
import kotlin.random.Random
import kotlin.time.DurationUnit
import kotlin.time.TimeSource

class SplineTest {
@Test fun `test spline individual segments`() {
CubicSplineSolver.silent = false
CubicSplineSolver.solveMat(1, 1, 5, 5, 2, -1)
CubicSplineSolver.solveMat(5, 5, 10, 5, -1, -5)
}

@Test fun `test spline multiple segments`() {
CubicSplineSolver.silent = false
val result = CubicSplineSolver.solveMultiSegment(
doubleArrayOf(0.0, 5.0, 15.0, 10.0, 17.0),
doubleArrayOf(0.0, 3.0, 0.0, 5.0, 10.0)
Expand All @@ -19,6 +24,7 @@ class SplineTest {
}

@Test fun `test spline 2d multiple segments`() {
CubicSplineSolver.silent = false
val result = CubicSplineSolver.solve2DMultiSegment(
doubleArrayOf(0.0, 5.0, 15.0, 10.0, 17.0),
doubleArrayOf(0.0, 3.0, 0.0, 5.0, 10.0)
Expand All @@ -30,6 +36,7 @@ class SplineTest {
}

@Test fun `Test large-scale`() {
CubicSplineSolver.silent = false
val result = CubicSplineSolver.solve2DMultiSegment(
doubleArrayOf(0.0, 48.0, 96.0, 96.0, 96.0),
doubleArrayOf(0.0, 0.0, 48.0, 96.0, 120.0)
Expand All @@ -44,7 +51,40 @@ class SplineTest {
println("desmos: ${waypointList.map{it.toDesmos()}}")
}

@Test fun `Test fuzzing`() {
CubicSplineSolver.silent = true
val resultSet = mutableListOf<Double>()
val iterCount = 500
for (index in 1 .. iterCount){
val x: MutableList<Double> = mutableListOf(0.0)
val y: MutableList<Double> = mutableListOf(0.0)
val rng = Random.Default
repeat(10) {
x.add(rng.nextDouble(-240.0, 240.0))
y.add(rng.nextDouble(-240.0, 240.0))
}
x.add(0.0)
y.add(0.0)

val start = TimeSource.Monotonic.markNow()
val result = CubicSplineSolver.solve2DMultiSegment(
x.toDoubleArray(),
y.toDoubleArray()
)
val waypointList: MutableList<CubicSplinePair.PointData> = mutableListOf()
for (i in result)
waypointList.addAll(i.computeWaypoints())
val timeEnd = TimeSource.Monotonic.markNow()
if (index % 10 == 0) println("%d completed of %d (%.2f%%)".format(index, iterCount, (index.toDouble())/iterCount*100))
resultSet.add((timeEnd - start).toDouble(DurationUnit.MILLISECONDS))
}
println(
"Min %.2f Avg %.2f Max %.2f (ms)".format(resultSet.min(), resultSet.average(), resultSet.max())
)
}

@Test fun `Test small-scale`() {
CubicSplineSolver.silent = false
val result = CubicSplineSolver.solve2DMultiSegment(
doubleArrayOf(0.0, 8.0, 8.0, 0.0, 0.0),
doubleArrayOf(0.0, 0.0, 8.0, 16.0, 0.0)
Expand Down

0 comments on commit 65b3e69

Please sign in to comment.