Unexpected discontinuity when transitioning from .splineToLinearHeading() to splineTo()

[madacker]

RR FTC Version

0.1.8

Observed Behavior

Using MeepMeep 1.0.2:

            myBot.runAction(myBot.getDrive().actionBuilder(new Pose2d(-34, -64, (Math.toRadians(90))))
                    .splineToLinearHeading(new Pose2d(-55, -30, 0), Math.toRadians(90))
                    .splineTo(new Vector2d(0, 0), 0)
                    .build());

It's non-obvious to us as to why there is a trajectory discontinuity that causes the robot to stop at the beginning of the .splineTo().

Tuning Files

No response

Robot Logs

No response

[rbrott]

There's a discontinuity in angular velocity between linear interpolation in the first segment and tangent (default) interpolation in the second segment. Linear interpolation is great for changing heading, but it doesn't play nicely with most other interpolators (including other linear interpolators). You can try fixing the continuity by

• Replacing the first segment withsplineToSplineHeading()
• Replacing the second segment with splineToSplineHeading()
• Using a lower-level API to make a trajectory with two path segments and one linear heading interpolator (can give more details if desired)

The first two options are easiest, though spline heading can have undesirable swings/overshoot. The latter might not be what you want either.