WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
SwerveDriveKinematicsConstraint.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <cmath>
8
11#include "units/math.h"
12#include "units/velocity.h"
13
14namespace frc {
15
16/**
17 * A class that enforces constraints on the swerve drive kinematics.
18 * This can be used to ensure that the trajectory is constructed so that the
19 * commanded velocities of the wheels stay below a certain limit.
20 */
21template <size_t NumModules>
23 public:
26 units::meters_per_second_t maxSpeed)
27 : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
28
29 units::meters_per_second_t MaxVelocity(
30 const Pose2d& pose, units::curvature_t curvature,
31 units::meters_per_second_t velocity) const override {
32 auto xVelocity = velocity * pose.Rotation().Cos();
33 auto yVelocity = velocity * pose.Rotation().Sin();
34 auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
35 {xVelocity, yVelocity, velocity * curvature});
36 m_kinematics.DesaturateWheelSpeeds(&wheelSpeeds, m_maxSpeed);
37
38 auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
39
40 return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
41 }
42
44 units::meters_per_second_t speed) const override {
45 return {};
46 }
47
48 private:
50 units::meters_per_second_t m_maxSpeed;
51};
52
53} // namespace frc
Represents a 2D pose containing translational and rotational elements.
Definition Pose2d.h:28
constexpr const Rotation2d & Rotation() const
Returns the underlying rotation.
Definition Pose2d.h:128
constexpr double Cos() const
Returns the cosine of the rotation.
Definition Rotation2d.h:231
constexpr double Sin() const
Returns the sine of the rotation.
Definition Rotation2d.h:238
A class that enforces constraints on the swerve drive kinematics.
Definition SwerveDriveKinematicsConstraint.h:22
MinMax MinMaxAcceleration(const Pose2d &pose, units::curvature_t curvature, units::meters_per_second_t speed) const override
Returns the minimum and maximum allowable acceleration for the trajectory given pose,...
Definition SwerveDriveKinematicsConstraint.h:43
SwerveDriveKinematicsConstraint(const frc::SwerveDriveKinematics< NumModules > &kinematics, units::meters_per_second_t maxSpeed)
Definition SwerveDriveKinematicsConstraint.h:24
units::meters_per_second_t MaxVelocity(const Pose2d &pose, units::curvature_t curvature, units::meters_per_second_t velocity) const override
Returns the max velocity given the current pose and curvature.
Definition SwerveDriveKinematicsConstraint.h:29
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition SwerveDriveKinematics.h:54
An interface for defining user-defined velocity and acceleration constraints while generating traject...
Definition TrajectoryConstraint.h:21
constexpr UnitTypeLhs hypot(const UnitTypeLhs &x, const UnitTypeRhs &y)
Computes the square root of the sum-of-squares of x and y.
Definition math.h:508
Definition CAN.h:11
Represents a minimum and maximum acceleration.
Definition TrajectoryConstraint.h:37