Add python documentation to trajectories (#2618)

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

@@ -53,6 +53,31 @@ Users can create their own constraint by implementing the ``TrajectoryConstraint
         // code here
       }
 
+   .. code-block:: python
+
+      from wpimath import units
+      from wpimath.geometry import Pose2d
+      from wpimath.trajectory.constraint import TrajectoryConstraint
+
+
+      class MyConstraint(TrajectoryConstraint):
+          def maxVelocity(
+              self,
+              pose: Pose2d,
+              curvature: units.radians_per_meter,
+              velocity: units.meters_per_second,
+          ) -> units.meters_per_second:
+              ...
+
+          def minMaxAcceleration(
+              self,
+              pose: Pose2d,
+              curvature: units.radians_per_meter,
+              speed: units.meters_per_second,
+          ) -> TrajectoryConstraint.MinMax:
+              ...
+
+
 The ``MaxVelocity`` method should return the maximum allowed velocity for the given pose, curvature, and original velocity of the trajectory without any constraints. The ``MinMaxAcceleration`` method should return the minimum and maximum allowed acceleration for the given pose, curvature, and constrained velocity.
 
 See the source code (`Java <https://github.com/wpilibsuite/allwpilib/tree/main/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint>`_, `C++ <https://github.com/wpilibsuite/allwpilib/tree/main/wpimath/src/main/native/cpp/trajectory/constraint>`_) for the WPILib-provided constraints for more examples on how to write your own custom trajectory constraints.

source/docs/software/advanced-controls/trajectories/examples/trajectory-generation-1/py/ExampleTrajectory.py

@@ -0,0 +1,20 @@
+from wpimath.geometry import Pose2d, Rotation2d, Translation2d
+from wpimath.trajectory import TrajectoryGenerator, TrajectoryConfig
+
+
+def generateTrajectory():
+    # 2018 cross scale auto waypoints.
+    sideStart = Pose2d.fromFeet(1.54, 23.23, Rotation2d.fromDegrees(-180))
+    crossScale = Pose2d.fromFeet(23.7, 6.8, Rotation2d.fromDegrees(-160))
+
+    interiorWaypoints = [
+        Translation2d.fromFeet(14.54, 23.23),
+        Translation2d.fromFeet(21.04, 18.23),
+    ]
+
+    config = TrajectoryConfig.fromFps(12, 12)
+    config.setReversed(True)
+
+    trajectory = TrajectoryGenerator.generateTrajectory(
+        sideStart, interiorWaypoints, crossScale, config
+    )

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

@@ -34,13 +34,33 @@ The final parameter is a ``ProfiledPIDController`` for the rotation of the robot
       // of 1 rotation per second and a max acceleration of 180 degrees
       // per second squared.
 
+   .. code-block:: python
+
+      from wpimath.controller import (
+         HolonomicDriveController,
+         PIDController,
+         ProfiledPIDControllerRadians,
+      )
+      from wpimath.trajectory import TrapezoidProfileRadians
+
+      controller = HolonomicDriveController(
+         PIDController(1, 0, 0),
+         PIDController(1, 0, 0),
+         ProfiledPIDControllerRadians(
+            1, 0, 0, TrapezoidProfileRadians.Constraints(6.28, 3.14)
+         ),
+      )
+      # Here, our rotation profile constraints were a max velocity
+      # of 1 rotation per second and a max acceleration of 180 degrees
+      # per second squared.
+
 Getting Adjusted Velocities
 ---------------------------
 The holonomic drive controller returns "adjusted velocities" such that when the robot tracks these velocities, it accurately reaches the goal point. The controller should be updated periodically with the new goal. The goal is comprised of a desired pose, linear velocity, and heading.
 
 .. note:: The "goal pose" represents the position that the robot should be at a particular timestamp when tracking the trajectory. It does NOT represent the trajectory's endpoint.
 
