Determining path discontinuities

[Team14423]

Is there an easy way to test for path discontinuity?

We're trying this (this is in RRPathVisualizer in Kotlin, but our FTC Java gives us the same error)

val trajectory3 = TrajectoryBuilder(Pose2d(20.0,-32.0,175.0.toRadians),175.0.toRadians, combinedConstraints)

        trajectory3
            .splineToLinearHeading(Pose2d(-8.0, -48.0 , 0.0.toRadians),0.0.toRadians)
            .splineToLinearHeading(Pose2d(-32.0, -47.0 ,0.0.toRadians),0.0.toRadians)

        val endtraj3 =trajectory3.build()
        list.add(endtraj3)

We keep getting discontinuity errors between the first and the second. This happens when we use different combinations of splineto or lineto. It seems like this is a path that should be easily doable with splines. Obviously we're missing something in the math, but reading the code didn't pop out to me how we can make sure we've got continuity.

I'll note that when we call this in two separate trajectories (which we are trying to avoid for speed), on the second one we call it in reverse (though that seems unnecessary if we are moving to a linearHeading).

[Team14423]

Here's the path separately, and the discontinuity becomes pretty obvious. I guess the question is why is there a hook on the end (which I know is related to the tangent), and why doesn't the builder put the two paths together? The lineto looks similar, though I would think you could add a spline onto the end of a line.

[rbrott]

I guess the question is why is there a hook on the end (which I know is related to the tangent), and why doesn't the builder put the two paths together?

Yes, the hook comes from a mismatched tangent. Since the path is roughly flowing from positive to negative X, you probably want a tangent near 180deg.

The second issue a bit more subtle. Linear heading interpolators ruin continuity and can only be used for single-segment paths. You can usually replace them with spline heading interpolators for a similar result.

TrajectoryBuilder(
    Pose2d(20.0,-32.0, 175.0.toRadians()), 175.0.toRadians(),
    velConstraint, accelConstraint
)
    .splineToSplineHeading(Pose2d(-8.0, -48.0, 0.0.toRadians()), 180.0.toRadians())
    .splineToSplineHeading(Pose2d(-32.0, -47.0, 0.0.toRadians()), 180.0.toRadians())
    .build())

[Team14423]

Beautiful - thank you. Funny that the second one is less subtle - We need to read up on how the tangent affects the curve and heading in splines, as it's all a black box at the moment.