diff --git a/source/docs/software/kinematics-and-odometry/mecanum-drive-kinematics.rst b/source/docs/software/kinematics-and-odometry/mecanum-drive-kinematics.rst index 3e6472e6c1..f9f2e703c0 100644 --- a/source/docs/software/kinematics-and-odometry/mecanum-drive-kinematics.rst +++ b/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. + 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 wheel locations. + m_kinematics = MecanumDriveKinematics( + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_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 = kinematics.toWheelSpeeds(speeds) + Field-oriented drive ~~~~~~~~~~~~~~~~~~~~ :ref:`Recall ` 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 = 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 = kinematics.toChassisSpeeds(wheelSpeeds) + + # Getting individual speeds + double forward = chassisSpeeds.vx + double sideways = chassisSpeeds.vy + double angular = chassisSpeeds.omega diff --git a/source/docs/software/kinematics-and-odometry/mecanum-drive-odometry.rst b/source/docs/software/kinematics-and-odometry/mecanum-drive-odometry.rst index 004fccc9a5..82cd4eee38 100644 --- a/source/docs/software/kinematics-and-odometry/mecanum-drive-odometry.rst +++ b/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 ` 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 ` 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,39 @@ 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. + 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 wheel locations. + m_kinematics = MecanumDriveKinematics( + 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 = MecanumDriveOdometry( + m_kinematics, + m_gyro.getRotation2d(), + MecanumDriveWheelPositions( + m_frontLeftEncoder.getDistance(), m_frontRightEncoder.getDistance(), + m_backLeftEncoder.getDistance(), m_backRightEncoder.getDistance() + ), + Pose2d(5.0, 13.5, Rotation2d()) + ) + Updating the robot pose ----------------------- @@ -114,12 +147,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( + m_frontLeftEncoder.getDistance(), m_frontRightEncoder.getDistance(), + m_backLeftEncoder.getDistance(), m_backRightEncoder.getDistance()) + + # Get the rotation of the robot from the gyro. + gyroAngle = m_gyro.getRotation2d() + + # Update the pose + m_pose = m_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++ `_ / `Java `_. +.. note:: A full example of a mecanum drive robot with odometry is available here: `C++ `_ / `Java `_ / `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.