adding python to swerve kin and odom

source/docs/software/kinematics-and-odometry/swerve-drive-kinematics.rst

@@ -6,7 +6,7 @@ The swerve module state class
 -----------------------------
 The ``SwerveModuleState`` class contains information about the velocity and angle of a singular module of a swerve drive. The constructor for a ``SwerveModuleState`` takes in two arguments, the velocity of the wheel on the module, and the angle of the module.
 
-.. note:: In Java, the velocity of the wheel must be in meters per second. In C++, the units library can be used to provide the velocity using any linear velocity unit.
+.. note:: In Java / Python, the velocity of the wheel must be in meters per second. In C++, the units library can be used to provide the velocity using any linear velocity unit.
 .. note:: An angle of 0 corresponds to the modules facing forward.
 
 Constructing the kinematics object
@@ -47,9 +47,27 @@ The locations for the modules must be relative to the center of the robot. Posit
         m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
         m_backRightLocation};
 
+   .. code-block:: python
+
+      # Python requires using the right class for the number of modules you have
+
+      from wpimath.geometry import Translation2d
+      from wpimath.kinematics import SwerveDrive4Kinematics
+
+      # Locations for the swerve drive modules relative to the robot center.
+      m_frontLeftLocation = Translation2d(0.381, 0.381)
+      m_frontRightLocation = Translation2d(0.381, -0.381)
+      m_backLeftLocation = Translation2d(-0.381, 0.381)
+      m_backRightLocation = Translation2d(-0.381, -0.381)
+
+      # Creating my kinematics object using the module locations
+      m_kinematics = SwerveDrive4Kinematics(
+        m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
+      )
+
 Converting chassis speeds to module states
 ------------------------------------------
-The ``toSwerveModuleStates(ChassisSpeeds speeds)`` (Java) / ``ToSwerveModuleStates(ChassisSpeeds speeds)`` (C++) method should be used to convert a ``ChassisSpeeds`` object to a an array of ``SwerveModuleState`` objects. This is useful in situations where you have to convert a forward velocity, sideways velocity, and an angular velocity into individual module states.
+The ``toSwerveModuleStates(ChassisSpeeds speeds)`` (Java / Python) / ``ToSwerveModuleStates(ChassisSpeeds speeds)`` (C++) method should be used to convert a ``ChassisSpeeds`` object to a an array of ``SwerveModuleState`` objects. This is useful in situations where you have to convert a forward velocity, sideways velocity, and an angular velocity into individual module states.
 
 The elements in the array that is returned by this method are the same order in which the kinematics object was constructed. For example, if the kinematics object was constructed with the front left module location, front right module location, back left module location, and the back right module location in that order, the elements in the array would be the front left module state, front right module state, back left module state, and back right module state in that order.
 
@@ -89,6 +107,18 @@ The elements in the array that is returned by this method are the same order in
       // individual SwerveModuleState components.
       auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(speeds);
 
+   .. code-block:: python
+
+      from wpimath.kinematics import ChassisSpeeds
+
+      # Example chassis speeds: 1 meter per second forward, 3 meters
+      # per second to the left, and rotation at 1.5 radians per second
+      # counterclockwise.
+      speeds = ChassisSpeeds(1.0, 3.0, 1.5)
+
+      # Convert to module states
+      frontLeft, frontRight, backLeft, backRight = kinematics.toSwerveModuleStates(speeds)
+
 Module angle optimization
 ^^^^^^^^^^^^^^^^^^^^^^^^^
 The ``SwerveModuleState`` class contains a static ``optimize()`` (Java) / ``Optimize()`` (C++) method that is used to "optimize" the speed and angle setpoint of a given ``SwerveModuleState`` to minimize the change in heading. For example, if the angular setpoint of a certain module from inverse kinematics is 90 degrees, but your current angle is -89 degrees, this method will automatically negate the speed of the module setpoint and make the angular setpoint -90 degrees to reduce the distance the module has to travel.
