[C++] Seeing rotation target reset while following path

[jon-bassi]

Describe the bug
Seeing the rotation from getTargetHolonomicPose() resetting after rotation target has been reached, if reached before end of path.
This occurs both when using Rotation Targets within the UI within a single path or when transitioning from one path to another and setting rotation using the Goal End State.

By resetting, what I mean is that the robot's rotation seems to revert to what it would be had there not been a rotation target present within the path at all. i.e a target of 90 degrees at 50% through the path with an End Goal of 90 degrees will reach 90 at the halfway point, then immediately jump to 45 degrees, and make its way back to 90 degrees by the end of the path.

Similarly, when using multiple paths, where there should be no rotation in the second path, I see what looks to be the robot reverting to a rotation of 0 as soon as the path starts, even when reaching the target rotation in the previous path

To Reproduce
Note that I am not using the Auto Builder or the FollowPathCommand, I'm using the path class to read the path files, a PPHolonomicDriveController to calculate our relativeSpeeds, and the Trajectory class to generate the trajectories that are required by the DriveController. The implementation closely matches the implementation of FollowPathCommand with some changes to get everything to work with our Drive subsystem.

That said, the error I'm seeing is fully within the data returned to us by PathPlanner::State::getTargetHolonomicPose().Rotation()

1. Draw a path with 2 points
2. Set a target rotation of anything (say 90 degrees) at 50% though the path
3. run with the C++ PathPlanner implementation

with multiple paths

1. draw one path which rotates throughout (say 90 degrees)
2. draw a second path which starts where the first path ended with a Goal End State of 90 degrees for rotation
3. run with the C++ PathPlanner implementation

Expected behavior
Bot should be allowed to reach overall rotation target before end of path and not revert rotation halfway through

Screenshots

Versions: (please complete the following information):

• OS: build on Intel Mac
• GUI Version: 2024.1.1
• PPLib Version: 2024.1.1
• PPLib Language: C++

Additional context
My thought was that it seems that the code is not necessarily using the startingRotation I was passing in and I tracked what was going on to PathPlannerTrajectory::GenerateStates()

Not sure if this is the root cause but this line seems sus
https://github.com/mjansen4857/pathplanner/blob/07ff0d684cd4a51062b7a9b1bc902df15343b44e/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp#L88C4-L89C60

[mjansen4857]

The line you linked was correct but the actual interpolation right below it was wrong. Parenthesis in the wrong spot.