Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[commands, wpimath] Remove Mecanum/SwerveControllerCommand and HolonomicDriveController (#8119)

Browse Source
This commit is contained in:
Gold856
2025-08-01 02:05:42 -04:00
committed by GitHub
parent b251d16ef7
commit e0e774abde
51 changed files with 0 additions and 5642 deletions
Download Patch File Download Diff File Expand all files Collapse all files

168
wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
View File

@@ -1,168 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.controller;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.util.Units;
/**
* This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain
* (i.e. swerve or mecanum). Holonomic trajectory following is a much simpler problem to solve
* compared to skid-steer style drivetrains because it is possible to individually control
* field-relative x, y, and angular velocity.
*
* <p>The holonomic drive controller takes in one PID controller for each direction, field-relative
* x and y, and one profiled PID controller for the angular direction. Because the heading dynamics
* are decoupled from translations, users can specify a custom heading that the drivetrain should
* point toward. This heading reference is profiled for smoothness.
*/
public class HolonomicDriveController {
private Pose2d m_poseError = Pose2d.kZero;
private Rotation2d m_rotationError = Rotation2d.kZero;
private Pose2d m_poseTolerance = Pose2d.kZero;
private boolean m_enabled = true;
private final PIDController m_xController;
private final PIDController m_yController;
private final ProfiledPIDController m_thetaController;
private boolean m_firstRun = true;
/**
* Constructs a holonomic drive controller.
*
* @param xController A PID Controller to respond to error in the field-relative x direction.
* @param yController A PID Controller to respond to error in the field-relative y direction.
* @param thetaController A profiled PID controller to respond to error in angle.
*/
public HolonomicDriveController(
PIDController xController, PIDController yController, ProfiledPIDController thetaController) {
m_xController = xController;
m_yController = yController;
m_thetaController = thetaController;
m_thetaController.enableContinuousInput(0, Units.degreesToRadians(360.0));
}
/**
* Returns true if the pose error is within tolerance of the reference.
*
* @return True if the pose error is within tolerance of the reference.
*/
public boolean atReference() {
final var eTranslate = m_poseError.getTranslation();
final var eRotate = m_rotationError;
final var tolTranslate = m_poseTolerance.getTranslation();
final var tolRotate = m_poseTolerance.getRotation();
return Math.abs(eTranslate.getX()) < tolTranslate.getX()
&& Math.abs(eTranslate.getY()) < tolTranslate.getY()
&& Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
}
/**
* Sets the pose error which is considered tolerance for use with atReference().
*
* @param tolerance The pose error which is tolerable.
*/
public void setTolerance(Pose2d tolerance) {
m_poseTolerance = tolerance;
}
/**
* Returns the next output of the holonomic drive controller.
*
* @param currentPose The current pose, as measured by odometry or pose estimator.
* @param trajectoryPose The desired trajectory pose, as sampled for the current timestep.
* @param desiredLinearVelocity The desired linear velocity in m/s.
* @param desiredHeading The desired heading.
* @return The next output of the holonomic drive controller.
*/
public ChassisSpeeds calculate(
Pose2d currentPose,
Pose2d trajectoryPose,
double desiredLinearVelocity,
Rotation2d desiredHeading) {
// If this is the first run, then we need to reset the theta controller to the current pose's
// heading.
if (m_firstRun) {
m_thetaController.reset(currentPose.getRotation().getRadians());
m_firstRun = false;
}
// Calculate feedforward velocities (field-relative).
double xFF = desiredLinearVelocity * trajectoryPose.getRotation().getCos();
double yFF = desiredLinearVelocity * trajectoryPose.getRotation().getSin();
double thetaFF =
m_thetaController.calculate(
currentPose.getRotation().getRadians(), desiredHeading.getRadians());
m_poseError = trajectoryPose.relativeTo(currentPose);
m_rotationError = desiredHeading.minus(currentPose.getRotation());
if (!m_enabled) {
return new ChassisSpeeds(xFF, yFF, thetaFF).toRobotRelative(currentPose.getRotation());
}
// Calculate feedback velocities (based on position error).
double xFeedback = m_xController.calculate(currentPose.getX(), trajectoryPose.getX());
double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY());
// Return next output.
return new ChassisSpeeds(xFF + xFeedback, yFF + yFeedback, thetaFF)
.toRobotRelative(currentPose.getRotation());
}
/**
* Returns the next output of the holonomic drive controller.
*
* @param currentPose The current pose, as measured by odometry or pose estimator.
* @param desiredState The desired trajectory pose, as sampled for the current timestep.
* @param desiredHeading The desired heading.
* @return The next output of the holonomic drive controller.
*/
public ChassisSpeeds calculate(
Pose2d currentPose, Trajectory.State desiredState, Rotation2d desiredHeading) {
return calculate(currentPose, desiredState.pose, desiredState.velocity, desiredHeading);
}
/**
* Enables and disables the controller for troubleshooting problems. When calculate() is called on
* a disabled controller, only feedforward values are returned.
*
* @param enabled If the controller is enabled or not.
*/
public void setEnabled(boolean enabled) {
m_enabled = enabled;
}
/**
* Returns the x controller.
*
* @return X PIDController
*/
public PIDController getXController() {
return m_xController;
}
/**
* Returns the y controller.
*
* @return Y PIDController
*/
public PIDController getYController() {
return m_yController;
}
/**
* Returns the heading controller.
*
* @return heading ProfiledPIDController
*/
public ProfiledPIDController getThetaController() {
return m_thetaController;
}
}