@@ -106,6 +136,15 @@ This method takes two parameters: the desired state (usually from the ``toSwerve
       auto flOptimized = frc::SwerveModuleState::Optimize(fl,
          units::radian_t(m_turningEncoder.GetDistance()));
 
+   .. code-block:: python
+
+      from wpimath.kinematics import SwerveModuleState
+      from wpimath.geometry import Rotation2d
+
+      frontLeftOptimized = SwerveModuleState.optimize(frontLeft,
+         Rotation2d(m_turningEncoder.getDistance()))
+
+
 Field-oriented drive
 ^^^^^^^^^^^^^^^^^^^^
 :ref:`Recall <docs/software/kinematics-and-odometry/intro-and-chassis-speeds:Creating a ChassisSpeeds object from field-relative speeds>` that a ``ChassisSpeeds`` object can be created from a set of desired field-oriented speeds. This feature can be used to get module states from a set of desired field-oriented speeds.
@@ -138,6 +177,23 @@ Field-oriented drive
       // Now use this in our kinematics
       auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(speeds);
 
+   .. code-block:: python
+
+      from wpimath.kinematics import ChassisSpeeds
+      import math
+      from wpimath.geometry import Rotation2d
+
+      # The desired field relative speed here is 2 meters per second
+      # toward the opponent's alliance station wall, and 2 meters per
+      # second toward the left field boundary. The desired rotation
+      # is a quarter of a rotation per second counterclockwise. The current
+      # robot angle is 45 degrees.
+      speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+        2.0, 2.0, math.pi / 2.0, Rotation2d.fromDegrees(45.0))
+
+      # Now use this in our kinematics
+      moduleStates = kinematics.toSwerveModuleStates(speeds)
+
 Using custom centers of rotation
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Sometimes, rotating around one specific corner might be desirable for certain evasive maneuvers. This type of behavior is also supported by the WPILib classes. The same ``ToSwerveModuleStates()`` method accepts a second parameter for the center of rotation (as a ``Translation2d``). Just like the wheel locations, the ``Translation2d`` representing the center of rotation should be relative to the robot center.
@@ -148,7 +204,7 @@ For example, one can set the center of rotation on a certain module and if the p
 
 Converting module states to chassis speeds
 ------------------------------------------
-One can also use the kinematics object to convert an array of ``SwerveModuleState`` objects to a singular ``ChassisSpeeds`` object. The ``toChassisSpeeds(SwerveModuleState... states)`` (Java) / ``ToChassisSpeeds(SwerveModuleState... states)`` (C++) method can be used to achieve this.
+One can also use the kinematics object to convert an array of ``SwerveModuleState`` objects to a singular ``ChassisSpeeds`` object. The ``toChassisSpeeds(SwerveModuleState... states)`` (Java / Python) / ``ToChassisSpeeds(SwerveModuleState... states)`` (C++) method can be used to achieve this.
 
 .. tab-set-code::
 
@@ -182,3 +238,23 @@ One can also use the kinematics object to convert an array of ``SwerveModuleStat
       // three components.
       auto [forward, sideways, angular] = kinematics.ToChassisSpeeds(
         frontLeftState, frontRightState, backLeftState, backRightState);
+
+   .. code-block:: python
+
+      from wpimath.kinematics import SwerveModuleState
+      from wpimath.geometry import Rotation2d
+
+      # Example module states
+      frontLeftState = SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19))
+      frontRightState = SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81))
+      backLeftState = SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44))
+      backRightState = SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56))
+
+      # Convert to chassis speeds
+      chassisSpeeds = kinematics.toChassisSpeeds(
+        frontLeftState, frontRightState, backLeftState, backRightState)
+
+      # Getting individual speeds
+      double forward = chassisSpeeds.vx
+      double sideways = chassisSpeeds.vy
+      double angular = chassisSpeeds.omega

