Add Python to mecanum kinematics and odometry (#2435)

* add python to mecanum kin and odom

* address review

* more review items

* fixed spacing and link

Author: jasondaming

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

@@ -21,7 +21,6 @@ The ``MecanumDriveKinematics`` class accepts four constructor arguments, with ea
         m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
       );
 
-
    .. code-block:: c++
 
       // Locations of the wheels relative to the robot center.
@@ -35,9 +34,25 @@ The ``MecanumDriveKinematics`` class accepts four constructor arguments, with ea
         m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
         m_backRightLocation};
 
+   .. code-block:: python
+
+      from wpimath.geometry import Translation2d
+      from wpimath.kinematics import MecanumDriveKinematics
+
+      # Locations of the wheels relative to the robot center.
+      frontLeftLocation = Translation2d(0.381, 0.381)
+      frontRightLocation = Translation2d(0.381, -0.381)
+      backLeftLocation = Translation2d(-0.381, 0.381)
+      backRightLocation = Translation2d(-0.381, -0.381)
+
+      # Creating my kinematics object using the wheel locations.
+      self.kinematics = MecanumDriveKinematics(
+        frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation
+      )
+
 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 ``MecanumDriveWheelSpeeds`` object. This is useful in situations where you have to convert a forward velocity, sideways velocity, and an angular velocity into individual wheel speeds.
+The ``toWheelSpeeds(ChassisSpeeds speeds)`` (Java / Python) / ``ToWheelSpeeds(ChassisSpeeds speeds)`` (C++) method should be used to convert a ``ChassisSpeeds`` object to a ``MecanumDriveWheelSpeeds`` object. This is useful in situations where you have to convert a forward velocity, sideways velocity, and an angular velocity into individual wheel speeds.
 
 .. tab-set-code::
 