-The controller can be updated using the ``Calculate`` (C++) / ``calculate`` (Java) method. There are two overloads for this method. Both of these overloads accept the current robot position as the first parameter and the desired heading as the last parameter. For the middle parameters, one overload accepts the desired pose and the linear velocity reference while the other accepts a ``Trajectory.State`` object, which contains information about the goal pose. The latter method is preferred for 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 and the desired heading as the last parameter. For the middle parameters, one overload accepts the desired pose and the linear velocity reference while the other accepts a ``Trajectory.State`` object, which contains information about the goal pose. The latter method is preferred for tracking trajectories.
 
 .. tab-set-code::
    .. code-block:: java
@@ -63,6 +83,19 @@ The controller can be updated using the ``Calculate`` (C++) / ``calculate`` (Jav
       const auto adjustedSpeeds = controller.Calculate(
         currentRobotPose, goal, 70_deg);
 
+   .. code-block:: python
+
+      from wpimath.geometry import Rotation2d
+
+      # Sample the trajectory at 3.4 seconds from the beginning.
+      goal = trajectory.sample(3.4)
+
+      # Get the adjusted speeds. Here, we want the robot to be facing
+      # 70 degrees (in the field-relative coordinate system).
+      adjustedSpeeds = controller.calculate(
+         currentRobotPose, goal, Rotation2d.fromDegrees(70.0)
+      )
+
 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).
@@ -83,4 +116,8 @@ The returned adjusted speeds can be converted into usable speeds using the kinem
 
       auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(adjustedSpeeds);
 
+   .. code-block:: python
+
+      fl, fr, bl, br = kinematics.toSwerveModuleStates(adjustedSpeeds)
+
 Because these swerve module states are still speeds and angles, you will need to use PID controllers to set these speeds and angles.

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

@@ -4,7 +4,7 @@ Once a trajectory has been generated, you can retrieve information from it using
 
 Getting the total duration of the trajectory
 --------------------------------------------
-Because all trajectories have timestamps at each point, the amount of time it should take for a robot to traverse the entire trajectory is pre-determined. The ``TotalTime()`` (C++) / ``getTotalTimeSeconds()`` (Java) method can be used to determine the time it takes to traverse the trajectory.
+Because all trajectories have timestamps at each point, the amount of time it should take for a robot to traverse the entire trajectory is pre-determined. The ``TotalTime()`` (C++) / ``getTotalTimeSeconds()`` (Java) / ``totalTime`` (Python) method can be used to determine the time it takes to traverse the trajectory.
 
 
 .. tab-set-code::
@@ -19,9 +19,14 @@ Because all trajectories have timestamps at each point, the amount of time it sh
       // Get the total time of the trajectory
       units::second_t duration = trajectory.TotalTime();
 
+   .. code-block:: python
+
+      # Get the total time of the trajectory
+      duration = trajectory.totalTime()
+
 Sampling the trajectory
 -----------------------
-The trajectory can be sampled at various timesteps to get the pose, velocity, and acceleration at that point. The ``Sample(units::second_t time)`` (C++) / ``sample(double timeSeconds)`` (Java) method can be used to sample the trajectory at any timestep. The parameter refers to the amount of time passed since 0 seconds (the starting point of the trajectory). This method returns a ``Trajectory::Sample`` with information about that sample point.
+The trajectory can be sampled at various timesteps to get the pose, velocity, and acceleration at that point. The ``Sample(units::second_t time)`` (C++) / ``sample(double timeSeconds)`` (Java/Python) method can be used to sample the trajectory at any timestep. The parameter refers to the amount of time passed since 0 seconds (the starting point of the trajectory). This method returns a ``Trajectory::Sample`` with information about that sample point.
 
 .. tab-set-code::
 
@@ -37,6 +42,12 @@ The trajectory can be sampled at various timesteps to get the pose, velocity, an
       // should be after 1.2 seconds of traversal.
       Trajectory::State point = trajectory.Sample(1.2_s);
 
+   .. code-block:: python
+
+      # Sample the trajectory at 1.2 seconds. This represents where the robot
+      # should be after 1.2 seconds of traversal.
+      point = trajectory.sample(1.2)
+
 The ``Trajectory::Sample`` struct has several pieces of information about the sample point:
 
 * ``t``: The time elapsed from the beginning of the trajectory up to the sample point.
@@ -49,4 +60,4 @@ Note: The angular velocity at the sample point can be calculated by multiplying
 
 Getting all states of the trajectory (advanced)
 -----------------------------------------------