source/docs/software/kinematics-and-odometry/swerve-drive-odometry.rst

@@ -67,6 +67,40 @@ The fourth optional argument is the starting pose of your robot on the field (as
         m_backLeft.GetPosition(), m_backRight.GetPosition()},
         frc::Pose2d{5_m, 13.5_m, 0_rad}};
 
+  .. code-block:: python
+
+      # Python requires using the right class for the number of modules you have
+      # For both the Kinematics and Odometry classes
+
+      from wpimath.geometry import Translation2d
+      from wpimath.kinematics import SwerveDrive4Kinematics
+      from wpimath.kinematics import SwerveDrive4Odometry
+      from wpimath.geometry import Pose2d
+      from wpimath.geometry import Rotation2d
+
+      # Locations for the swerve drive modules relative to the robot center.
+      m_frontLeftLocation = Translation2d(0.381, 0.381)
+      m_frontRightLocation = Translation2d(0.381, -0.381)
+      m_backLeftLocation = Translation2d(-0.381, 0.381)
+      m_backRightLocation = Translation2d(-0.381, -0.381)
+
+      # Creating my kinematics object using the module locations
+      m_kinematics = SwerveDrive4Kinematics(
+        m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
+      )
+
+      # Creating my odometry object from the kinematics object and the initial wheel positions.
+      # Here, our starting pose is 5 meters along the long end of the field and in the
+      # center of the field along the short end, facing the opposing alliance wall.
+      m_odometry = SwerveDrive4Odometry(
+        m_kinematics, m_gyro.getRotation2d(),
+        (
+          m_frontLeftModule.getPosition(),
+          m_frontRightModule.getPosition(),
+          m_backLeftModule.getPosition(),
+          m_backRightModule.getPosition()
+        ),
+        Pose2d(5.0, 13.5, Rotation2d()))
 
 Updating the robot pose
 -----------------------
@@ -103,14 +137,27 @@ This ``update`` method must be called periodically, preferably in the ``periodic
             m_frontLeftModule.GetPosition(), m_frontRightModule.GetPosition(),
             m_backLeftModule.GetPosition(), m_backRightModule.GetPosition()
           };
+        )
       }
 
+  .. code-block:: python
+
+      def periodic(self):
+        # Get the rotation of the robot from the gyro.
+        gyroAngle = m_gyro.getRotation2d()
+
+        # Update the pose
+        m_pose = m_odometry.update(gyroAngle,
+            m_frontLeftModule.getPosition(), m_frontRightModule.getPosition(),
+            m_backLeftModule.getPosition(), m_backRightModule.getPosition()
+        )
+
 Resetting the Robot Pose
 ------------------------
 The robot pose can be reset via the ``resetPosition`` method. This method accepts three arguments: the current gyro angle, an array of the current module positions (as in the constructor and update method), and the new field-relative pose.
 
 .. important::  If at any time, you decide to reset your gyroscope or wheel encoders, the ``resetPosition`` method MUST be called with the new gyro angle and wheel encoder positions.
 
-.. note:: The implementation of ``getPosition() / GetPosition()`` above is left to the user. The idea is to get the module position (distance and angle) from each module. For a full example, see here: `C++ <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibcExamples/src/main/cpp/examples/SwerveBot>`_ / `Java <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot>`_.
+.. note:: The implementation of ``getPosition() / GetPosition()`` above is left to the user. The idea is to get the module position (distance and angle) from each module. For a full example, see here: `C++ <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibcExamples/src/main/cpp/examples/SwerveBot>`_ / `Java <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot>`_ / `Python <>`
 
-In addition, the ``GetPose`` (C++) / ``getPoseMeters`` (Java) methods can be used to retrieve the current robot pose without an update.
+In addition, the ``GetPose`` (C++) / ``getPoseMeters`` (Java / Python) methods can be used to retrieve the current robot pose without an update.