53
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java
View File

@@ -1,53 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
/**
* Represents the motor voltages for a mecanum drive drivetrain.
*
* @deprecated Use {@link
* edu.wpi.first.wpilibj2.command.MecanumControllerCommand.MecanumVoltagesConsumer}
*/
@Deprecated(since = "2025", forRemoval = true)
public class MecanumDriveMotorVoltages {
/** Voltage of the front left motor. */
public double frontLeft;
/** Voltage of the front right motor. */
public double frontRight;
/** Voltage of the rear left motor. */
public double rearLeft;
/** Voltage of the rear right motor. */
public double rearRight;
/** Constructs a MecanumDriveMotorVoltages with zeros for all member fields. */
public MecanumDriveMotorVoltages() {}
/**
* Constructs a MecanumDriveMotorVoltages.
*
* @param frontLeft Voltage of the front left motor.
* @param frontRight Voltage of the front right motor.
* @param rearLeft Voltage of the rear left motor.
* @param rearRight Voltage of the rear right motor.
*/
public MecanumDriveMotorVoltages(
double frontLeft, double frontRight, double rearLeft, double rearRight) {
this.frontLeft = frontLeft;
this.frontRight = frontRight;
this.rearLeft = rearLeft;
this.rearRight = rearRight;
}
@Override
public String toString() {
return String.format(
"MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
+ "Rear Left: %.2f V, Rear Right: %.2f V)",
frontLeft, frontRight, rearLeft, rearRight);
}
}

218
wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
View File