-A more advanced user can get a list of all states of the trajectory by calling the ``States()`` (C++) / ``getStates()`` (Java) method. Each state represents a point on the trajectory. :ref:`When the trajectory is created <docs/software/advanced-controls/trajectories/trajectory-generation:Generating the trajectory>` using the ``TrajectoryGenerator::GenerateTrajectory(...)`` method, a list of trajectory points / states are created. When the user samples the trajectory at a particular timestep, a new sample point is interpolated between two existing points / states in the list.
+A more advanced user can get a list of all states of the trajectory by calling the ``States()`` (C++) / ``getStates()`` (Java) / ``states`` (Python) method. Each state represents a point on the trajectory. :ref:`When the trajectory is created <docs/software/advanced-controls/trajectories/trajectory-generation:Generating the trajectory>` using the ``TrajectoryGenerator::GenerateTrajectory(...)`` method, a list of trajectory points / states are created. When the user samples the trajectory at a particular timestep, a new sample point is interpolated between two existing points / states in the list.

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

@@ -29,13 +29,26 @@ The Ramsete controller should be initialized with two gains, namely ``b`` and ``
       // the user can choose any other gains.
       frc::RamseteController controller2{2.1, 0.8};
 
+   .. code-block:: python
+
+      from wpimath.controller import RamseteController
+
+      # Using the default constructor of RamseteController. Here
+      # the gains are initialized to 2.0 and 0.7.
+      controller1 = RamseteController()
+
+      # Using the secondary constructor of RamseteController where
+      # the user can choose any other gains.
+      controller2 = RamseteController(2.1, 0.8)
+
+
 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.
 
 .. 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.
 
-The controller can be updated using the ``Calculate`` (C++) / ``calculate`` (Java) 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 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.
 
 .. tab-set-code::
 
@@ -50,6 +63,11 @@ The controller can be updated using the ``Calculate`` (C++) / ``calculate`` (Jav
       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);
 
+   .. code-block:: python
+
+      goal = trajectory.sample(3.4)  # sample the trajectory at 3.4 seconds from the beginning
+      adjustedSpeeds = controller.calculate(currentRobotPose, goal)
+
 These calculations should be performed at every loop iteration, with an updated robot position and goal.
 
 Using the Adjusted Velocities
@@ -73,7 +91,14 @@ The returned adjusted speeds can be converted to usable speeds using the kinemat
       DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.ToWheelSpeeds(adjustedSpeeds);
       auto [left, right] = kinematics.ToWheelSpeeds(adjustedSpeeds);
 
-Because these new left and right velocities are still speeds and not voltages, two PID Controllers, one for each side may be used to track these velocities. 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>`_) can be used, or the Velocity PID feature on smart motor controllers such as the TalonSRX and the SPARK MAX can be used.
+   .. code-block:: python
+
+      adjustedSpeeds = controller.calculate(currentRobotPose, goal)
+      wheelSpeeds = kinematics.toWheelSpeeds(adjustedSpeeds)
+      left = wheelSpeeds.left
+      right = wheelSpeeds.right
+
+Because these new left and right velocities are still speeds and not voltages, two PID Controllers, one for each side may be used to track these velocities. 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>`) can be used, or the Velocity PID feature on smart motor controllers such as the TalonSRX and the SPARK MAX can be used.
 
 Ramsete in the Command-Based Framework
 --------------------------------------

source/docs/software/advanced-controls/trajectories/trajectory-generation.rst

@@ -23,10 +23,10 @@ Creating the trajectory config
 ------------------------------
 A configuration must be created in order to generate a trajectory. The config contains information about special constraints, the max velocity, the max acceleration in addition to the start velocity and end velocity. The config also contains information about whether the trajectory should be reversed (robot travels backward along the waypoints). The ``TrajectoryConfig`` class should be used to construct a config. The constructor for this class takes two arguments, the max velocity and max acceleration. The other fields (``startVelocity``, ``endVelocity``, ``reversed``, ``constraints``) are defaulted to reasonable values (``0``, ``0``, ``false``, ``{}``) when the object is created. If you wish to modify the values of any of these fields, you can call the following methods:
 
