Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module states (speed and angle). More...

#include <frc/kinematics/SwerveDriveKinematics.h>

Inheritance diagram for frc::SwerveDriveKinematics< NumModules >:

Public Member Functions
template<std::convertible_to< Translation2d >... ModuleTranslations> requires (sizeof...(ModuleTranslations) == NumModules)
	SwerveDriveKinematics (ModuleTranslations &&... moduleTranslations)
	Constructs a swerve drive kinematics object. More...

	SwerveDriveKinematics (const wpi::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. More...

void	ResetHeadings (wpi::array< Rotation2d, NumModules > moduleHeadings)
	Reset the internal swerve module headings. More...

wpi::array< SwerveModuleState, NumModules >	ToSwerveModuleStates (const ChassisSpeeds &chassisSpeeds, const Translation2d &centerOfRotation=Translation2d{}) const
	Performs inverse kinematics to return the module states from a desired chassis velocity. More...

SwerveDriveWheelSpeeds< NumModules >	ToWheelSpeeds (const ChassisSpeeds &chassisSpeeds) const override
	Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. More...

template<std::convertible_to< SwerveModuleState >... ModuleStates> requires (sizeof...(ModuleStates) == NumModules)
ChassisSpeeds	ToChassisSpeeds (ModuleStates &&... moduleStates) const
	Performs forward kinematics to return the resulting chassis state from the given module states. More...

ChassisSpeeds	ToChassisSpeeds (const wpi::array< SwerveModuleState, NumModules > &moduleStates) const override
	Performs forward kinematics to return the resulting chassis state from the given module states. More...

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. More...

Twist2d	ToTwist2d (wpi::array< SwerveModulePosition, NumModules > moduleDeltas) const
	Performs forward kinematics to return the resulting Twist2d from the given module position deltas. More...

Twist2d	ToTwist2d (const SwerveDriveWheelPositions< NumModules > &start, const SwerveDriveWheelPositions< NumModules > &end) const override
	Performs forward kinematics to return the resulting Twist2d from the given change in wheel positions. More...

Public Member Functions inherited from frc::Kinematics< SwerveDriveWheelSpeeds< NumModules >, SwerveDriveWheelPositions< NumModules > >
virtual ChassisSpeeds	ToChassisSpeeds (const SwerveDriveWheelSpeeds< NumModules > &wheelSpeeds) const=0
	Performs forward kinematics to return the resulting chassis speed from the wheel speeds. More...

virtual SwerveDriveWheelSpeeds< NumModules >	ToWheelSpeeds (const ChassisSpeeds &chassisSpeeds) const=0
	Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. More...

virtual Twist2d	ToTwist2d (const SwerveDriveWheelPositions< NumModules > &start, const SwerveDriveWheelPositions< NumModules > &end) const=0
	Performs forward kinematics to return the resulting Twist2d from the given change in wheel positions. More...

Static Public Member Functions
static void	DesaturateWheelSpeeds (wpi::array< SwerveModuleState, NumModules > *moduleStates, units::meters_per_second_t attainableMaxSpeed)
	Renormalizes the wheel speeds if any individual speed is above the specified maximum. More...

static void	DesaturateWheelSpeeds (wpi::array< SwerveModuleState, NumModules > *moduleStates, ChassisSpeeds desiredChassisSpeed, units::meters_per_second_t attainableMaxModuleSpeed, units::meters_per_second_t attainableMaxRobotTranslationSpeed, units::radians_per_second_t attainableMaxRobotRotationSpeed)
	Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well as getting rid of joystick saturation at edges of joystick. More...

Detailed Description

template<size_t NumModules>
class frc::SwerveDriveKinematics< NumModules >

Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module states (speed 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: [moduleStates] = [moduleLocations] * [chassisSpeeds] We take the Moore-Penrose pseudoinverse of [moduleLocations] and then multiply by [moduleStates] to get our chassis speeds.

Forward kinematics is also used for odometry – determining the position of the robot on the field using encoders and a gyro.

Constructor & Destructor Documentation

◆ SwerveDriveKinematics() [1/3]

template<size_t NumModules>

template<std::convertible_to< Translation2d >... ModuleTranslations>
requires (sizeof...(ModuleTranslations) == NumModules)

frc::SwerveDriveKinematics< NumModules >::SwerveDriveKinematics ( ModuleTranslations &&... moduleTranslations )

inlineexplicit

Constructs a swerve drive kinematics object.

This takes in a variable number of module locations as Translation2ds. The order in which you pass in the module locations is the same order that you will receive the module states when performing inverse kinematics. It is also expected that you pass in the module states in the same order when calling the forward kinematics methods.

Parameters

moduleTranslations The locations of the modules relative to the physical center of the robot.

◆ SwerveDriveKinematics() [2/3]

template<size_t NumModules>

frc::SwerveDriveKinematics< NumModules >::SwerveDriveKinematics ( const wpi::array< Translation2d, NumModules > & modules )

inlineexplicit

◆ SwerveDriveKinematics() [3/3]

template<size_t NumModules>

frc::SwerveDriveKinematics< NumModules >::SwerveDriveKinematics ( const SwerveDriveKinematics< NumModules > & )

default

Member Function Documentation

◆ DesaturateWheelSpeeds() [1/2]

template<size_t NumModules>

void frc::SwerveDriveKinematics< NumModules >::DesaturateWheelSpeeds	(	wpi::array< SwerveModuleState, NumModules > *	moduleStates,
		ChassisSpeeds	desiredChassisSpeed,
		units::meters_per_second_t	attainableMaxModuleSpeed,
		units::meters_per_second_t	attainableMaxRobotTranslationSpeed,
		units::radians_per_second_t	attainableMaxRobotRotationSpeed
	)

static

Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well as getting rid of joystick saturation at edges of joystick.

Sometimes, after inverse kinematics, the requested speed from one or more modules may be above the max attainable speed for the driving motor on that module. To fix this issue, one can reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the absolute threshold, while maintaining the ratio of speeds between modules.

Parameters

moduleStates	Reference to array of module states. The array will be mutated with the normalized speeds!
desiredChassisSpeed	The desired speed of the robot
attainableMaxModuleSpeed	The absolute max speed a module can reach
attainableMaxRobotTranslationSpeed	The absolute max speed the robot can reach while translating
attainableMaxRobotRotationSpeed	The absolute max speed the robot can reach while rotating

◆ DesaturateWheelSpeeds() [2/2]

template<size_t NumModules>

void frc::SwerveDriveKinematics< NumModules >::DesaturateWheelSpeeds	(	wpi::array< SwerveModuleState, NumModules > *	moduleStates,
		units::meters_per_second_t	attainableMaxSpeed
	)

static

Renormalizes the wheel speeds if any individual speed is above the specified maximum.

Sometimes, after inverse kinematics, the requested speed from one or more modules may be above the max attainable speed for the driving motor on that module. To fix this issue, one can reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the absolute threshold, while maintaining the ratio of speeds between modules.

Parameters

moduleStates	Reference to array of module states. The array will be mutated with the normalized speeds!
attainableMaxSpeed	The absolute max speed that a module can reach.

◆ ResetHeadings() [1/2]

template<size_t NumModules>

template<std::convertible_to< Rotation2d >... ModuleHeadings>
requires (sizeof...(ModuleHeadings) == NumModules)

void frc::SwerveDriveKinematics< NumModules >::ResetHeadings ( ModuleHeadings &&... moduleHeadings )

inline

Reset the internal swerve module headings.

Parameters

moduleHeadings The swerve module headings. The order of the module headings should be same as passed into the constructor of this class.

◆ ResetHeadings() [2/2]

template<size_t NumModules>

void frc::SwerveDriveKinematics< NumModules >::ResetHeadings ( wpi::array< Rotation2d, NumModules > moduleHeadings )

Reset the internal swerve module headings.

Parameters

moduleHeadings The swerve module headings. The order of the module headings should be same as passed into the constructor of this class.

◆ ToChassisSpeeds() [1/2]

template<size_t NumModules>

ChassisSpeeds frc::SwerveDriveKinematics< NumModules >::ToChassisSpeeds ( const wpi::array< SwerveModuleState, NumModules > & moduleStates ) const

override

Performs forward kinematics to return the resulting chassis state from the given module states.

This method is often used for odometry – determining the robot's position on the field using data from the real-world speed and angle of each module on the robot.

Parameters

moduleStates The state of the modules as an wpi::array of type SwerveModuleState, NumModules long as measured from respective encoders and gyros. The order of the swerve module states should be same as passed into the constructor of this class.

Returns: The resulting chassis speed.

◆ ToChassisSpeeds() [2/2]

template<size_t NumModules>

template<std::convertible_to< SwerveModuleState >... ModuleStates>
requires (sizeof...(ModuleStates) == NumModules)

ChassisSpeeds frc::SwerveDriveKinematics< NumModules >::ToChassisSpeeds ( ModuleStates &&... moduleStates ) const

inline

Performs forward kinematics to return the resulting chassis state from the given module states.

This method is often used for odometry – determining the robot's position on the field using data from the real-world speed and angle of each module on the robot.

Parameters

moduleStates The state of the modules (as a SwerveModuleState type) as measured from respective encoders and gyros. The order of the swerve module states should be same as passed into the constructor of this class.

Returns: The resulting chassis speed.

◆ ToSwerveModuleStates()

template<size_t NumModules>

wpi::array< SwerveModuleState, NumModules > frc::SwerveDriveKinematics< NumModules >::ToSwerveModuleStates	(	const ChassisSpeeds &	chassisSpeeds,
		const Translation2d &	centerOfRotation = `Translation2d{}`
	)		const

Performs inverse kinematics to return the module states from a desired chassis velocity.

This method is often used to convert joystick values into module speeds 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 speeds are zero (i.e. the robot will be stationary), the previously calculated module angle will be maintained.

Parameters

chassisSpeeds	The desired chassis speed.
centerOfRotation	The center of rotation. For example, if you set the center of rotation at one corner of the robot and provide a chassis speed 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 speeds to go above the attainable max velocity. Use the DesaturateWheelSpeeds(wpi::array<SwerveModuleState, NumModules>*, 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.ToSwerveModuleStates(chassisSpeeds);

◆ ToTwist2d() [1/3]

template<size_t NumModules>

Twist2d frc::SwerveDriveKinematics< NumModules >::ToTwist2d	(	const SwerveDriveWheelPositions< NumModules > &	start,
		const SwerveDriveWheelPositions< NumModules > &	end
	)		const

inlineoverridevirtual

Performs forward kinematics to return the resulting Twist2d from the given change in wheel positions.

This method is often used for odometry – determining the robot's position on the field using changes in the distance driven by each wheel on the robot.

Parameters

start	The starting distances driven by the wheels.
end	The ending distances driven by the wheels.

Returns: The resulting Twist2d in the robot's movement.

Implements frc::Kinematics< SwerveDriveWheelSpeeds< NumModules >, SwerveDriveWheelPositions< NumModules > >.

◆ ToTwist2d() [2/3]

template<size_t NumModules>

template<std::convertible_to< SwerveModulePosition >... ModuleDeltas>
requires (sizeof...(ModuleDeltas) == NumModules)

Twist2d frc::SwerveDriveKinematics< NumModules >::ToTwist2d ( ModuleDeltas &&... moduleDeltas ) const

inline

Performs forward kinematics to return the resulting Twist2d from the given module position deltas.

This method is often used for odometry – determining the robot's position on the field using data from the real-world position delta and angle of each module on the robot.

Parameters

moduleDeltas The latest change in position of the modules (as a SwerveModulePosition type) as measured from respective encoders and gyros. The order of the swerve module states should be same as passed into the constructor of this class.

Returns: The resulting Twist2d.

◆ ToTwist2d() [3/3]

template<size_t NumModules>

Twist2d frc::SwerveDriveKinematics< NumModules >::ToTwist2d ( wpi::array< SwerveModulePosition, NumModules > moduleDeltas ) const

Performs forward kinematics to return the resulting Twist2d from the given module position deltas.

This method is often used for odometry – determining the robot's position on the field using data from the real-world position delta and angle of each module on the robot.

Parameters

moduleDeltas The latest change in position of the modules (as a SwerveModulePosition type) as measured from respective encoders and gyros. The order of the swerve module states should be same as passed into the constructor of this class.

Returns: The resulting Twist2d.

◆ ToWheelSpeeds()

template<size_t NumModules>

SwerveDriveWheelSpeeds< NumModules > frc::SwerveDriveKinematics< NumModules >::ToWheelSpeeds ( const ChassisSpeeds & chassisSpeeds ) const

inlineoverridevirtual

Performs inverse kinematics to return the wheel speeds from a desired chassis velocity.

This method is often used to convert joystick values into wheel speeds.

Parameters

chassisSpeeds The desired chassis speed.

Returns: The wheel speeds.

Implements frc::Kinematics< SwerveDriveWheelSpeeds< NumModules >, SwerveDriveWheelPositions< NumModules > >.

The documentation for this class was generated from the following files:

frc/kinematics/SwerveDriveKinematics.h
frc/kinematics/SwerveDriveKinematics.inc

Public Member Functions

Static Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ SwerveDriveKinematics() [1/3]

◆ SwerveDriveKinematics() [2/3]

◆ SwerveDriveKinematics() [3/3]

Member Function Documentation

◆ DesaturateWheelSpeeds() [1/2]

◆ DesaturateWheelSpeeds() [2/2]

◆ ResetHeadings() [1/2]

◆ ResetHeadings() [2/2]

◆ ToChassisSpeeds() [1/2]

◆ ToChassisSpeeds() [2/2]

◆ ToSwerveModuleStates()

◆ ToTwist2d() [1/3]

◆ ToTwist2d() [2/3]

◆ ToTwist2d() [3/3]

◆ ToWheelSpeeds()