@@ -1,218 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <utility>
#include <wpi/SymbolExports.h>
#include "frc/controller/PIDController.h"
#include "frc/controller/ProfiledPIDController.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/trajectory/Trajectory.h"
#include "units/angle.h"
#include "units/angular_velocity.h"
#include "units/velocity.h"
namespace frc {
/**
* This holonomic drive controller can be used to follow trajectories using a
* holonomic drivetrain (i.e. swerve or mecanum). Holonomic trajectory following
* is a much simpler problem to solve compared to skid-steer style drivetrains
* because it is possible to individually control field-relative x, y, and
* angular velocity.
*
* The holonomic drive controller takes in one PID controller for each
* direction, field-relative x and y, and one profiled PID controller for the
* angular direction. Because the heading dynamics are decoupled from
* translations, users can specify a custom heading that the drivetrain should
* point toward. This heading reference is profiled for smoothness.
*/
class WPILIB_DLLEXPORT HolonomicDriveController {
public:
/**
* Constructs a holonomic drive controller.
*
* @param xController A PID Controller to respond to error in the
* field-relative x direction.
* @param yController A PID Controller to respond to error in the
* field-relative y direction.
* @param thetaController A profiled PID controller to respond to error in
* angle.
*/
constexpr HolonomicDriveController(
PIDController xController, PIDController yController,
ProfiledPIDController<units::radian> thetaController)
: m_xController(std::move(xController)),
m_yController(std::move(yController)),
m_thetaController(std::move(thetaController)) {
m_thetaController.EnableContinuousInput(0_deg, 360.0_deg);
}
constexpr HolonomicDriveController(const HolonomicDriveController&) = default;
constexpr HolonomicDriveController& operator=(
const HolonomicDriveController&) = default;
constexpr HolonomicDriveController(HolonomicDriveController&&) = default;
constexpr HolonomicDriveController& operator=(HolonomicDriveController&&) =
default;
/**
* Returns true if the pose error is within tolerance of the reference.
*/
constexpr bool AtReference() const {
const auto& eTranslate = m_poseError.Translation();
const auto& eRotate = m_rotationError;
const auto& tolTranslate = m_poseTolerance.Translation();
const auto& tolRotate = m_poseTolerance.Rotation();
return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
units::math::abs(eRotate.Radians()) < tolRotate.Radians();
}
/**
* Sets the pose error which is considered tolerable for use with
* AtReference().
*
* @param tolerance Pose error which is tolerable.
*/
constexpr void SetTolerance(const Pose2d& tolerance) {
m_poseTolerance = tolerance;
}
/**
* Returns the next output of the holonomic drive controller.
*
* @param currentPose The current pose, as measured by odometry or pose
* estimator.
* @param trajectoryPose The desired trajectory pose, as sampled for the
* current timestep.
* @param desiredLinearVelocity The desired linear velocity.
* @param desiredHeading The desired heading.
* @return The next output of the holonomic drive controller.
*/
constexpr ChassisSpeeds Calculate(
const Pose2d& currentPose, const Pose2d& trajectoryPose,
units::meters_per_second_t desiredLinearVelocity,
const Rotation2d& desiredHeading) {
// If this is the first run, then we need to reset the theta controller to
// the current pose's heading.
if (m_firstRun) {
m_thetaController.Reset(currentPose.Rotation().Radians());
m_firstRun = false;
}
// Calculate feedforward velocities (field-relative)
auto xFF = desiredLinearVelocity * trajectoryPose.Rotation().Cos();
auto yFF = desiredLinearVelocity * trajectoryPose.Rotation().Sin();
auto thetaFF = units::radians_per_second_t{m_thetaController.Calculate(
currentPose.Rotation().Radians(), desiredHeading.Radians())};
m_poseError = trajectoryPose.RelativeTo(currentPose);
m_rotationError = desiredHeading - currentPose.Rotation();
if (!m_enabled) {
return ChassisSpeeds{xFF, yFF, thetaFF}.ToRobotRelative(
currentPose.Rotation());
}
// Calculate feedback velocities (based on position error).
auto xFeedback = units::meters_per_second_t{m_xController.Calculate(
currentPose.X().value(), trajectoryPose.X().value())};
auto yFeedback = units::meters_per_second_t{m_yController.Calculate(
currentPose.Y().value(), trajectoryPose.Y().value())};
// Return next output.
return ChassisSpeeds{xFF + xFeedback, yFF + yFeedback, thetaFF}
.ToRobotRelative(currentPose.Rotation());
}
/**
* Returns the next output of the holonomic drive controller.
*
* @param currentPose The current pose, as measured by odometry or pose
* estimator.
* @param desiredState The desired trajectory pose, as sampled for the current
* timestep.
* @param desiredHeading The desired heading.
* @return The next output of the holonomic drive controller.
*/
constexpr ChassisSpeeds Calculate(const Pose2d& currentPose,
const Trajectory::State& desiredState,
const Rotation2d& desiredHeading) {
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
desiredHeading);
}
/**
* Enables and disables the controller for troubleshooting purposes. When
* Calculate() is called on a disabled controller, only feedforward values
* are returned.
*
* @param enabled If the controller is enabled or not.
*/
constexpr void SetEnabled(bool enabled) { m_enabled = enabled; }
/**
* Returns the X PIDController
*
* @deprecated Use GetXController() instead.
*/
[[deprecated("Use GetXController() instead")]]
constexpr PIDController& getXController() {
return m_xController;
}
/**
* Returns the Y PIDController
*
* @deprecated Use GetYController() instead.
*/
[[deprecated("Use GetYController() instead")]]
constexpr PIDController& getYController() {
return m_yController;
}
/**
* Returns the rotation ProfiledPIDController
*
* @deprecated Use GetThetaController() instead.
*/
[[deprecated("Use GetThetaController() instead")]]
constexpr ProfiledPIDController<units::radian>& getThetaController() {
return m_thetaController;
}
/**
* Returns the X PIDController
*/
constexpr PIDController& GetXController() { return m_xController; }
/**
* Returns the Y PIDController
*/
constexpr PIDController& GetYController() { return m_yController; }
/**
* Returns the rotation ProfiledPIDController
*/
constexpr ProfiledPIDController<units::radian>& GetThetaController() {
return m_thetaController;
}
private:
Pose2d m_poseError;
Rotation2d m_rotationError;
Pose2d m_poseTolerance;
bool m_enabled = true;
PIDController m_xController;
PIDController m_yController;
ProfiledPIDController<units::radian> m_thetaController;
bool m_firstRun = true;
};
} // namespace frc

