Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add before end markers #126

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
Open

Add before end markers #126

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
d04cd9b
Add before end time and disp
kevinthegreat1 Mar 4, 2025
38e69f2
Add separate builder methods for before end
kevinthegreat1 Mar 4, 2025
1ae1b2d
Merge branch 'master' into before-end-action
kevinthegreat1 Mar 26, 2025
855afe9
Add before end tests
kevinthegreat1 Mar 26, 2025
7b137ae
Integrate back into after time and disp
kevinthegreat1 Mar 26, 2025
b7fe82a
Add invalid time and disp test
kevinthegreat1 Mar 26, 2025
386a62d
Fix indents
kevinthegreat1 Mar 26, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
60 changes: 44 additions & 16 deletions road-runner/actions/src/main/kotlin/com/acmerobotics/roadrunner/Actions.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -151,17 +151,33 @@ private fun seqCons(hd: Action, tl: Action): Action =
private sealed class MarkerFactory(
val segmentIndex: Int,
) {
abstract fun make(t: TimeTrajectory, segmentDisp: Double): Action
abstract fun make(t: TimeTrajectory, segmentDisp: Double, segmentEndDisp: Double): Action
}

private class TimeMarkerFactory(segmentIndex: Int, val dt: Double, val a: Action) : MarkerFactory(segmentIndex) {
override fun make(t: TimeTrajectory, segmentDisp: Double) =
seqCons(SleepAction(t.profile.inverse(segmentDisp) + dt), a)
private class TimeMarkerFactory(segmentIndex: Int, val dt: Double, val a: Action, val relativeToStart: Boolean = true) :
MarkerFactory(segmentIndex) {
override fun make(t: TimeTrajectory, segmentDisp: Double, segmentEndDisp: Double) =
seqCons(
SleepAction(
t.profile.inverse(
if (relativeToStart) segmentDisp else segmentEndDisp,
) + dt,
),
a,
)
}

private class DispMarkerFactory(segmentIndex: Int, val ds: Double, val a: Action) : MarkerFactory(segmentIndex) {
override fun make(t: TimeTrajectory, segmentDisp: Double) =
seqCons(SleepAction(t.profile.inverse(segmentDisp + ds)), a)
private class DispMarkerFactory(segmentIndex: Int, val ds: Double, val a: Action, val relativeToStart: Boolean = true) :
MarkerFactory(segmentIndex) {
override fun make(t: TimeTrajectory, segmentDisp: Double, segmentEndDisp: Double) =
seqCons(
SleepAction(
t.profile.inverse(
(if (relativeToStart) segmentDisp else segmentEndDisp) + ds,
),
),
a,
)
}

fun interface TurnActionFactory {
Expand Down Expand Up @@ -297,7 +313,7 @@ class TrajectoryActionBuilder private constructor(
for (m in ms) {
val i = m.segmentIndex - offset
if (i >= 0) {
actions.add(m.make(timeTraj, traj.offsets[i]))
actions.add(m.make(timeTraj, traj.offsets[i], traj.offsets[i + 1]))
} else {
msRem.add(m)
}
Expand Down Expand Up @@ -337,29 +353,41 @@ class TrajectoryActionBuilder private constructor(
}

/**
* Schedules action [a] to execute in parallel starting at a displacement [ds] after the last trajectory segment.
* The action start is clamped to the span of the current trajectory.
* Schedules action [a] to execute in parallel starting at a displacement [ds] relative to the start of the next trajectory segment, turn, or other action.
*
* If [ds] is positive, the action will execute [ds] units after the start of the next trajectory segment.
* If [ds] is negative, the action will execute [ds] units before the end of the previous trajectory segment.
* Note that the start of the next trajectory segment is the same as the end of the previous trajectory segment.
*
* The action start is clamped to the span of the current trajectory (not the trajectory segment),
* but the current trajectory will wait for the action to complete.
*
* Cannot be called without an applicable pending trajectory.
*/
// TODO: Should calling this without an applicable trajectory implicitly begin an empty trajectory and execute the
// action immediately?
fun afterDisp(ds: Double, a: Action): TrajectoryActionBuilder {
require(ds >= 0.0) { "Displacement ($ds) must be non-negative" }
val relativeToStart = ds >= 0.0

return TrajectoryActionBuilder(
this, tb, n, lastPoseUnmapped, lastPose, lastTangent,
ms + listOf(DispMarkerFactory(n, ds, a)), cont
ms + listOf(DispMarkerFactory(if (relativeToStart) n else n - 1, ds, a, relativeToStart)), cont,
)
}
fun afterDisp(ds: Double, f: InstantFunction) = afterDisp(ds, InstantAction(f))

/**
* Schedules action [a] to execute in parallel starting [dt] seconds after the last trajectory segment, turn, or
* other action.
* Schedules action [a] to execute in parallel starting [dt] seconds relative to the start of the next trajectory segment, turn, or other action.
*
* If [dt] is positive, the action will execute [dt] seconds after the start of the next trajectory segment.
* If [dt] is negative, the action will execute [dt] seconds before the end of the previous trajectory segment.
* Note that the start of the next trajectory segment is the same as the end of the previous trajectory segment.
*
* The action start is clamped to the span of the current trajectory (not the trajectory segment),
* but the current trajectory will wait for the action to complete.
*/
fun afterTime(dt: Double, a: Action): TrajectoryActionBuilder {
require(dt >= 0.0) { "Time ($dt) must be non-negative" }
val relativeToStart = dt >= 0.0

return if (n == 0) {
TrajectoryActionBuilder(this, tb, 0, lastPoseUnmapped, lastPose, lastTangent, emptyList()) { tail ->
Expand All @@ -368,7 +396,7 @@ class TrajectoryActionBuilder private constructor(
} else {
TrajectoryActionBuilder(
this, tb, n, lastPoseUnmapped, lastPose, lastTangent,
ms + listOf(TimeMarkerFactory(n, dt, a)), cont
ms + listOf(TimeMarkerFactory(if (relativeToStart) n else n - 1, dt, a, relativeToStart)), cont,
)
}
}
Expand Down
114 changes: 112 additions & 2 deletions road-runner/actions/src/test/kotlin/com/acmerobotics/roadrunner/ActionRegressionTest.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -141,9 +141,66 @@ class ActionRegressionTest {
assertEquals(
"ParallelAction(initialActions=[SequentialAction(initialActions=[]), " +
"SequentialAction(initialActions=[" +
"SleepAction(dt=1.0), a])])",
"SleepAction(dt=1.0), A])])",
base
.afterTime(1.0, LabelAction("a"))
.afterTime(1.0, LabelAction("A"))
.build()
.toString()
)
}

@Test
@Strictfp
fun testTrajectoryTimeNegativeMarkers() {
assertEquals(
"ParallelAction(initialActions=[" +
"SequentialAction(initialActions=[Trajectory, Trajectory]), " +
"SequentialAction(initialActions=[SleepAction(dt=2.32842712474619), A])" +
"])",
base
.lineToX(20.0)
.afterTime(-0.5, LabelAction("A"))
.lineToXLinearHeading(30.0, Math.PI / 2)
.build()
.toString()
)

assertEquals(
"SequentialAction(initialActions=[Trajectory, ParallelAction(initialActions=[" +
"SequentialAction(initialActions=[Trajectory, Trajectory]), " +
"SequentialAction(initialActions=[SleepAction(dt=1.4999999999999993), A])" +
"])])",
base
.lineToX(10.0)
.lineToXLinearHeading(20.0, Math.PI / 2)
.afterTime(-0.5, LabelAction("A"))
.lineToX(30.0)
.build()
.toString(),
)

assertEquals(
"SequentialAction(initialActions=[Trajectory, Trajectory, ParallelAction(initialActions=[" +
"SequentialAction(initialActions=[Trajectory]), " +
"SequentialAction(initialActions=[SleepAction(dt=1.5000000000000009), A])" +
"])])",
base
.lineToX(10.0)
.lineToXLinearHeading(20.0, Math.PI / 2)
.lineToX(30.0)
.afterTime(-0.5, LabelAction("A"))
.build()
.toString()
)

assertEquals(
"ParallelAction(initialActions=[" +
"SequentialAction(initialActions=[Trajectory]), " +
"SequentialAction(initialActions=[SleepAction(dt=-0.5), A])" +
"])",
base
.afterTime(-0.5, LabelAction("A"))
.lineToX(10.0)
.build()
.toString()
)
Expand Down Expand Up @@ -202,4 +259,57 @@ class ActionRegressionTest {
.toString()
)
}

@Test
@Strictfp
fun testTrajectoryDispNegativeMarkers() {
assertEquals(
"ParallelAction(initialActions=[" +
"SequentialAction(initialActions=[Trajectory, Trajectory]), " +
"SequentialAction(initialActions=[SleepAction(dt=2.121320343559643), A])" +
"])",
base
.lineToX(20.0)
.afterDisp(-2.5, LabelAction("A"))
.lineToXLinearHeading(30.0, Math.PI / 2)
.build()
.toString()
)

assertEquals(
"SequentialAction(initialActions=[Trajectory, ParallelAction(initialActions=[" +
"SequentialAction(initialActions=[Trajectory, Trajectory]), " +
"SequentialAction(initialActions=[SleepAction(dt=1.2928932188134519), A])" +
"])])",
base
.lineToX(10.0)
.lineToXLinearHeading(20.0, Math.PI / 2)
.afterDisp(-2.5, LabelAction("A"))
.lineToX(30.0)
.build()
.toString(),
)

assertEquals(
"SequentialAction(initialActions=[Trajectory, Trajectory, ParallelAction(initialActions=[" +
"SequentialAction(initialActions=[Trajectory]), " +
"SequentialAction(initialActions=[SleepAction(dt=1.2928932188134532), A])" +
"])])",
base
.lineToX(10.0)
.lineToXLinearHeading(20.0, Math.PI / 2)
.lineToX(30.0)
.afterDisp(-2.5, LabelAction("A"))
.build()
.toString()
)

assertFails {
base
.afterDisp(-2.5, LabelAction("A"))
.lineToX(10.0)
.build()
.toString()
}
}
}