@@ -69,6 +84,18 @@ The ``toWheelSpeeds(ChassisSpeeds speeds)`` (Java) / ``ToWheelSpeeds(ChassisSpee
       // struct into it's individual components
       auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(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 wheel speeds
+      frontLeft, frontRight, backLeft, backRight = self.kinematics.toWheelSpeeds(speeds)
+
 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 wheel speeds from a set of desired field-oriented speeds.
@@ -101,6 +128,23 @@ Field-oriented drive
       // Now use this in our kinematics
       auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(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
+      wheelSpeeds = self.kinematics.toWheelSpeeds(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 ``ToWheelSpeeds()`` 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.
@@ -111,7 +155,7 @@ For example, one can set the center of rotation on a certain wheel and if the pr
 
 Converting wheel speeds to chassis speeds
 -----------------------------------------
-One can also use the kinematics object to convert a ``MecanumDriveWheelSpeeds`` object to a singular ``ChassisSpeeds`` object. The ``toChassisSpeeds(MecanumDriveWheelSpeeds speeds)`` (Java) / ``ToChassisSpeeds(MecanumDriveWheelSpeeds speeds)`` (C++) method can be used to achieve this.
+One can also use the kinematics object to convert a ``MecanumDriveWheelSpeeds`` object to a singular ``ChassisSpeeds`` object. The ``toChassisSpeeds(MecanumDriveWheelSpeeds speeds)`` (Java / Python) / ``ToChassisSpeeds(MecanumDriveWheelSpeeds speeds)`` (C++) method can be used to achieve this.
 
 .. tab-set-code::
 
@@ -137,3 +181,18 @@ One can also use the kinematics object to convert a ``MecanumDriveWheelSpeeds``
       // feature to automatically break up the ChassisSpeeds struct into its
       // three components.
       auto [forward, sideways, angular] = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+   .. code-block:: python
+
+      from wpimath.kinematics import MecanumDriveWheelSpeeds
+
+      # Example wheel speeds
+      wheelSpeeds = MecanumDriveWheelSpeeds(-17.67, 20.51, -13.44, 16.26)
+
+      # Convert to chassis speeds
+      chassisSpeeds = self.kinematics.toChassisSpeeds(wheelSpeeds)
+
+      # Getting individual speeds
+      forward = chassisSpeeds.vx
+      sideways = chassisSpeeds.vy
+      angular = chassisSpeeds.omega

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

@@ -12,7 +12,7 @@ The mandatory arguments are:
 
 * The kinematics object that represents your mecanum drive (as a ``MecanumDriveKinematics`` instance)
 * The angle reported by your gyroscope (as a ``Rotation2d``)
-* The initial positions of the wheels (as ``MecanumDriveWheelPositions``). In Java, this must be constructed with each wheel position in meters. In C++, the :doc:`units library </docs/software/basic-programming/cpp-units>` must be used to represent your wheel positions.
+* The initial positions of the wheels (as ``MecanumDriveWheelPositions``). In Java / Python, this must be constructed with each wheel position in meters. In C++, the :doc:`units library </docs/software/basic-programming/cpp-units>` must be used to represent your wheel positions.
 
 The fourth 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``.
 
@@ -74,6 +74,38 @@ The fourth optional argument is the starting pose of your robot on the field (as
         },
         frc::Pose2d{5_m, 13.5_m, 0_rad}};
 
+   .. code-block:: python
+
+      from wpimath.geometry import Translation2d
+      from wpimath.kinematics import MecanumDriveKinematics
+      from wpimath.kinematics import MecanumDriveOdometry
+      from wpimath.kinematics import MecanumDriveWheelPositions
+      from wpimath.geometry import Pose2d
+      from wpimath.geometry import Rotation2d
+
+      # Locations of the wheels relative to the robot center.
+      frontLeftLocation = Translation2d(0.381, 0.381)
+      frontRightLocation = Translation2d(0.381, -0.381)
+      backLeftLocation = Translation2d(-0.381, 0.381)
+      backRightLocation = Translation2d(-0.381, -0.381)
+
+      # Creating my kinematics object using the wheel locations.
+      self.kinematics = MecanumDriveKinematics(
+        frontLeftLocation, frontRightLocation, backLeftLocation, 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.
+      self.odometry = MecanumDriveOdometry(
+        self.kinematics,
+        self.gyro.getRotation2d(),
+        MecanumDriveWheelPositions(
+          self.frontLeftEncoder.getDistance(), self.frontRightEncoder.getDistance(),
+          self.backLeftEncoder.getDistance(), self.backRightEncoder.getDistance()
+        ),
+        Pose2d(5.0, 13.5, Rotation2d())
+      )
 
 Updating the robot pose
 -----------------------
@@ -114,12 +146,28 @@ The ``update`` method of the odometry class updates the robot position on the fi
         m_pose = m_odometry.Update(gyroAngle, wheelPositions);
       }
 
+   .. code-block:: python
+
+      from wpimath.kinematics import MecanumDriveWheelPositions
+
+      def periodic(self):
+        # Get my wheel positions
+        wheelPositions = MecanumDriveWheelPositions(
+          self.frontLeftEncoder.getDistance(), self.frontRightEncoder.getDistance(),
+          self.backLeftEncoder.getDistance(), self.backRightEncoder.getDistance())
+
+        # Get the rotation of the robot from the gyro.
+        gyroAngle = gyro.getRotation2d()
+
+        # Update the pose
+        self.pose = odometry.update(gyroAngle, wheelPositions)
+
 Resetting the Robot Pose
 ------------------------
 The robot pose can be reset via the ``resetPosition`` method. This method accepts three arguments: the current gyro angle, the current 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 positions.
 
-.. note:: A full example of a mecanum drive robot with odometry is available here: `C++ <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibcExamples/src/main/cpp/examples/MecanumBot>`_ / `Java <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot>`_.
+.. note:: A full example of a mecanum drive robot with odometry is available here: `C++ <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibcExamples/src/main/cpp/examples/MecanumBot>`_ / `Java <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot>`_ / `Python <https://github.com/robotpy/examples/tree/main/MecanumBot>`_
 
-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.