10#include <units/angular_velocity.h>
11#include <units/velocity.h>
27 units::meters_per_second_t
speed = 0_mps;
44 auto const delta = desiredState.
angle - currentAngle;
45 if (units::math::abs(delta.Degrees()) > 90_deg) {
46 return {-desiredState.
speed, desiredState.
angle + Rotation2d{180_deg}};
54 return units::math::abs(
speed - other.
speed) < 1E-9_mps &&
100 units::meters_per_second_t
vx = 0_mps;
104 units::meters_per_second_t
vy = 0_mps;
108 units::radians_per_second_t
omega = 0_rad_per_s;
141 Pose2d
const desiredDeltaPose{continuousSpeeds.
vx * dt, continuousSpeeds.
vy * dt, continuousSpeeds.
omega * dt};
142 auto const twist = Pose2d{}.Log(desiredDeltaPose);
143 return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
162 auto const rotated = Translation2d{fieldRelativeSpeeds.
vx * 1_s, fieldRelativeSpeeds.
vy * 1_s}
163 .RotateBy(-robotAngle);
164 return {rotated.X() / 1_s, rotated.Y() / 1_s, fieldRelativeSpeeds.
omega};
183 auto const rotated = Translation2d{fieldRelativeSpeeds.
vx * 1_s, fieldRelativeSpeeds.
vy * 1_s}
184 .RotateBy(robotAngle);
185 return {rotated.X() / 1_s, rotated.Y() / 1_s, fieldRelativeSpeeds.
omega};
195 return *
this + -other;
199 return {scalar *
vx, scalar *
vy, scalar *
omega};
203 return *
this * (1.0 / scalar);
237 std::vector<Translation2d> m_moduleLocations;
239 struct KinematicsMatrices;
240 std::unique_ptr<KinematicsMatrices> m_matrices;
242 std::vector<Rotation2d> m_lastModuleHeading;
243 Translation2d m_lastCOR{};
277 for (
size_t i = 0; i < m_numModules && i < moduleHeadings.size(); ++i) {
278 m_lastModuleHeading[i] = moduleHeadings[i];
353 for (
size_t i = 0; i < m_numModules && i < std::min(start.size(), end.size()); ++i) {
354 result[i] = {end[i].distance - start[i].distance, end[i].angle};
371 for (
size_t i = 0; i < m_numModules && i < std::min(start.size(), end.size()); ++i) {
372 result[i] = start[i].Interpolate(end[i], t);
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition: SwerveDriveKinematics.hpp:235
WheelSpeeds ToSwerveModuleStates(ChassisSpeeds const &speeds, Translation2d const ¢erOfRotation=Translation2d{})
Performs inverse kinematics to return the module states from a desired chassis velocity.
SwerveDriveKinematics(std::vector< Translation2d > moduleLocations)
Constructs a swerve drive kinematics object.
Twist2d ToTwist2d(WheelPositions const &start, WheelPositions const &end) const
Performs forward kinematics to return the resulting Twist2d from the given change in wheel positions.
Definition: SwerveDriveKinematics.hpp:350
std::vector< SwerveModulePosition > WheelPositions
Definition: SwerveDriveKinematics.hpp:247
SwerveDriveKinematics(SwerveDriveKinematics &&)
SwerveDriveKinematics & operator=(SwerveDriveKinematics const &)
static void DesaturateWheelSpeeds(WheelSpeeds *moduleStates, units::meters_per_second_t attainableMaxSpeed)
Renormalizes the wheel speeds if any individual speed is above the specified maximum.
ChassisSpeeds ToChassisSpeeds(WheelSpeeds const &moduleStates) const
Performs forward kinematics to return the resulting chassis state from the given module states.
WheelPositions Interpolate(WheelPositions const &start, WheelPositions const &end, double t) const
Performs interpolation between two values.
Definition: SwerveDriveKinematics.hpp:368
SwerveDriveKinematics(SwerveDriveKinematics const &)
SwerveDriveKinematics & operator=(SwerveDriveKinematics &&)
void ResetHeadings(std::vector< Rotation2d > const &moduleHeadings)
Reset the internal swerve module headings.
Definition: SwerveDriveKinematics.hpp:275
Twist2d ToTwist2d(WheelPositions const &moduleDeltas) const
Performs forward kinematics to return the resulting Twist2d from the given module position deltas.
std::vector< SwerveModuleState > WheelSpeeds
Definition: SwerveDriveKinematics.hpp:246
Represents the state of one swerve module.
Definition: StatusCodes.h:18
Represents the speed of a robot chassis.
Definition: SwerveDriveKinematics.hpp:96
constexpr ChassisSpeeds operator-(ChassisSpeeds const &other) const
Definition: SwerveDriveKinematics.hpp:193
constexpr ChassisSpeeds operator*(double scalar) const
Definition: SwerveDriveKinematics.hpp:197
constexpr ChassisSpeeds operator-() const
Definition: SwerveDriveKinematics.hpp:188
static ChassisSpeeds FromFieldRelativeSpeeds(ChassisSpeeds const &fieldRelativeSpeeds, Rotation2d const &robotAngle)
Converts a user provided field-relative ChassisSpeeds object into a robot-relative ChassisSpeeds obje...
Definition: SwerveDriveKinematics.hpp:160
bool operator!=(ChassisSpeeds const &other) const
Definition: SwerveDriveKinematics.hpp:210
constexpr Twist2d ToTwist2d(units::second_t dt) const
Creates a Twist2d from ChassisSpeeds.
Definition: SwerveDriveKinematics.hpp:117
units::radians_per_second_t omega
Represents the angular velocity of the robot frame.
Definition: SwerveDriveKinematics.hpp:108
units::meters_per_second_t vx
Velocity along the x-axis.
Definition: SwerveDriveKinematics.hpp:100
units::meters_per_second_t vy
Velocity along the y-axis.
Definition: SwerveDriveKinematics.hpp:104
constexpr ChassisSpeeds operator+(ChassisSpeeds const &other) const
Definition: SwerveDriveKinematics.hpp:189
static ChassisSpeeds Discretize(ChassisSpeeds const &continuousSpeeds, units::second_t dt)
Disretizes a continuous-time chassis speed.
Definition: SwerveDriveKinematics.hpp:139
constexpr ChassisSpeeds operator/(double scalar) const
Definition: SwerveDriveKinematics.hpp:201
bool operator==(ChassisSpeeds const &other) const
Definition: SwerveDriveKinematics.hpp:206
static ChassisSpeeds FromRobotRelativeSpeeds(ChassisSpeeds const &fieldRelativeSpeeds, Rotation2d const &robotAngle)
Converts a user provided robot-relative ChassisSpeeds object into a field-relative ChassisSpeeds obje...
Definition: SwerveDriveKinematics.hpp:181
Represents the position of one swerve module.
Definition: SwerveDriveKinematics.hpp:63
bool operator!=(SwerveModulePosition const &other) const
Definition: SwerveDriveKinematics.hpp:83
bool operator==(SwerveModulePosition const &other) const
Definition: SwerveDriveKinematics.hpp:78
Rotation2d angle
Angle of the module.
Definition: SwerveDriveKinematics.hpp:71
units::meter_t distance
Distance the wheel of a module has traveled.
Definition: SwerveDriveKinematics.hpp:67
SwerveModulePosition Interpolate(SwerveModulePosition const &endValue, double t) const
Definition: SwerveDriveKinematics.hpp:73
Definition: SwerveDriveKinematics.hpp:23
bool operator==(SwerveModuleState const &other) const
Definition: SwerveDriveKinematics.hpp:52
units::meters_per_second_t speed
Speed of the wheel of the module.
Definition: SwerveDriveKinematics.hpp:27
Rotation2d angle
Angle of the module.
Definition: SwerveDriveKinematics.hpp:31
bool operator!=(SwerveModuleState const &other) const
Definition: SwerveDriveKinematics.hpp:57
static SwerveModuleState Optimize(SwerveModuleState const &desiredState, Rotation2d currentAngle)
Minimize the change in heading the desired swerve module state would require by potentially reversing...
Definition: SwerveDriveKinematics.hpp:42