diff --git a/road-runner/actions/src/main/kotlin/com/acmerobotics/roadrunner/actions/TrajectoryActionBuilder.kt b/road-runner/actions/src/main/kotlin/com/acmerobotics/roadrunner/actions/TrajectoryActionBuilder.kt index 3486587..c95dab2 100644 --- a/road-runner/actions/src/main/kotlin/com/acmerobotics/roadrunner/actions/TrajectoryActionBuilder.kt +++ b/road-runner/actions/src/main/kotlin/com/acmerobotics/roadrunner/actions/TrajectoryActionBuilder.kt @@ -261,13 +261,28 @@ class TrajectoryActionBuilder private constructor( } fun afterTime(dt: Double, f: InstantFunction) = afterTime(dt, InstantAction(f)) + /** + * Sets the starting tangent of the next path segment. + * See [RoadRunner docs](https://rr.brott.dev/docs/v1-0/guides/tangents/). + */ fun setTangent(r: Rotation2d) = TrajectoryActionBuilder(this, tb.setTangent(r), n, lastPoseUnmapped, lastPose, lastTangent, ms, cont) + + /** + * Sets the starting tangent of the next path segment. + * See [RoadRunner docs](https://rr.brott.dev/docs/v1-0/guides/tangents/). + */ fun setTangent(r: Double) = setTangent(Rotation2d.exp(r)) + /** + * Reverses the next path segment; actually a call to [setTangent(Math.PI)][setTangent]! + */ fun setReversed(reversed: Boolean) = TrajectoryActionBuilder(this, tb.setReversed(reversed), n, lastPoseUnmapped, lastPose, lastTangent, ms, cont) + /** + * Turns [angle] radians. + */ @JvmOverloads fun turn(angle: Double, turnConstraintsOverride: TurnConstraints? = null): TrajectoryActionBuilder { val b = endTrajectory() @@ -299,6 +314,9 @@ class TrajectoryActionBuilder private constructor( b2.n, lastPoseUnmapped, lastPose, lastTangent, b2.ms, b2.cont ) } + /** + * Turns to face heading [heading]. + */ @JvmOverloads fun turnTo(heading: Rotation2d, turnConstraintsOverride: TurnConstraints? = null): TrajectoryActionBuilder { val b = endTrajectory() @@ -308,6 +326,9 @@ class TrajectoryActionBuilder private constructor( fun turnTo(heading: Double, turnConstraintsOverride: TurnConstraints? = null) = turnTo(Rotation2d.exp(heading), turnConstraintsOverride) + /** + * Adds a line segment that goes forward [ds] in the current direction. + */ @JvmOverloads fun forward( ds: Double, @@ -321,6 +342,10 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes forward [ds] while maintaining current heading. + * Equivalent to [forward]. + */ @JvmOverloads fun forwardConstantHeading( ds: Double, @@ -334,6 +359,10 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes forward [ds], + * changing heading from current to [heading] using linear interpolation. + */ @JvmOverloads fun forwardLinearHeading( ds: Double, @@ -347,6 +376,11 @@ class TrajectoryActionBuilder private constructor( ), n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + + /** + * Adds a line segment that goes forward [ds], + * changing heading from current to [heading] using linear interpolation. + */ @JvmOverloads fun forwardLinearHeading( ds: Double, @@ -361,6 +395,10 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes forward [ds], + * changing heading from current to [heading] using spline interpolation. + */ @JvmOverloads fun forwardSplineHeading( ds: Double, @@ -374,6 +412,11 @@ class TrajectoryActionBuilder private constructor( ), n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + + /** + * Adds a line segment that goes forward [ds], + * changing heading from current to [heading] using spline interpolation. + */ @JvmOverloads fun forwardSplineHeading( ds: Double, @@ -388,6 +431,11 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes to x-coordinate [posX]. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the x-axis, this throws an error. + */ @JvmOverloads fun lineToX( posX: Double, @@ -401,6 +449,12 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes to x-coordinate [posX]. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the x-axis, this throws an error. + * Equivalent to [lineToX]. + */ @JvmOverloads fun lineToXConstantHeading( posX: Double, @@ -414,6 +468,12 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes to x-coordinate [posX], + * while changing heading from current to [heading] using linear interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the x-axis, this throws an error. + */ @JvmOverloads fun lineToXLinearHeading( posX: Double, @@ -427,6 +487,13 @@ class TrajectoryActionBuilder private constructor( ), n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + + /** + * Adds a line segment that goes to x-coordinate [posX], + * while changing heading from current to [heading] using linear interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the x-axis, this throws an error. + */ @JvmOverloads fun lineToXLinearHeading( posX: Double, @@ -441,6 +508,12 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes to x-coordinate [posX], + * while changing heading from current to [heading] using spline interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the x-axis, this throws an error. + */ @JvmOverloads fun lineToXSplineHeading( posX: Double, @@ -454,6 +527,13 @@ class TrajectoryActionBuilder private constructor( ), n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + + /** + * Adds a line segment that goes to x-coordinate [posX], + * while changing heading from current to [heading] using spline interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the x-axis, this throws an error. + */ @JvmOverloads fun lineToXSplineHeading( posX: Double, @@ -468,6 +548,11 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes to y-coordinate [posY]. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the y-axis, this throws an error. + */ @JvmOverloads fun lineToY( posY: Double, @@ -481,6 +566,12 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes to y-coordinate [posY]. + * * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the y-axis, this throws an error. + * Equivalent to [lineToY]. + */ @JvmOverloads fun lineToYConstantHeading( posY: Double, @@ -494,6 +585,12 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes to y-coordinate [posY], + * while changing heading from current to [heading] using linear interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the y-axis, this throws an error. + */ @JvmOverloads fun lineToYLinearHeading( posY: Double, @@ -507,6 +604,13 @@ class TrajectoryActionBuilder private constructor( ), n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + + /** + * Adds a line segment that goes to y-coordinate [posY], + * while changing heading from current to [heading] using linear interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the y-axis, this throws an error. + */ @JvmOverloads fun lineToYLinearHeading( posY: Double, @@ -521,6 +625,12 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes to y-coordinate [posY], + * while changing heading from current to [heading] using spline interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the y-axis, this throws an error. + */ @JvmOverloads fun lineToYSplineHeading( posY: Double, @@ -534,6 +644,13 @@ class TrajectoryActionBuilder private constructor( ), n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + + /** + * Adds a line segment that goes to y-coordinate [posY], + * while changing heading from current to [heading] using spline interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the y-axis, this throws an error. + */ @JvmOverloads fun lineToYSplineHeading( posY: Double, @@ -548,6 +665,9 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes to [pos]. + */ @JvmOverloads fun strafeTo( pos: Vector2d, @@ -561,6 +681,10 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes to [pos]. + * Equivalent to [strafeTo]. + */ @JvmOverloads fun strafeToConstantHeading( pos: Vector2d, @@ -574,6 +698,10 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes to [pos], + * changing heading from current to [heading] using linear interpolation. + */ @JvmOverloads fun strafeToLinearHeading( pos: Vector2d, @@ -587,6 +715,11 @@ class TrajectoryActionBuilder private constructor( ), n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + + /** + * Adds a line segment that goes to [pos], + * changing heading from current to [heading] using linear interpolation. + */ @JvmOverloads fun strafeToLinearHeading( pos: Vector2d, @@ -601,6 +734,10 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a line segment that goes to [pos], + * changing heading from current to [heading] using spline interpolation. + */ @JvmOverloads fun strafeToSplineHeading( pos: Vector2d, @@ -614,6 +751,11 @@ class TrajectoryActionBuilder private constructor( ), n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + + /** + * Adds a line segment that goes to [pos], + * changing heading from current to [heading] using spline interpolation. + */ @JvmOverloads fun strafeToSplineHeading( pos: Vector2d, @@ -628,6 +770,12 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pos] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending [pos] and [tangent]. + */ @JvmOverloads fun splineTo( pos: Vector2d, @@ -641,6 +789,13 @@ class TrajectoryActionBuilder private constructor( ), n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pos] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending [pos] and [tangent]. + */ @JvmOverloads fun splineTo( pos: Vector2d, @@ -655,6 +810,14 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pos] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending [pos] and [tangent]. + * The robot's heading remains constant + * as opposed to matching the tangent. + */ @JvmOverloads fun splineToConstantHeading( pos: Vector2d, @@ -668,6 +831,15 @@ class TrajectoryActionBuilder private constructor( ), n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pos] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending [pos] and [tangent]. + * The robot's heading remains constant + * as opposed to matching the tangent. + */ @JvmOverloads fun splineToConstantHeading( pos: Vector2d, @@ -682,6 +854,14 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pose.position][pose] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending position and [tangent]. + * The robot's heading linearly interpolates from its current heading + * to [pose.heading][pose]. + */ @JvmOverloads fun splineToLinearHeading( pose: Pose2d, @@ -695,6 +875,15 @@ class TrajectoryActionBuilder private constructor( ), n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pose.position][pose] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending position and [tangent]. + * The robot's heading linearly interpolates from its current heading + * to [pose.heading][pose]. + */ @JvmOverloads fun splineToLinearHeading( pose: Pose2d, @@ -709,6 +898,14 @@ class TrajectoryActionBuilder private constructor( n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pose.position][pose] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending position and [tangent]. + * The robot's heading interpolates from its current heading + * to [pose.heading][pose] using spline interpolation. + */ @JvmOverloads fun splineToSplineHeading( pose: Pose2d, @@ -722,6 +919,15 @@ class TrajectoryActionBuilder private constructor( ), n + 1, lastPoseUnmapped, lastPose, lastTangent, ms, cont ) + + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pose.position][pose] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending position and [tangent]. + * The robot's heading interpolates from its current heading + * to [pose.heading][pose] using spline interpolation. + */ @JvmOverloads fun splineToSplineHeading( pose: Pose2d, @@ -750,6 +956,9 @@ class TrajectoryActionBuilder private constructor( ).setTangent(it.lastTangent) } + /** + * Returns an Action object that follows the full sequence. + */ fun build(): Action { return endTrajectory().cont(NullAction()) } diff --git a/road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/paths/BezierCurves.kt b/road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/paths/BezierCurves.kt index 3a6cb7a..52e4879 100644 --- a/road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/paths/BezierCurves.kt +++ b/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) = ArclengthReparamCurve2d( +fun fromPoints(points: List) = ArclengthReparamCurve2d( BezierCurve2dInternal.fromPoints(points), 1e-6 ) diff --git a/road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/paths/PosePaths.kt b/road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/paths/PosePaths.kt index 923ca8c..d8affa4 100644 --- a/road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/paths/PosePaths.kt +++ b/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, @JvmField @@ -76,8 +78,6 @@ data class CompositePosePath( } } - constructor(paths: List) : 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( diff --git a/road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/paths/PositionPaths.kt b/road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/paths/PositionPaths.kt index 146afcc..a7e4fc3 100644 --- a/road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/paths/PositionPaths.kt +++ b/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 { 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(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 PositionPath.wrtArclength(): PositionPath = when(this) { + is Line -> this + is ArclengthReparamCurve2d -> this + else -> ArclengthReparamCurve2d(this, 1e-6) +} + data class CompositePositionPath @JvmOverloads constructor( @JvmField val paths: List>, diff --git a/road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/trajectories/Builders.kt b/road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/trajectories/Builders.kt index 8630f91..0707a3c 100644 --- a/road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/trajectories/Builders.kt +++ b/road-runner/core/src/main/kotlin/com/acmerobotics/roadrunner/trajectories/Builders.kt @@ -1,35 +1,8 @@ package com.acmerobotics.roadrunner.trajectories -import com.acmerobotics.roadrunner.profiles.ProfileParams -import com.acmerobotics.roadrunner.geometry.Arclength -import com.acmerobotics.roadrunner.geometry.DualNum -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.geometry.Pose2dDual -import com.acmerobotics.roadrunner.geometry.Rotation2d -import com.acmerobotics.roadrunner.geometry.Rotation2dDual -import com.acmerobotics.roadrunner.geometry.Vector2d -import com.acmerobotics.roadrunner.paths.ArclengthReparamCurve2d -import com.acmerobotics.roadrunner.paths.CompositePosePath -import com.acmerobotics.roadrunner.paths.CompositePositionPath -import com.acmerobotics.roadrunner.paths.ConstantHeadingPath -import com.acmerobotics.roadrunner.paths.HeadingPosePath -import com.acmerobotics.roadrunner.paths.IdentityPoseMap -import com.acmerobotics.roadrunner.paths.Line -import com.acmerobotics.roadrunner.paths.LinearHeadingPath -import com.acmerobotics.roadrunner.paths.MappedPosePath -import com.acmerobotics.roadrunner.paths.PoseMap -import com.acmerobotics.roadrunner.paths.PosePath -import com.acmerobotics.roadrunner.paths.PositionPath -import com.acmerobotics.roadrunner.paths.PositionPathView -import com.acmerobotics.roadrunner.paths.QuinticSpline1d -import com.acmerobotics.roadrunner.paths.QuinticSpline2dInternal -import com.acmerobotics.roadrunner.paths.SplineHeadingPath -import com.acmerobotics.roadrunner.paths.TangentPath -import com.acmerobotics.roadrunner.profiles.AccelConstraint -import com.acmerobotics.roadrunner.profiles.CompositeAccelConstraint -import com.acmerobotics.roadrunner.profiles.CompositeVelConstraint -import com.acmerobotics.roadrunner.profiles.VelConstraint -import com.acmerobotics.roadrunner.profiles.profile +import com.acmerobotics.roadrunner.geometry.* +import com.acmerobotics.roadrunner.paths.* +import com.acmerobotics.roadrunner.profiles.* import kotlin.math.PI import kotlin.math.abs @@ -662,14 +635,26 @@ class TrajectoryBuilder private constructor( accelConstraints + listOf(accelConstraintOverride ?: baseAccelConstraint) ) + /** + * Sets the starting tangent of the next path segment. + * See [RoadRunner docs](https://rr.brott.dev/docs/v1-0/guides/tangents/). + */ fun setTangent(r: Rotation2d) = TrajectoryBuilder( profileParams, pathBuilder.setTangent(r), beginEndVel, baseVelConstraint, baseAccelConstraint, poseMap, velConstraints, accelConstraints, ) + + /** + * Sets the starting tangent of the next path segment. + * See [RoadRunner docs](https://rr.brott.dev/docs/v1-0/guides/tangents/). + */ fun setTangent(r: Double) = setTangent(Rotation2d.exp(r)) + /** + * Reverses the next path segment; actually a call to [setTangent(Math.PI)][setTangent]! + */ fun setReversed(reversed: Boolean) = TrajectoryBuilder( profileParams, @@ -677,6 +662,9 @@ class TrajectoryBuilder private constructor( poseMap, velConstraints, accelConstraints, ) + /** + * Adds a line segment that goes forward [ds]. + */ @JvmOverloads fun forward( ds: Double, @@ -685,6 +673,10 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.forward(ds), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes forward [ds] while maintaining current heading. + * Equivalent to [forward]. + */ @JvmOverloads fun forwardConstantHeading( ds: Double, @@ -693,6 +685,10 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.forwardConstantHeading(ds), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes forward [ds], + * changing heading from current to [heading] using linear interpolation. + */ @JvmOverloads fun forwardLinearHeading( ds: Double, @@ -701,6 +697,11 @@ class TrajectoryBuilder private constructor( accelConstraintOverride: AccelConstraint? = null ) = add(pathBuilder.forwardLinearHeading(ds, heading), velConstraintOverride, accelConstraintOverride) + + /** + * Adds a line segment that goes forward [ds], + * changing heading from current to [heading] using linear interpolation. + */ @JvmOverloads fun forwardLinearHeading( ds: Double, @@ -710,6 +711,10 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.forwardLinearHeading(ds, heading), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes forward [ds], + * changing heading from current to [heading] using spline interpolation. + */ @JvmOverloads fun forwardSplineHeading( ds: Double, @@ -718,6 +723,11 @@ class TrajectoryBuilder private constructor( accelConstraintOverride: AccelConstraint? = null ) = add(pathBuilder.forwardSplineHeading(ds, heading), velConstraintOverride, accelConstraintOverride) + + /** + * Adds a line segment that goes forward [ds], + * changing heading from current to [heading] using spline interpolation. + */ @JvmOverloads fun forwardSplineHeading( ds: Double, @@ -727,6 +737,11 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.forwardSplineHeading(ds, heading), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes to \(x\)-coordinate [posX]. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the x-axis, this throws an error. + */ @JvmOverloads fun lineToX( posX: Double, @@ -735,6 +750,12 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.lineToX(posX), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes to \(x\)-coordinate [posX]. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the x-axis, this throws an error. + * Equivalent to [lineToX]. + */ @JvmOverloads fun lineToXConstantHeading( posX: Double, @@ -743,6 +764,12 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.lineToXConstantHeading(posX), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes to \(x\)-coordinate [posX], + * while changing heading from current to [heading] using linear interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the x-axis, this throws an error. + */ @JvmOverloads fun lineToXLinearHeading( posX: Double, @@ -751,6 +778,13 @@ class TrajectoryBuilder private constructor( accelConstraintOverride: AccelConstraint? = null ) = add(pathBuilder.lineToXLinearHeading(posX, heading), velConstraintOverride, accelConstraintOverride) + + /** + * Adds a line segment that goes to \(x\)-coordinate [posX], + * while changing heading from current to [heading] using linear interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the x-axis, this throws an error. + */ @JvmOverloads fun lineToXLinearHeading( posX: Double, @@ -760,6 +794,12 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.lineToXLinearHeading(posX, heading), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes to \(x\)-coordinate [posX], + * while changing heading from current to [heading] using spline interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the x-axis, this throws an error. + */ @JvmOverloads fun lineToXSplineHeading( posX: Double, @@ -768,6 +808,13 @@ class TrajectoryBuilder private constructor( accelConstraintOverride: AccelConstraint? = null ) = add(pathBuilder.lineToXSplineHeading(posX, heading), velConstraintOverride, accelConstraintOverride) + + /** + * Adds a line segment that goes to \(x\)-coordinate [posX], + * while changing heading from current to [heading] using spline interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the x-axis, this throws an error. + */ @JvmOverloads fun lineToXSplineHeading( posX: Double, @@ -777,6 +824,11 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.lineToXSplineHeading(posX, heading), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes to \(y\)-coordinate [posY]. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the y-axis, this throws an error. + */ @JvmOverloads fun lineToY( posY: Double, @@ -785,6 +837,12 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.lineToY(posY), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes to \(y\)-coordinate [posY]. + * * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the y-axis, this throws an error. + * Equivalent to [lineToY]. + */ @JvmOverloads fun lineToYConstantHeading( posY: Double, @@ -793,6 +851,12 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.lineToYConstantHeading(posY), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes to \(y\)-coordinate [posY], + * while changing heading from current to [heading] using linear interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the y-axis, this throws an error. + */ @JvmOverloads fun lineToYLinearHeading( posY: Double, @@ -801,6 +865,13 @@ class TrajectoryBuilder private constructor( accelConstraintOverride: AccelConstraint? = null ) = add(pathBuilder.lineToYLinearHeading(posY, heading), velConstraintOverride, accelConstraintOverride) + + /** + * Adds a line segment that goes to \(y\)-coordinate [posY], + * while changing heading from current to [heading] using linear interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the y-axis, this throws an error. + */ @JvmOverloads fun lineToYLinearHeading( posY: Double, @@ -810,6 +881,12 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.lineToYLinearHeading(posY, heading), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes to \(y\)-coordinate [posY], + * while changing heading from current to [heading] using spline interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the y-axis, this throws an error. + */ @JvmOverloads fun lineToYSplineHeading( posY: Double, @@ -818,6 +895,13 @@ class TrajectoryBuilder private constructor( accelConstraintOverride: AccelConstraint? = null ) = add(pathBuilder.lineToYSplineHeading(posY, heading), velConstraintOverride, accelConstraintOverride) + + /** + * Adds a line segment that goes to \(y\)-coordinate [posY], + * while changing heading from current to [heading] using spline interpolation. + * The robot will continue traveling in the direction it is currently in; + * if the robot is perpendicular to the y-axis, this throws an error. + */ @JvmOverloads fun lineToYSplineHeading( posY: Double, @@ -827,6 +911,9 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.lineToYSplineHeading(posY, heading), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes to [pos]. + */ @JvmOverloads fun strafeTo( pos: Vector2d, @@ -835,6 +922,10 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.strafeTo(pos), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes to [pos]. + * Equivalent to [strafeTo]. + */ @JvmOverloads fun strafeToConstantHeading( pos: Vector2d, @@ -843,6 +934,10 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.strafeToConstantHeading(pos), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes to [pos], + * changing heading from current to [heading] using linear interpolation. + */ @JvmOverloads fun strafeToLinearHeading( pos: Vector2d, @@ -851,6 +946,11 @@ class TrajectoryBuilder private constructor( accelConstraintOverride: AccelConstraint? = null ) = add(pathBuilder.strafeToLinearHeading(pos, heading), velConstraintOverride, accelConstraintOverride) + + /** + * Adds a line segment that goes to [pos], + * changing heading from current to [heading] using linear interpolation. + */ @JvmOverloads fun strafeToLinearHeading( pos: Vector2d, @@ -860,6 +960,10 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.strafeToLinearHeading(pos, heading), velConstraintOverride, accelConstraintOverride) + /** + * Adds a line segment that goes to [pos], + * changing heading from current to [heading] using spline interpolation. + */ @JvmOverloads fun strafeToSplineHeading( pos: Vector2d, @@ -868,6 +972,11 @@ class TrajectoryBuilder private constructor( accelConstraintOverride: AccelConstraint? = null ) = add(pathBuilder.strafeToSplineHeading(pos, heading), velConstraintOverride, accelConstraintOverride) + + /** + * Adds a line segment that goes to [pos], + * changing heading from current to [heading] using spline interpolation. + */ @JvmOverloads fun strafeToSplineHeading( pos: Vector2d, @@ -877,6 +986,12 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.strafeToSplineHeading(pos, heading), velConstraintOverride, accelConstraintOverride) + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pos] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending [pos] and [tangent]. + */ @JvmOverloads fun splineTo( pos: Vector2d, @@ -885,6 +1000,13 @@ class TrajectoryBuilder private constructor( accelConstraintOverride: AccelConstraint? = null ) = add(pathBuilder.splineTo(pos, tangent), velConstraintOverride, accelConstraintOverride) + + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pos] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending [pos] and [tangent]. + */ @JvmOverloads fun splineTo( pos: Vector2d, @@ -894,6 +1016,14 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.splineTo(pos, tangent), velConstraintOverride, accelConstraintOverride) + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pos] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending [pos] and [tangent]. + * The robot's heading remains constant + * as opposed to matching the tangent. + */ @JvmOverloads fun splineToConstantHeading( pos: Vector2d, @@ -902,6 +1032,15 @@ class TrajectoryBuilder private constructor( accelConstraintOverride: AccelConstraint? = null ) = add(pathBuilder.splineToConstantHeading(pos, tangent), velConstraintOverride, accelConstraintOverride) + + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pos] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending [pos] and [tangent]. + * The robot's heading remains constant + * as opposed to matching the tangent. + */ @JvmOverloads fun splineToConstantHeading( pos: Vector2d, @@ -911,6 +1050,14 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.splineToConstantHeading(pos, tangent), velConstraintOverride, accelConstraintOverride) + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pose.position][pose] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending position and [tangent]. + * The robot's heading linearly interpolates from its current heading + * to [pose.heading][pose]. + */ @JvmOverloads fun splineToLinearHeading( pose: Pose2d, @@ -919,6 +1066,15 @@ class TrajectoryBuilder private constructor( accelConstraintOverride: AccelConstraint? = null ) = add(pathBuilder.splineToLinearHeading(pose, tangent), velConstraintOverride, accelConstraintOverride) + + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pose.position][pose] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending position and [tangent]. + * The robot's heading linearly interpolates from its current heading + * to [pose.heading][pose]. + */ @JvmOverloads fun splineToLinearHeading( pose: Pose2d, @@ -928,6 +1084,15 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.splineToLinearHeading(pose, tangent), velConstraintOverride, accelConstraintOverride) + + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pose.position][pose] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending position and [tangent]. + * The robot's heading interpolates from its current heading + * to [pose.heading][pose] using spline interpolation. + */ @JvmOverloads fun splineToSplineHeading( pose: Pose2d, @@ -936,6 +1101,15 @@ class TrajectoryBuilder private constructor( accelConstraintOverride: AccelConstraint? = null ) = add(pathBuilder.splineToSplineHeading(pose, tangent), velConstraintOverride, accelConstraintOverride) + + /** + * Adds a curved path segment using quintic Hermite splines + * that goes to [pose.position][pose] with an end tangent of [tangent]. + * The shape of the curve is based off of the starting position and tangent + * as well as the ending position and [tangent]. + * The robot's heading interpolates from its current heading + * to [pose.heading][pose] using spline interpolation. + */ @JvmOverloads fun splineToSplineHeading( pose: Pose2d, @@ -945,6 +1119,11 @@ class TrajectoryBuilder private constructor( ) = add(pathBuilder.splineToSplineHeading(pose, tangent), velConstraintOverride, accelConstraintOverride) + /** + * Builds the specified trajectories, creating a new CancelableTrajectory + * object for each discontinuity. + * @return the resulting list of CancelableTrajectory objects + */ fun buildToList(): List { val rawPaths = pathBuilder.build() val offsets = rawPaths.scan(0) { acc, rawPath -> acc + rawPath.paths.size } @@ -970,6 +1149,19 @@ class TrajectoryBuilder private constructor( } } + /** + * Builds the trajectory, creating a new CancelableTrajectory object + * for each discontinuity, and then composes them into a single CompositeTrajectory object. + * There may be complete stops in this composite based on where its components start and end. + * @return the resulting CompositeTrajectory object + */ fun buildToComposite() = CompositeTrajectory(buildToList()) + + /** + * Builds the specified trajectories, creating a new CancelableTrajectory + * object for each discontinuity. + * Equivalent to [buildToList]. + * @return the resulting list of CancelableTrajectory objects + */ fun build() = buildToList() } diff --git a/road-runner/core/src/test/kotlin/com/acmerobotics/roadrunner/paths/CurvesTest.kt b/road-runner/core/src/test/kotlin/com/acmerobotics/roadrunner/paths/CurvesTest.kt index b7f6017..8db9d54 100644 --- a/road-runner/core/src/test/kotlin/com/acmerobotics/roadrunner/paths/CurvesTest.kt +++ b/road-runner/core/src/test/kotlin/com/acmerobotics/roadrunner/paths/CurvesTest.kt @@ -464,7 +464,7 @@ class CurvesTest { Vector2d(1.0, 2.0), Vector2d(3.0, 1.0) ) - val curve = generateCurveFromPoints(points) + val curve = fromPoints(points) // Test that the curve starts and ends at the correct points val startPoint = curve[0.0, 1].value() @@ -476,7 +476,7 @@ class CurvesTest { assertEquals(points.last().y, endPoint.y, 1e-6) // Test the vararg version - val curveVararg = generateCurveFromPoints( + val curveVararg = fromPoints( Vector2d(0.0, 0.0), Vector2d(1.0, 2.0), Vector2d(3.0, 1.0) @@ -491,4 +491,24 @@ class CurvesTest { assertEquals(endPoint.x, endPointVararg.x, 1e-6) assertEquals(endPoint.y, endPointVararg.y, 1e-6) } + + @Test + fun testBezierFromSpline() { + val r = Random.Default + repeat(100) { + val spline = QuinticSpline1d( + DualNum(doubleArrayOf(r.nextDouble(), r.nextDouble(), r.nextDouble())), + DualNum(doubleArrayOf(r.nextDouble(), r.nextDouble(), r.nextDouble())), + ) + val bezier = BezierCurve1d.fromRRSpline(spline) + + val t = r.nextDouble(0.0, 1.0) + val splineValues = spline[t, 4] + val bezierValues = bezier[t, 4] + + splineValues.values().zip(bezierValues.values()).forEach { + assertEquals(it.first, it.second, 1e-6) + } + } + } } diff --git a/road-runner/ftc/src/main/java/com/acmerobotics/roadrunner/ftc/Robots.kt b/road-runner/ftc/src/main/java/com/acmerobotics/roadrunner/ftc/Robots.kt index 3cacbcf..a363c1f 100644 --- a/road-runner/ftc/src/main/java/com/acmerobotics/roadrunner/ftc/Robots.kt +++ b/road-runner/ftc/src/main/java/com/acmerobotics/roadrunner/ftc/Robots.kt @@ -9,8 +9,11 @@ import com.acmerobotics.roadrunner.geometry.Pose2d import com.acmerobotics.roadrunner.geometry.PoseVelocity2d import com.acmerobotics.roadrunner.geometry.PoseVelocity2dDual import com.acmerobotics.roadrunner.geometry.Time +import com.acmerobotics.roadrunner.paths.PosePath import com.acmerobotics.roadrunner.profiles.AccelConstraint import com.acmerobotics.roadrunner.profiles.VelConstraint +import com.acmerobotics.roadrunner.profiles.forwardProfile +import com.acmerobotics.roadrunner.trajectories.DisplacementTrajectory import com.acmerobotics.roadrunner.trajectories.TrajectoryBuilder import com.acmerobotics.roadrunner.trajectories.TurnConstraints @@ -37,6 +40,25 @@ interface Drive { fun updatePoseEstimate(): PoseVelocity2d { return localizer.update() } + + /** + * Creates and returns a time-optimal profile for the given path. + */ + fun createProfile(path: PosePath) = forwardProfile( + followerParams.profileParams, + path, + 0.0, + followerParams.velConstraint, + followerParams.accelConstraint, + ) + + /** + * Creates and returns a time-optimal profile for the given path. + */ + fun createTrajectory(path: PosePath) = DisplacementTrajectory( + path, + createProfile(path) + ) } interface Localizer { diff --git a/road-runner/gradle.properties b/road-runner/gradle.properties index e69a81d..9e48641 100644 --- a/road-runner/gradle.properties +++ b/road-runner/gradle.properties @@ -7,5 +7,5 @@ org.gradle.jvmargs=-XX:MaxMetaspaceSize=512m org.jetbrains.dokka.experimental.gradle.pluginMode=V2Enabled org.jetbrains.dokka.experimental.gradle.pluginMode.noWarn=true -version=2.3.1 +version=2.4.0 automaticMavenCentralSync=true \ No newline at end of file