Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module states (velocity and angle).
More...
|
template<std::convertible_to< Translation2d >... ModuleTranslations>
requires (sizeof...(ModuleTranslations) == NumModules) |
| | SwerveDriveKinematics (ModuleTranslations &&... moduleTranslations) |
| | Constructs a swerve drive kinematics object.
|
| | SwerveDriveKinematics (const wpi::util::array< Translation2d, NumModules > &modules) |
| | SwerveDriveKinematics (const SwerveDriveKinematics &)=default |
template<std::convertible_to< Rotation2d >... ModuleHeadings>
requires (sizeof...(ModuleHeadings) == NumModules) |
| void | ResetHeadings (ModuleHeadings &&... moduleHeadings) |
| | Reset the internal swerve module headings.
|
| void | ResetHeadings (wpi::util::array< Rotation2d, NumModules > moduleHeadings) |
| | Reset the internal swerve module headings.
|
| wpi::util::array< SwerveModuleVelocity, NumModules > | ToSwerveModuleVelocities (const ChassisVelocities &chassisVelocities, const Translation2d ¢erOfRotation=Translation2d{}) const |
| | Performs inverse kinematics to return the module states from a desired chassis velocity.
|
| wpi::util::array< SwerveModuleVelocity, NumModules > | ToWheelVelocities (const ChassisVelocities &chassisVelocities) const override |
| | Performs inverse kinematics to return the wheel velocities from a desired chassis velocity.
|
template<std::convertible_to< SwerveModuleVelocity >... ModuleVelocities>
requires (sizeof...(ModuleVelocities) == NumModules) |
| ChassisVelocities | ToChassisVelocities (ModuleVelocities &&... moduleVelocities) const |
| | Performs forward kinematics to return the resulting chassis state from the given module states.
|
| ChassisVelocities | ToChassisVelocities (const wpi::util::array< SwerveModuleVelocity, NumModules > &moduleVelocities) const override |
| | Performs forward kinematics to return the resulting chassis state from the given module states.
|
template<std::convertible_to< SwerveModulePosition >... ModuleDeltas>
requires (sizeof...(ModuleDeltas) == NumModules) |
| Twist2d | ToTwist2d (ModuleDeltas &&... moduleDeltas) const |
| | Performs forward kinematics to return the resulting Twist2d from the given module position deltas.
|
| Twist2d | ToTwist2d (wpi::util::array< SwerveModulePosition, NumModules > moduleDeltas) const |
| | Performs forward kinematics to return the resulting Twist2d from the given module position deltas.
|
| Twist2d | ToTwist2d (const wpi::util::array< SwerveModulePosition, NumModules > &start, const wpi::util::array< SwerveModulePosition, NumModules > &end) const override |
| | Performs forward kinematics to return the resulting Twist2d from the given change in wheel positions.
|
| wpi::util::array< SwerveModulePosition, NumModules > | Interpolate (const wpi::util::array< SwerveModulePosition, NumModules > &start, const wpi::util::array< SwerveModulePosition, NumModules > &end, double t) const override |
| | Performs interpolation between two values.
|
| const wpi::util::array< Translation2d, NumModules > & | GetModules () const |
| wpi::util::array< SwerveModuleAcceleration, NumModules > | ToSwerveModuleAccelerations (const ChassisAccelerations &chassisAccelerations, const units::radians_per_second_t angularVelocity=0.0_rad_per_s, const Translation2d ¢erOfRotation=Translation2d{}) const |
| | Performs inverse kinematics to return the module accelerations from a desired chassis acceleration.
|
| wpi::util::array< SwerveModuleAcceleration, NumModules > | ToWheelAccelerations (const ChassisAccelerations &chassisAccelerations) const override |
| | Performs inverse kinematics to return the module accelerations from a desired chassis acceleration.
|
template<std::convertible_to< SwerveModuleAcceleration >... ModuleAccelerations>
requires (sizeof...(ModuleAccelerations) == NumModules) |
| ChassisAccelerations | ToChassisAccelerations (ModuleAccelerations &&... moduleAccelerations) const |
| | Performs forward kinematics to return the resulting chassis accelerations from the given module accelerations.
|
| ChassisAccelerations | ToChassisAccelerations (const wpi::util::array< SwerveModuleAcceleration, NumModules > &moduleAccelerations) const override |
| | Performs forward kinematics to return the resulting chassis accelerations from the given module accelerations.
|
| virtual | ~Kinematics () noexcept=default |
template<size_t NumModules>
class wpi::math::SwerveDriveKinematics< NumModules >
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module states (velocity and angle).
The inverse kinematics (converting from a desired chassis velocity to individual module states) uses the relative locations of the modules with respect to the center of rotation. The center of rotation for inverse kinematics is also variable. This means that you can set your set your center of rotation in a corner of the robot to perform special evasion maneuvers.
Forward kinematics (converting an array of module states into the overall chassis motion) is performs the exact opposite of what inverse kinematics does. Since this is an overdetermined system (more equations than variables), we use a least-squares approximation.
The inverse kinematics: [moduleVelocities] = [moduleLocations] * [chassisVelocities] We take the Moore-Penrose pseudoinverse of [moduleLocations] and then multiply by [moduleVelocities] to get our chassis velocities.
Forward kinematics is also used for odometry – determining the position of the robot on the field using encoders and a gyro.
template<size_t NumModules>
Renormalizes the wheel velocities if any individual velocity is above the specified maximum, as well as getting rid of joystick saturation at edges of joystick.
Sometimes, after inverse kinematics, the requested velocity from one or more modules may be above the max attainable velocity for the driving motor on that module. To fix this issue, one can reduce all the wheel velocities to make sure that all requested module velocities are at-or-below the absolute threshold, while maintaining the ratio of velocities between modules.
Scaling down the module velocities rotates the direction of net motion in the opposite direction of rotational velocity, which makes discretizing the chassis velocities inaccurate because the discretization did not account for this translational skew.
- Parameters
-
| moduleVelocities | Reference to array of module states. The array will be mutated with the normalized velocities! |
| desiredChassisVelocity | The desired velocity of the robot |
| attainableMaxModuleVelocity | The absolute max velocity a module can reach |
| attainableMaxRobotTranslationVelocity | The absolute max velocity the robot can reach while translating |
| attainableMaxRobotRotationVelocity | The absolute max velocity the robot can reach while rotating |
template<size_t NumModules>
Renormalizes the wheel velocities if any individual velocity is above the specified maximum.
Sometimes, after inverse kinematics, the requested velocity from one or more modules may be above the max attainable velocity for the driving motor on that module. To fix this issue, one can reduce all the wheel velocities to make sure that all requested module velocities are at-or-below the absolute threshold, while maintaining the ratio of velocities between modules.
Scaling down the module velocities rotates the direction of net motion in the opposite direction of rotational velocity, which makes discretizing the chassis velocities inaccurate because the discretization did not account for this translational skew.
- Parameters
-
| moduleVelocities | Reference to array of module states. The array will be mutated with the normalized velocities! |
| attainableMaxVelocity | The absolute max velocity that a module can reach. |
template<size_t NumModules>
Performs inverse kinematics to return the module accelerations from a desired chassis acceleration.
This method is often used for dynamics calculations – converting desired robot accelerations into individual module accelerations.
This function also supports variable centers of rotation. During normal operations, the center of rotation is usually the same as the physical center of the robot; therefore, the argument is defaulted to that use case. However, if you wish to change the center of rotation for evasive maneuvers, vision alignment, or for any other use case, you can do so.
- Parameters
-
| chassisAccelerations | The desired chassis accelerations. |
| angularVelocity | The desired robot angular velocity. |
| centerOfRotation | The center of rotation. For example, if you set the center of rotation at one corner of the robot and provide a chassis acceleration that only has a dtheta component, the robot will rotate around that corner. |
- Returns
- An array containing the module accelerations.
template<size_t NumModules>
Performs inverse kinematics to return the module states from a desired chassis velocity.
This method is often used to convert joystick values into module velocities and angles.
This function also supports variable centers of rotation. During normal operations, the center of rotation is usually the same as the physical center of the robot; therefore, the argument is defaulted to that use case. However, if you wish to change the center of rotation for evasive maneuvers, vision alignment, or for any other use case, you can do so.
In the case that the desired chassis velocities are zero (i.e. the robot will be stationary), the previously calculated module angle will be maintained.
- Parameters
-
| chassisVelocities | The desired chassis velocity. |
| centerOfRotation | The center of rotation. For example, if you set the center of rotation at one corner of the robot and provide a chassis velocity that only has a dtheta component, the robot will rotate around that corner. |
- Returns
- An array containing the module states. Use caution because these module states are not normalized. Sometimes, a user input may cause one of the module velocities to go above the attainable max velocity. Use the DesaturateWheelVelocities(wpi::util::array<SwerveModuleVelocity,
NumModules>*, wpi::units::meters_per_second_t) function to rectify this issue. In addition, you can leverage the power of C++17 to directly assign the module states to variables:
auto [fl, fr, bl, br] =
kinematics.ToSwerveModuleVelocities(chassisVelocities);
1.15.0