85
wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java
View File

@@ -1,85 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import java.util.ArrayList;
import java.util.List;
import org.junit.jupiter.api.Test;
class HolonomicDriveControllerTest {
private static final double kTolerance = 1 / 12.0;
private static final double kAngularTolerance = Math.toRadians(2);
@Test
void testReachesReference() {
HolonomicDriveController controller =
new HolonomicDriveController(
new PIDController(1.0, 0.0, 0.0),
new PIDController(1.0, 0.0, 0.0),
new ProfiledPIDController(
1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2.0 * Math.PI, Math.PI)));
Pose2d robotPose = new Pose2d(2.7, 23.0, Rotation2d.kZero);
List<Pose2d> waypoints = new ArrayList<>();
waypoints.add(new Pose2d(2.75, 22.521, Rotation2d.kZero));
waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.8)));
TrajectoryConfig config = new TrajectoryConfig(8.0, 4.0);
Trajectory trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
final double kDt = 0.02;
final double kTotalTime = trajectory.getTotalTime();
for (int i = 0; i < (kTotalTime / kDt); i++) {
Trajectory.State state = trajectory.sample(kDt * i);
ChassisSpeeds output = controller.calculate(robotPose, state, Rotation2d.kZero);
robotPose = robotPose.exp(new Twist2d(output.vx * kDt, output.vy * kDt, output.omega * kDt));
}
final List<Trajectory.State> states = trajectory.getStates();
final Pose2d endPose = states.get(states.size() - 1).pose;
// Java lambdas require local variables referenced from a lambda expression
// must be final or effectively final.
final Pose2d finalRobotPose = robotPose;
assertAll(
() -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
() -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
() ->
assertEquals(
0.0,
MathUtil.angleModulus(finalRobotPose.getRotation().getRadians()),
kAngularTolerance));
}
@Test
void testDoesNotRotateUnnecessarily() {
var controller =
new HolonomicDriveController(
new PIDController(1, 0, 0),
new PIDController(1, 0, 0),
new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(4, 2)));
ChassisSpeeds speeds =
controller.calculate(
new Pose2d(0, 0, new Rotation2d(1.57)), Pose2d.kZero, 0, new Rotation2d(1.57));
assertEquals(0.0, speeds.omega);
}
}

67
wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
View File

@@ -1,67 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <numbers>
#include <gtest/gtest.h>
#include "frc/MathUtil.h"
#include "frc/controller/HolonomicDriveController.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "units/angular_acceleration.h"
#include "units/math.h"
#include "units/time.h"
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
EXPECT_LE(units::math::abs(val1 - val2), eps)
static constexpr units::meter_t kTolerance{1 / 12.0};
static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
180.0};
TEST(HolonomicDriveControllerTest, ReachesReference) {
frc::HolonomicDriveController controller{
frc::PIDController{1.0, 0.0, 0.0}, frc::PIDController{1.0, 0.0, 0.0},
frc::ProfiledPIDController<units::radian>{
1.0, 0.0, 0.0,
frc::TrapezoidProfile<units::radian>::Constraints{
units::radians_per_second_t{2.0 * std::numbers::pi},
units::radians_per_second_squared_t{std::numbers::pi}}}};
frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.0_mps, 4.0_mps_sq});
constexpr units::second_t kDt = 20_ms;
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
auto state = trajectory.Sample(kDt * i);
auto [vx, vy, omega] = controller.Calculate(robotPose, state, 0_rad);
robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, vy * kDt, omega * kDt});
}
auto& endPose = trajectory.States().back().pose;
EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
EXPECT_NEAR_UNITS(frc::AngleModulus(robotPose.Rotation().Radians()), 0_rad,
kAngularTolerance);
}
TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
frc::HolonomicDriveController controller{
frc::PIDController{1, 0, 0}, frc::PIDController{1, 0, 0},
frc::ProfiledPIDController<units::radian>{
1, 0, 0,
frc::TrapezoidProfile<units::radian>::Constraints{
4_rad_per_s, 2_rad_per_s / 1_s}}};
frc::ChassisSpeeds speeds = controller.Calculate(
frc::Pose2d{0_m, 0_m, 1.57_rad}, frc::Pose2d{}, 0_mps, 1.57_rad);
EXPECT_EQ(0, speeds.omega.value());
}