TrapezoidProfile does not correctly limit initial/goal velocities

[amquake]

Describe the bug When constructing a TrapezoidProfile with an initial velocity of greater magnitude than its maximum velocity constraint and in the opposite direction of the goal, the initial velocity is not constrained. The goal velocity is not limited to the velocity constraint at all.

To Reproduce

    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(2, 2);
    TrapezoidProfile.State initial = new TrapezoidProfile.State(0, 2); // vary these
    TrapezoidProfile.State goal = new TrapezoidProfile.State(10, 5); // vary these
    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, initial);
    Timer timer = new Timer();

    @Override
    public void robotInit() {
        SmartDashboard.putNumber("Velocity constraint", constraints.maxVelocity);
        SmartDashboard.putNumber("Acceleration constraint", constraints.maxAcceleration);
        SmartDashboard.putNumber("Initial position", initial.position);
        SmartDashboard.putNumber("Initial velocity", initial.velocity);
        SmartDashboard.putNumber("Goal position", goal.position);
        SmartDashboard.putNumber("Goal velocity", goal.velocity);
    }

    @Override
    public void autonomousInit() {
        timer.reset();
        timer.start();
    }

    @Override
    public void autonomousPeriodic() {
        TrapezoidProfile.State state = profile.calculate(timer.get());
        SmartDashboard.putNumber("State position", state.position);
        SmartDashboard.putNumber("State velocity", state.velocity);
    }

    @Override
    public void disabledInit() {
        SmartDashboard.putNumber("State position", 0);
        SmartDashboard.putNumber("State velocity", 0);
    }

Observe the profile's state with the initial +/- velocity magnitude greater than the velocity constraint, as well as with goal +/- velocity magnitude greater than the velocity constraint.

Expected behavior It appears the intention is to limit the profile state velocity to always obey its velocity constraints-- when the profile is constructed in this way, this is not the case. As mentioned in the TrapezoidProfile class doc, the class is not intended to be used directly in this manner; its implementation usually sees an initial/goal velocity of 0, though (like in ProfiledPIDController) it is still possible for the user to set the initial/goal velocities arbitrarily. I would expect the initial/goal velocities to be clamped to the velocity constraint in the profile.

Screenshots The relevant piece of code in the TrapezoidProfile constructor: The expected behavior with positive initial velocity: The unconstrained state velocity with negative initial velocity:

The profile (correctly?) overshoots goal position to reach the goal velocity of the opposite direction, but violates the velocity constraint to do so: The profile incorrectly overshoots the goal position as it waits to reach the goal velocity in the same direction, which is not constrained. This means the profile waits until it is "finished"(also calculated using the unconstrained goal velocity) and simply returns the goal state, which can create large jumps in state position/velocity:

An example simulated mechanism controlled with a ProfiledPIDController when a goal is set with velocity greater than the constraint:

• WPILib Version: 2022.2.1

[calcmogul]

An initial velocity whose magnitude is above the magnitude of the velocity constraint violates the preconditions of TrapezoidProfile. We should document that.