-* ``setStartVelocity(double startVelocityMetersPerSecond)`` (Java) / ``SetStartVelocity(units::meters_per_second_t startVelocity)`` (C++)
-* ``setEndVelocity(double endVelocityMetersPerSecond)`` (Java) / ``SetEndVelocity(units::meters_per_second_t endVelocity)`` (C++)
-* ``setReversed(boolean reversed)`` (Java) / ``SetReversed(bool reversed)`` (C++)
-* ``addConstraint(TrajectoryConstraint constraint)`` (Java) / ``AddConstraint(TrajectoryConstraint constraint)`` (C++)
+* ``setStartVelocity(double startVelocityMetersPerSecond)`` (Java/Python) / ``SetStartVelocity(units::meters_per_second_t startVelocity)`` (C++)
+* ``setEndVelocity(double endVelocityMetersPerSecond)`` (Java/Python) / ``SetEndVelocity(units::meters_per_second_t endVelocity)`` (C++)
+* ``setReversed(boolean reversed)`` (Java/Python) / ``SetReversed(bool reversed)`` (C++)
+* ``addConstraint(TrajectoryConstraint constraint)`` (Java/Python) / ``AddConstraint(TrajectoryConstraint constraint)`` (C++)
 
 
 .. note:: The ``reversed`` property simply represents whether the robot is traveling backward. If you specify four waypoints, a, b, c, and d, the robot will still travel in the same order through the waypoints when the ``reversed`` flag is set to ``true``. This also means that you must account for the direction of the robot when providing the waypoints. For example, if your robot is facing your alliance station wall and travels backwards to some field element, the starting waypoint should have a rotation of 180 degrees.
@@ -59,14 +59,20 @@ Here is an example of generating a trajectory using clamped cubic splines for th
          :language: c++
          :lines: 8-22
 
+   .. tab-item:: Python
+
+      .. literalinclude:: examples/trajectory-generation-1/py/ExampleTrajectory.py
+         :language: python
+         :lines: 5-20
+
 .. note:: The Java code utilizes the `Units <https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/util/Units.html>`_ utility, for easy unit conversions.
 
 .. note:: Generating a typical trajectory takes about 10 ms to 25 ms. This isn't long, but it's still highly recommended to generate all trajectories on startup (``robotInit``).
 
 Concatenating Trajectories
 --------------------------
 
-Trajectories in Java can be combined into a single trajectory using the ``concatenate(trajectory)`` function. C++ users can simply add (``+``) the two trajectories together.
+Trajectories in Java can be combined into a single trajectory using the ``concatenate(trajectory)`` function. C++/Python users can simply add (``+``) the two trajectories together.
 
 .. warning:: It is up to the user to ensure that the end of the initial and start of the appended trajectory match. It is also the user's responsibility to ensure that the start and end velocities of their trajectories match.
 
@@ -103,3 +109,26 @@ Trajectories in Java can be combined into a single trajectory using the ``concat
          frc::Pose2d(6_m, 0_m, 0_rad), frc::TrajectoryConfig(3_fps, 3_fps_sq));
 
       auto concatTraj = m_trajectoryOne + m_trajectoryTwo;
+
+   .. code-block:: python
+
+      from wpimath.geometry import Pose2d, Rotation2d, Translation2d
+      from wpimath.trajectory import TrajectoryGenerator, TrajectoryConfig
+
+
+      trajectoryOne = TrajectoryGenerator.generateTrajectory(
+         Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+         [Translation2d(1, 1), Translation2d(2, -1)],
+         Pose2d(3, 0, Rotation2d.fromDegrees(0)),
+         TrajectoryConfig.fromFps(3.0, 3.0),
+      )
+
+      trajectoryTwo = TrajectoryGenerator.generateTrajectory(
+         Pose2d(3, 0, Rotation2d.fromDegrees(0)),
+         [Translation2d(4, 4), Translation2d(6, 3)],
+         Pose2d(6, 0, Rotation2d.fromDegrees(0)),
+         TrajectoryConfig.fromFps(3.0, 3.0),
+      )
+
+      concatTraj = trajectoryOne + trajectoryTwo
+