Add documentation for LTV Unicycle Controller (#2792)

* Add documentation for  LTV Unicycle Controller

RamseteController was deprecated in wpilibsuite/allwpilib#6494
Does 1 part of #2651

Signed-off-by: Jade Turner <[email protected]>

* Restore ramsete doc, add deprecation warning

* Add deprecation notice to trajectory tutorial

---------

Signed-off-by: Jade Turner <[email protected]>
Co-authored-by: sciencewhiz <[email protected]>
Co-authored-by: Tyler Veness <[email protected]>

source/docs/software/advanced-controls/trajectories/index.rst

@@ -11,6 +11,7 @@ This section describes WPILib support for generating parameterized spline trajec
    constraints
    manipulating-trajectories
    transforming-trajectories
+   ltv
    ramsete
    holonomic
    troubleshooting

source/docs/software/advanced-controls/trajectories/ltv.rst

@@ -0,0 +1,81 @@
+# LTV Unicycle Controller
+The LTV Unicycle Controller ([C++](https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_l_t_v_unicycle_controller.html), [Java](https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/LTVUnicycleController.html), :external:py:class:`Python <wpimath.controller.LTVUnicycleController>`) is a trajectory tracker that is built in to WPILib. This tracker can be used to accurately track trajectories with correction for minor disturbances for differential drivetrains.
+
+.. note:: LTV has replaced RAMSETE, because it has more intuitive tuning.
+
+## Constructing the LTV Unicycle Controller Object
+
+The LTV Unicycle controller should be initialized with four parameters. `qelems` is a vector of the maximum desired error tolerance in the X and Y directions and heading. `relems` is a vector of the desired control effort in linear velocity and angular velocity. `dt` represents the timestep used in calculations (the default loop rate of 20 ms is a reasonable value) and `maxVelocity` should be the max velocity your robot can achieve. See :ref:`The State Space control LQR Tuning <docs/software/advanced-controls/state-space/state-space-intro:LQR: tuning>` for more information on the effect of `qelems` and `relems` on the controller.
+
+The code example below initializes the LTV Unicycle Controller with `qelems` of 0.0625 m in X, 0.125 m in Y, and 2 radians in heading; `relems` of 1 m/s of linear velocity, and 2 rad/sec angular velocity; `dt` of 20 ms; and `maxVelocity` of 9 m/s.
+
+.. note:: The maximum desired error of the heading is much larger then X and Y to ensure that the heading controller doesn't overpower the Y controller.
+
+.. tab-set-code::
+   ```java
+   LTVUnicycleController controller = new LTVUnicycleController(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), 0.02, 9);
+   ```
+
+   ```c++
+   frc::LTVUnicycleController controller{{0.0625, 0.125, 2.0}, {1.0, 2.0}, 0.02_s, 9_mps};
+   ```
+
+   ```python
+   controller = LTVUnicycleController([0.0625, 0.125, 2.0], [1.0, 2.0], 0.02, 9)
+   ```
+
+## Getting Velocity Commands
+The LTV Unicycle controller returns linear and angular velocity commands which, if followed, will make the robot accurately reach a goal state consisting of a desired pose, desired linear velocity, and desired angular velocity.
+
+.. note:: The "reference pose" represents the position that the robot should be at a particular timestep when tracking the trajectory. It does NOT represent the final endpoint of the trajectory, which is the goal.
+
+The controller can be updated using the ``Calculate`` (C++) / ``calculate`` (Java/Python) method. The first overload of this method accepts the current robot pose, desired robot pose, desired linear velocity, and desired angular velocity. The second overload accepts the current robot pose and desired pose, linear velocity, and angular velocity as a ``Trajectory.State`` object. This overload is ideal for those using the WPILib trajectory API.
+
+.. tab-set-code::
+
+   ```java
+   Trajectory.State reference = trajectory.sample(3.4); // sample the trajectory at 3.4 seconds from the beginning
+   ChassisSpeeds adjustedSpeeds = controller.calculate(currentRobotPose, reference);
+   ```
+
+   ```c++
+   const Trajectory::State reference = trajectory.Sample(3.4_s); // sample the trajectory at 3.4 seconds from the beginning
+   ChassisSpeeds adjustedSpeeds = controller.Calculate(currentRobotPose, reference);
+   ```
+
+   ```python
+   reference = trajectory.sample(3.4)  # sample the trajectory at 3.4 seconds from the beginning
+   adjustedSpeeds = controller.calculate(currentRobotPose, reference)
+   ```
+
+These calculations should be performed at every loop iteration, with an updated robot position and reference.
+
+## Using the Velocity Commands
+The velocity commands are of type ``ChassisSpeeds``, which contains a ``vx`` (linear velocity in the forward direction), a ``vy`` (linear velocity in the sideways direction), and an ``omega`` (angular velocity around the center of the robot frame).
+
+The returned adjusted speeds can be converted to usable speeds using the kinematics classes for your drivetrain type. For example, the adjusted velocities can be converted to left and right velocities for a differential drive using a ``DifferentialDriveKinematics`` object.
+
+.. tab-set-code::
+
+   ```java
+   ChassisSpeeds adjustedSpeeds = controller.calculate(currentRobotPose, reference);
+   DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(adjustedSpeeds);
+   double left = wheelSpeeds.leftMetersPerSecond;
+   double right = wheelSpeeds.rightMetersPerSecond;
+   ```
+
+   ```c++
+   ChassisSpeeds adjustedSpeeds = controller.Calculate(currentRobotPose, reference);
+   DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.ToWheelSpeeds(adjustedSpeeds);
+   auto [left, right] = kinematics.ToWheelSpeeds(adjustedSpeeds);
+   ```
+
+   ```python
+   adjustedSpeeds = controller.calculate(currentRobotPose, reference)
+   wheelSpeeds = kinematics.toWheelSpeeds(adjustedSpeeds)
+   left = wheelSpeeds.left
+   right = wheelSpeeds.right
+   ```
+
+These new left and right velocities are still speeds and not voltages, so two PID Controllers, one for each side, should be used to track them. You can use either the WPILib PIDController ([C++](https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_p_i_d_controller.html), [Java](https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/PIDController.html), :external:py:class:`Python <wpimath.controller.PIDController>`) or the Velocity PID feature on smart motor controllers such as the TalonSRX and the SPARK MAX.
+

source/docs/software/advanced-controls/trajectories/ramsete.rst

@@ -1,5 +1,8 @@
 # Ramsete Controller
-The Ramsete Controller is a trajectory tracker that is built in to WPILib. This tracker can be used to accurately track trajectories with correction for minor disturbances.
+
+.. warning:: Ramsete Controller has been :term:`deprecated`. Use :doc:`LTV Unicycle Controller <ltv>` which has more intuitive tuning.
+
+The Ramsete Controller is a trajectory tracker that is built in to WPILib. This tracker can be used to accurately track trajectories with correction for minor disturbances for differential drivetrains.
 
 ## Constructing the Ramsete Controller Object
 The Ramsete controller should be initialized with two gains, namely ``b`` and ``zeta``. Larger values of ``b`` make convergence more aggressive like a proportional term whereas larger values of ``zeta`` provide more damping in the response. These controller gains only dictate how the controller will output adjusted velocities. It does NOT affect the actual velocity tracking of the robot. This means that these controller gains are generally robot-agnostic.
@@ -36,30 +39,30 @@ The Ramsete controller should be initialized with two gains, namely ``b`` and ``
    ```
 
 ## Getting Adjusted Velocities
-The Ramsete controller returns "adjusted velocities" so that the when the robot tracks these velocities, it accurately reaches the goal point. The controller should be updated periodically with the new goal. The goal comprises of a desired pose, desired linear velocity, and desired angular velocity. Furthermore, the current position of the robot should also be updated periodically. The controller uses these four arguments to return the adjusted linear and angular velocity. Users should command their robot to these linear and angular velocities to achieve optimal trajectory tracking.
+The Ramsete controller returns "adjusted velocities" so that the when the robot tracks these velocities, it accurately reaches the goal point. The controller should be updated periodically with the new reference, which is where it should be at the current time. The reference comprises of a desired pose, desired linear velocity, and desired angular velocity. Furthermore, the current position of the robot should also be updated periodically. The controller uses these four arguments to return the adjusted linear and angular velocity. Users should command their robot to these linear and angular velocities to achieve optimal trajectory tracking.
 
-.. note:: The "goal pose" represents the position that the robot should be at a particular timestep when tracking the trajectory. It does NOT represent the final endpoint of the trajectory.
+.. note:: The "reference pose" represents the position that the robot should be at a particular timestep when tracking the trajectory. It does NOT represent the final endpoint of the trajectory, which is the goal.
 
-The controller can be updated using the ``Calculate`` (C++) / ``calculate`` (Java/Python) method. There are two overloads for this method. Both of these overloads accept the current robot position as the first parameter. For the other parameters, one of these overloads takes in the goal as three separate parameters (pose, linear velocity, and angular velocity) whereas the other overload accepts a ``Trajectory.State`` object, which contains information about the goal pose. For its ease, users should use the latter method when tracking trajectories.
+The controller can be updated using the ``Calculate`` (C++) / ``calculate`` (Java/Python) method. There are two overloads for this method. Both of these overloads accept the current robot position as the first parameter. For the other parameters, one of these overloads takes in the reference as three separate parameters (pose, linear velocity, and angular velocity) whereas the other overload accepts a ``Trajectory.State`` object, which contains information about the reference pose. For its ease, users should use the latter method when tracking trajectories.
 
 .. tab-set-code::
 
    ```java
-   Trajectory.State goal = trajectory.sample(3.4); // sample the trajectory at 3.4 seconds from the beginning
-   ChassisSpeeds adjustedSpeeds = controller.calculate(currentRobotPose, goal);
+   Trajectory.State reference = trajectory.sample(3.4); // sample the trajectory at 3.4 seconds from the beginning
+   ChassisSpeeds adjustedSpeeds = controller.calculate(currentRobotPose, reference);
    ```
 
    ```c++
-   const Trajectory::State goal = trajectory.Sample(3.4_s); // sample the trajectory at 3.4 seconds from the beginning
-   ChassisSpeeds adjustedSpeeds = controller.Calculate(currentRobotPose, goal);
+   const Trajectory::State reference = trajectory.Sample(3.4_s); // sample the trajectory at 3.4 seconds from the beginning
+   ChassisSpeeds adjustedSpeeds = controller.Calculate(currentRobotPose, reference);
    ```
 
    ```python
-   goal = trajectory.sample(3.4)  # sample the trajectory at 3.4 seconds from the beginning
-   adjustedSpeeds = controller.calculate(currentRobotPose, goal)
+   reference = trajectory.sample(3.4)  # sample the trajectory at 3.4 seconds from the beginning
+   adjustedSpeeds = controller.calculate(currentRobotPose, reference)
    ```
 
-These calculations should be performed at every loop iteration, with an updated robot position and goal.
+These calculations should be performed at every loop iteration, with an updated robot position and reference.
 
 ## Using the Adjusted Velocities
 The adjusted velocities are of type ``ChassisSpeeds``, which contains a ``vx`` (linear velocity in the forward direction), a ``vy`` (linear velocity in the sideways direction), and an ``omega`` (angular velocity around the center of the robot frame). Because the Ramsete controller is a controller for non-holonomic robots (robots which cannot move sideways), the adjusted speeds object has a ``vy`` of zero.
@@ -69,20 +72,20 @@ The returned adjusted speeds can be converted to usable speeds using the kinemat
 .. tab-set-code::
 
    ```java
-   ChassisSpeeds adjustedSpeeds = controller.calculate(currentRobotPose, goal);
+   ChassisSpeeds adjustedSpeeds = controller.calculate(currentRobotPose, reference);
    DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(adjustedSpeeds);
    double left = wheelSpeeds.leftMetersPerSecond;
    double right = wheelSpeeds.rightMetersPerSecond;
    ```
 
    ```c++
-   ChassisSpeeds adjustedSpeeds = controller.Calculate(currentRobotPose, goal);
+   ChassisSpeeds adjustedSpeeds = controller.Calculate(currentRobotPose, reference);
    DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.ToWheelSpeeds(adjustedSpeeds);
    auto [left, right] = kinematics.ToWheelSpeeds(adjustedSpeeds);
    ```
 
    ```python
-   adjustedSpeeds = controller.calculate(currentRobotPose, goal)
+   adjustedSpeeds = controller.calculate(currentRobotPose, reference)
    wheelSpeeds = kinematics.toWheelSpeeds(adjustedSpeeds)
    left = wheelSpeeds.left
    right = wheelSpeeds.right

source/docs/software/pathplanning/index.rst

@@ -1,6 +1,6 @@
 # Path Planning
 
-Path Planning is the process of creating and following trajectories. These paths use the WPILib trajectory APIs for generation and a :ref:`Ramsete Controller <docs/software/advanced-controls/trajectories/ramsete:Ramsete Controller>` for following. This section highlights the process of characterizing your robot for system identification, trajectory following and usage of PathWeaver. Users may also want to read the :ref:`generic trajectory following documents <docs/software/advanced-controls/trajectories/index:Trajectory Generation and Following with WPILib>` for additional information about the API and non-commandbased usage.
+Path Planning is the process of creating and following trajectories. These paths use the WPILib trajectory APIs for generation and a :ref:`LTV Unicycle Controller <docs/software/advanced-controls/trajectories/ltv:LTV Unicycle Controller>` for following. This section highlights the process of characterizing your robot for system identification, trajectory following and usage of PathWeaver. Users may also want to read the :ref:`generic trajectory following documents <docs/software/advanced-controls/trajectories/index:Trajectory Generation and Following with WPILib>` for additional information about the API and non-commandbased usage.
 
 ## Notice on Swerve Support
 

source/docs/software/pathplanning/pathweaver/introduction.rst

@@ -10,4 +10,4 @@ A more advanced approach to autonomous is called "path planning". Instead of dri
 
 WPILib contains a trajectory generation suite that can be used by teams to generate and follow trajectories. This series of articles will go over how to generate and visualize trajectories using PathWeaver. For a comprehensive tutorial on following trajectories, please visit the :ref:`end-to-end trajectory tutorial <docs/software/pathplanning/trajectory-tutorial/index:Trajectory Tutorial>`.
 
-.. note:: :ref:`Trajectory following <docs/software/advanced-controls/trajectories/ramsete:Ramsete Controller>` code is required to use PathWeaver.  We recommend that you start with Trajectory following and get that working with simple paths.  From there you can continue on to testing more complicated paths generated by PathWeaver.
+.. note:: :doc:`Trajectory following </docs/software/advanced-controls/trajectories/ltv>` code is required to use PathWeaver.  We recommend that you start with Trajectory following and get that working with simple paths.  From there you can continue on to testing more complicated paths generated by PathWeaver.

source/docs/software/pathplanning/trajectory-tutorial/entering-constants.rst

@@ -60,7 +60,11 @@ We must also decide on a nominal max acceleration and max velocity for the robot
 
 ## Ramsete Parameters
 
-Finally, we must include a pair of parameters for the RAMSETE controller.  The values shown below should work well for most robots, provided distances have been correctly measured in meters - for more information on tuning these values (if it is required), see :ref:`docs/software/advanced-controls/trajectories/ramsete:Constructing the Ramsete Controller Object`.
+Finally, we must include a pair of parameters for the RAMSETE controller.  The values ``b`` and ``zeta`` shown below should work well for most robots, provided distances have been correctly measured in meters.
+
+Larger values of ``b`` make convergence more aggressive like a proportional term whereas larger values of ``zeta`` provide more damping in the response. These controller gains only dictate how the controller will output adjusted velocities. It does NOT affect the actual velocity tracking of the robot. This means that these controller gains are generally robot-agnostic.
+
+.. note:: Gains of ``2.0`` and ``0.7`` for ``b`` and ``zeta`` have been tested repeatedly to produce desirable results when all units were in meters. As such, a zero-argument constructor for ``RamseteController`` exists with gains defaulted to these values.
 
 .. tab-set-code::
 

source/docs/software/pathplanning/trajectory-tutorial/trajectory-tutorial-overview.rst

@@ -2,7 +2,7 @@
 
 # Trajectory Tutorial Overview
 
-.. todo:: add pathweaver stuff once it is available
+.. warning:: This tutorial uses the Ramsete Controller which has been :term:`deprecated`. The replacement is :doc:`LTV Unicycle Controller </docs/software/advanced-controls/trajectories/ltv>` which has more intuitive tuning. We are looking for volunteers to [update this tutorial](https://github.com/wpilibsuite/frc-docs/issues/2651).
 
 .. note:: Before following this tutorial, it is helpful (but not strictly necessary) to have a baseline familiarity with WPILib's :ref:`PID control <docs/software/advanced-controls/controllers/pidcontroller:PID Control in WPILib>`, :ref:`feedforward <docs/software/advanced-controls/controllers/feedforward:Feedforward Control in WPILib>`, and :ref:`trajectory <docs/software/advanced-controls/trajectories/index:Trajectory Generation and Following with WPILib>` features.