Can't figure out discontinuity between two segments

[giselaelise]

I'd be grateful for your help figuring out why there is a discontinuity between these two segments:

RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep)
  .setDimensions(8.0, 14.0)
  .followTrajectorySequence(drive ->
    drive.trajectorySequenceBuilder(
      new Pose2d(36.0, -65.0, toRadians(0.0))).setReversed(true)
      .splineToSplineHeading(new Pose2d(8.0, -65.0, toRadians(0.0)), toRadians(180.0))
      .splineToLinearHeading(new Pose2d(4.0, -40.0, toRadians(30.0)), toRadians(120.0))
      .build()
    );

[rbrott]

You've stumbled on a subtle limitation of the current path builder system. The constant, spline, and tangent interpolators all agree on setting zero velocity at knots by default. (This agreement is achieved by setting the spline second derivatives to zero. That's where the extra quintic polynomial constraints go.) But linear interpolators only have zero knot velocity in the degenerate case where they have constant heading.

In principle, you should be able to join the linear and spline heading segments smoothly. It's actually possible to achieve this using the lower-level API. You want to construct a SplineInterpolator with endHeadingDeriv equal to the linear interpolator's startDeriv(). I'm happy to adapt your sample following this approach if you would like.

The other, easier approach is to replace the linear interpolator with another spline one, though I know the splines can be finicky.

[giselaelise]

Thank you @rbrott. If you could show how to create the SplineInterpolator to keep that path, that would be awesome.

[rbrott]

Hopefully this gets close to what you want.

val pose1 = Pose2d(36.0, -65.0, 0.0)
val pose2 = Pose2d(8.0, -65.0, 0.0)
val pose3 = Pose2d(4.0, -40.0, PI / 6.0)

val derivMag2 = (pose2.vec() distTo pose3.vec())
val spline2 = QuinticSpline(
    QuinticSpline.Knot(pose2.vec(), Vector2d.polar(derivMag2, PI)),
    QuinticSpline.Knot(pose3.vec(), Vector2d.polar(derivMag2, 2.0 * PI / 3.0)))
val interp2 = LinearInterpolator(0.0, PI / 6.0)
interp2.init(spline2)
val seg2 = PathSegment(spline2, interp2)

val derivMag1 = (pose1.vec() distTo pose2.vec())
val spline1 = QuinticSpline(
    QuinticSpline.Knot(pose1.vec(), Vector2d.polar(derivMag1, PI)),
    QuinticSpline.Knot(pose2.vec(), Vector2d.polar(derivMag1, PI)))
val interp1 = SplineInterpolator(0.0, 0.0,
    endHeadingDeriv = interp2.startDeriv())
interp1.init(spline1)
val seg1 = PathSegment(spline1, interp1)

val path = Path(listOf(seg1, seg2))
val traj = TrajectoryGenerator.generateTrajectory(path,
    { _, _, _, _ -> 40.0 }, { _, _, _, _ -> 30.0 })

You'll need to replace the velocity/acceleration constraints on the last line with your own.