add python for kinematics and odometry

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

@@ -6,11 +6,11 @@ Constructing the Kinematics Object
 ----------------------------------
 The ``DifferentialDriveKinematics`` object accepts one constructor argument, which is the track width of the robot. This represents the distance between the two sets of wheels on a differential drive.
 
-.. note:: In Java, the track width must be in meters. In C++, the units library can be used to pass in the track width using any length unit.
+.. note:: In Java and Python, the track width must be in meters. In C++, the units library can be used to pass in the track width using any length unit.
 
 Converting Chassis Speeds to Wheel Speeds
 -----------------------------------------
-The ``toWheelSpeeds(ChassisSpeeds speeds)`` (Java) / ``ToWheelSpeeds(ChassisSpeeds speeds)`` (C++) method should be used to convert a ``ChassisSpeeds`` object to a ``DifferentialDriveWheelSpeeds`` object. This is useful in situations where you have to convert a linear velocity (``vx``) and an angular velocity (``omega``) to left and right wheel velocities.
+The ``toWheelSpeeds(ChassisSpeeds speeds)`` (Java / Python) / ``ToWheelSpeeds(ChassisSpeeds speeds)`` (C++) method should be used to convert a ``ChassisSpeeds`` object to a ``DifferentialDriveWheelSpeeds`` object. This is useful in situations where you have to convert a linear velocity (``vx``) and an angular velocity (``omega``) to left and right wheel velocities.
 
 .. tab-set-code::
 
@@ -47,9 +47,30 @@ The ``toWheelSpeeds(ChassisSpeeds speeds)`` (Java) / ``ToWheelSpeeds(ChassisSpee
       // struct into left and right velocities.
       auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
 
+   .. code-block:: python
+
+      from wpimath.kinematics import DifferentialDriveKinematics
+      from wpimath.kinematics import ChassisSpeeds
+      from wpimath.units import inchesToMeters
+
+      # Creating my kinematics object: track width of 27 inches
+      kinematics = DifferentialDriveKinematics(Units.inchesToMeters(27.0))
+
+      # Example chassis speeds: 2 meters per second linear velocity,
+      # 1 radian per second angular velocity.
+      chassisSpeeds = ChassisSpeeds(2.0, 0, 1.0)
+
+      # Convert to wheel speeds
+      wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds)
+
+      # Left velocity
+      leftVelocity = wheelSpeeds.left
+      # Right velocity
+      rightVelocity = wheelSpeeds.right
+
 Converting Wheel Speeds to Chassis Speeds
 -----------------------------------------
-One can also use the kinematics object to convert individual wheel speeds (left and right) to a singular ``ChassisSpeeds`` object. The ``toChassisSpeeds(DifferentialDriveWheelSpeeds speeds)`` (Java) / ``ToChassisSpeeds(DifferentialDriveWheelSpeeds speeds)`` (C++) method should be used to achieve this.
+One can also use the kinematics object to convert individual wheel speeds (left and right) to a singular ``ChassisSpeeds`` object. The ``toChassisSpeeds(DifferentialDriveWheelSpeeds speeds)`` (Java / Python) / ``ToChassisSpeeds(DifferentialDriveWheelSpeeds speeds)`` (C++) method should be used to achieve this.
 
 .. tab-set-code::
 
@@ -86,3 +107,25 @@ One can also use the kinematics object to convert individual wheel speeds (left
       // Note that because a differential drive is non-holonomic, the vy variable
       // will be equal to zero.
       auto [linearVelocity, vy, angularVelocity] = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+   .. code-block:: python
+
+      from wpimath.kinematics import DifferentialDriveKinematics
+      from wpimath.kinematics import DifferentialDriveWheelSpeeds
+      from wpimath.units import inchesToMeters
+
+      # Creating my kinematics object: track width of 27 inches
+      kinematics = DifferentialDriveKinematics(inchesToMeters(27.0))
+
+      # Example differential drive wheel speeds: 2 meters per second
+      # for the left side, 3 meters per second for the right side.
+      wheelSpeeds = DifferentialDriveWheelSpeeds(2.0, 3.0)
+
+      # Convert to chassis speeds.
+      chassisSpeeds = kinematics.toChassisSpeeds(wheelSpeeds)
+
+      # Linear velocity
+      linearVelocity = chassisSpeeds.vx
+
+      # Angular velocity
+      angularVelocity = chassisSpeeds.omega

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

@@ -9,7 +9,7 @@ Creating the Odometry Object
 The ``DifferentialDriveOdometry`` class constructor requires three mandatory arguments and one optional argument. The mandatory arguments are:
 
 * The angle reported by your gyroscope (as a ``Rotation2d``)
-* The initial left and right encoder readings. In Java, these are each a ``double``, and must represent the distance traveled by each side in meters.  In C++, the :doc:`units library </docs/software/basic-programming/cpp-units>` must be used to represent your wheel positions.
+* The initial left and right encoder readings. In Java / Python, these are each a ``double``, and must represent the distance traveled by each side in meters.  In C++, the :doc:`units library </docs/software/basic-programming/cpp-units>` must be used to represent your wheel positions.
 
 The optional argument is the starting pose of your robot on the field (as a ``Pose2d``). By default, the robot will start at ``x = 0, y = 0, theta = 0``.
 
@@ -38,6 +38,20 @@ The optional argument is the starting pose of your robot on the field (as a ``Po
         units::meter_t{m_rightEncoder.GetDistance()},
         frc::Pose2d{5_m, 13.5_m, 0_rad}};
 
+   .. code-block:: python
+
+      from wpimath.kinematics import DifferentialDriveOdometry
+      from wpimath.odometry import Pose2d
+      from wpimath.odometry import Rotation2d
+
+      # Creating my odometry object. 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 forward.
+      m_odometry = DifferentialDriveOdometry(
+        m_gyro.getRotation2d(),
+        m_leftEncoder.getDistance(), m_rightEncoder.getDistance(),
+        Pose2d(5.0, 13.5, Rotation2d()))
+
 
 Updating the Robot Pose
 -----------------------
@@ -72,12 +86,23 @@ The ``update`` method can be used to update the robot's position on the field. T
           units::meter_t{m_rightEncoder.GetDistance()});
       }
 
+   .. 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_leftEncoder.getDistance(),
+          m_rightEncoder.getDistance())
+
 Resetting the Robot Pose
 ------------------------
 The robot pose can be reset via the ``resetPosition`` method. This method accepts four arguments: the current gyro angle, the left and right wheel positions, and the new field-relative pose.
 
 .. important:: If at any time, you decide to reset your gyroscope or encoders, the ``resetPosition`` method MUST be called with the new gyro angle and wheel distances.
 
-.. note:: A full example of a differential drive robot with odometry is available here: `C++ <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot>`_ / `Java <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot>`_.
+.. note:: A full example of a differential drive robot with odometry is available here: `C++ <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot>`_ / `Java <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot>`_ / `Python <https://github.com/robotpy/examples/tree/main/differential-drive-bot>`_
 
-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.