diff --git a/shared/examplecheck.gradle b/shared/examplecheck.gradle index 9463ea1c8f7..ed913dcc1c0 100644 --- a/shared/examplecheck.gradle +++ b/shared/examplecheck.gradle @@ -67,9 +67,8 @@ def tagList = [ "SmartDashboard", "Shuffleboard", "Sendable", "DataLog", /* --- Controls --- */ - "Exponential Profile", "PID", "State-Space", "Ramsete", "Path Following", "Trajectory", - "SysId", "Simulation", "Trapezoid Profile", "Profiled PID", "Odometry", "LQR", - "Pose Estimator", + "Exponential Profile", "PID", "State-Space", "Path Following", "Trajectory", "SysId", + "Simulation", "Trapezoid Profile", "Profiled PID", "Odometry", "LQR", "Pose Estimator", /* --- Hardware --- */ "Analog", "Ultrasonic", "Gyro", "Pneumatics", "I2C", "Duty Cycle", "PDP", "DMA", "Relay", diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java deleted file mode 100644 index 23ec61c6227..00000000000 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java +++ /dev/null @@ -1,227 +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.wpilibj2.command; - -import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.RamseteController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; -import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.Timer; -import java.util.function.BiConsumer; -import java.util.function.Supplier; - -/** - * A command that uses a RAMSETE controller ({@link RamseteController}) to follow a trajectory - * {@link Trajectory} with a differential drive. - * - *

The command handles trajectory-following, PID calculations, and feedforwards internally. This - * is intended to be a more-or-less "complete solution" that can be used by teams without a great - * deal of controls expertise. - * - *

Advanced teams seeking more flexibility (for example, those who wish to use the onboard PID - * functionality of a "smart" motor controller) may use the secondary constructor that omits the PID - * and feedforward functionality, returning only the raw wheel speeds from the RAMSETE controller. - * - *

This class is provided by the NewCommands VendorDep - */ -public class RamseteCommand extends Command { - private final Timer m_timer = new Timer(); - private final boolean m_usePID; - private final Trajectory m_trajectory; - private final Supplier m_pose; - private final RamseteController m_follower; - private final SimpleMotorFeedforward m_feedforward; - private final DifferentialDriveKinematics m_kinematics; - private final Supplier m_speeds; - private final PIDController m_leftController; - private final PIDController m_rightController; - private final BiConsumer m_output; - private DifferentialDriveWheelSpeeds m_prevSpeeds = new DifferentialDriveWheelSpeeds(); - private double m_prevLeftSpeedSetpoint; // m/s - private double m_prevRightSpeedSetpoint; // m/s - private double m_prevTime; - - /** - * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory. PID - * control and feedforward are handled internally, and outputs are scaled -12 to 12 representing - * units of volts. - * - *

Note: The controller will *not* set the outputVolts to zero upon completion of the path - - * this is left to the user, since it is not appropriate for paths with nonstationary endstates. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose - use one of the odometry classes to - * provide this. - * @param controller The RAMSETE controller used to follow the trajectory. - * @param feedforward The feedforward to use for the drive. - * @param kinematics The kinematics for the robot drivetrain. - * @param wheelSpeeds A function that supplies the speeds of the left and right sides of the robot - * drive. - * @param leftController The PIDController for the left side of the robot drive. - * @param rightController The PIDController for the right side of the robot drive. - * @param outputVolts A function that consumes the computed left and right outputs (in volts) for - * the robot drive. - * @param requirements The subsystems to require. - * @deprecated Use LTVUnicycleController instead. - */ - @Deprecated(since = "2025", forRemoval = true) - @SuppressWarnings("this-escape") - public RamseteCommand( - Trajectory trajectory, - Supplier pose, - RamseteController controller, - SimpleMotorFeedforward feedforward, - DifferentialDriveKinematics kinematics, - Supplier wheelSpeeds, - PIDController leftController, - PIDController rightController, - BiConsumer outputVolts, - Subsystem... requirements) { - m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand"); - m_pose = requireNonNullParam(pose, "pose", "RamseteCommand"); - m_follower = requireNonNullParam(controller, "controller", "RamseteCommand"); - m_feedforward = feedforward; - m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand"); - m_speeds = requireNonNullParam(wheelSpeeds, "wheelSpeeds", "RamseteCommand"); - m_leftController = requireNonNullParam(leftController, "leftController", "RamseteCommand"); - m_rightController = requireNonNullParam(rightController, "rightController", "RamseteCommand"); - m_output = requireNonNullParam(outputVolts, "outputVolts", "RamseteCommand"); - - m_usePID = true; - - addRequirements(requirements); - } - - /** - * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory. - * Performs no PID control and calculates no feedforwards; outputs are the raw wheel speeds from - * the RAMSETE controller, and will need to be converted into a usable form by the user. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose - use one of the odometry classes to - * provide this. - * @param follower The RAMSETE follower used to follow the trajectory. - * @param kinematics The kinematics for the robot drivetrain. - * @param outputMetersPerSecond A function that consumes the computed left and right wheel speeds. - * @param requirements The subsystems to require. - * @deprecated Use LTVUnicycleController instead. - */ - @Deprecated(since = "2025", forRemoval = true) - @SuppressWarnings("this-escape") - public RamseteCommand( - Trajectory trajectory, - Supplier pose, - RamseteController follower, - DifferentialDriveKinematics kinematics, - BiConsumer outputMetersPerSecond, - Subsystem... requirements) { - m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand"); - m_pose = requireNonNullParam(pose, "pose", "RamseteCommand"); - m_follower = requireNonNullParam(follower, "follower", "RamseteCommand"); - m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand"); - m_output = - requireNonNullParam(outputMetersPerSecond, "outputMetersPerSecond", "RamseteCommand"); - - m_feedforward = null; - m_speeds = null; - m_leftController = null; - m_rightController = null; - - m_usePID = false; - - addRequirements(requirements); - } - - @Override - public void initialize() { - m_prevTime = -1; - var initialState = m_trajectory.sample(0); - m_prevSpeeds = - m_kinematics.toWheelSpeeds( - new ChassisSpeeds( - initialState.velocityMetersPerSecond, - 0, - initialState.curvatureRadPerMeter * initialState.velocityMetersPerSecond)); - m_prevLeftSpeedSetpoint = m_prevSpeeds.leftMetersPerSecond; - m_prevRightSpeedSetpoint = m_prevSpeeds.rightMetersPerSecond; - m_timer.restart(); - if (m_usePID) { - m_leftController.reset(); - m_rightController.reset(); - } - } - - @Override - public void execute() { - double curTime = m_timer.get(); - - if (m_prevTime < 0) { - m_output.accept(0.0, 0.0); - m_prevTime = curTime; - return; - } - - var targetWheelSpeeds = - m_kinematics.toWheelSpeeds( - m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime))); - - double leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond; - double rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond; - - double leftOutput; - double rightOutput; - - if (m_usePID) { - double leftFeedforward = m_feedforward.calculate(m_prevLeftSpeedSetpoint, leftSpeedSetpoint); - - double rightFeedforward = - m_feedforward.calculate(m_prevRightSpeedSetpoint, rightSpeedSetpoint); - - leftOutput = - leftFeedforward - + m_leftController.calculate(m_speeds.get().leftMetersPerSecond, leftSpeedSetpoint); - - rightOutput = - rightFeedforward - + m_rightController.calculate( - m_speeds.get().rightMetersPerSecond, rightSpeedSetpoint); - } else { - leftOutput = leftSpeedSetpoint; - rightOutput = rightSpeedSetpoint; - } - - m_output.accept(leftOutput, rightOutput); - m_prevSpeeds = targetWheelSpeeds; - m_prevTime = curTime; - } - - @Override - public void end(boolean interrupted) { - m_timer.stop(); - - if (interrupted) { - m_output.accept(0.0, 0.0); - } - } - - @Override - public boolean isFinished() { - return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds()); - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addDoubleProperty("leftVelocity", () -> m_prevSpeeds.leftMetersPerSecond, null); - builder.addDoubleProperty("rightVelocity", () -> m_prevSpeeds.rightMetersPerSecond, null); - } -} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp deleted file mode 100644 index e0da8c8eaa4..00000000000 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp +++ /dev/null @@ -1,133 +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 "frc2/command/RamseteCommand.h" - -#include - -#include -#include -#include - -using namespace frc2; -using kv_unit = units::compound_unit>; - -RamseteCommand::RamseteCommand( - frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::SimpleMotorFeedforward feedforward, - frc::DifferentialDriveKinematics kinematics, - std::function wheelSpeeds, - frc::PIDController leftController, frc::PIDController rightController, - std::function output, - Requirements requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_controller(controller), - m_feedforward(feedforward), - m_kinematics(std::move(kinematics)), - m_speeds(std::move(wheelSpeeds)), - m_leftController(std::make_unique(leftController)), - m_rightController(std::make_unique(rightController)), - m_outputVolts(std::move(output)), - m_usePID(true) { - AddRequirements(requirements); -} - -RamseteCommand::RamseteCommand( - frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::DifferentialDriveKinematics kinematics, - std::function - output, - Requirements requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_controller(controller), - m_feedforward(0_V, units::unit_t{0}), - m_kinematics(std::move(kinematics)), - m_outputVel(std::move(output)), - m_usePID(false) { - AddRequirements(requirements); -} - -void RamseteCommand::Initialize() { - m_prevTime = -1_s; - auto initialState = m_trajectory.Sample(0_s); - m_prevSpeeds = m_kinematics.ToWheelSpeeds( - frc::ChassisSpeeds{initialState.velocity, 0_mps, - initialState.velocity * initialState.curvature}); - m_timer.Restart(); - if (m_usePID) { - m_leftController->Reset(); - m_rightController->Reset(); - } -} - -void RamseteCommand::Execute() { - auto curTime = m_timer.Get(); - - if (m_prevTime < 0_s) { - if (m_usePID) { - m_outputVolts(0_V, 0_V); - } else { - m_outputVel(0_mps, 0_mps); - } - - m_prevTime = curTime; - return; - } - - auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds( - m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime))); - - if (m_usePID) { - auto leftFeedforward = - m_feedforward.Calculate(m_prevSpeeds.left, targetWheelSpeeds.left); - - auto rightFeedforward = - m_feedforward.Calculate(m_prevSpeeds.right, targetWheelSpeeds.right); - - auto leftOutput = - units::volt_t{m_leftController->Calculate( - m_speeds().left.value(), targetWheelSpeeds.left.value())} + - leftFeedforward; - - auto rightOutput = - units::volt_t{m_rightController->Calculate( - m_speeds().right.value(), targetWheelSpeeds.right.value())} + - rightFeedforward; - - m_outputVolts(leftOutput, rightOutput); - } else { - m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right); - } - m_prevSpeeds = targetWheelSpeeds; - m_prevTime = curTime; -} - -void RamseteCommand::End(bool interrupted) { - m_timer.Stop(); - - if (interrupted) { - if (m_usePID) { - m_outputVolts(0_V, 0_V); - } else { - m_outputVel(0_mps, 0_mps); - } - } -} - -bool RamseteCommand::IsFinished() { - return m_timer.HasElapsed(m_trajectory.TotalTime()); -} - -void RamseteCommand::InitSendable(wpi::SendableBuilder& builder) { - Command::InitSendable(builder); - builder.AddDoubleProperty( - "leftVelocity", [this] { return m_prevSpeeds.left.value(); }, nullptr); - builder.AddDoubleProperty( - "rightVelocity", [this] { return m_prevSpeeds.right.value(); }, nullptr); -} diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h deleted file mode 100644 index 5a15f548830..00000000000 --- a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h +++ /dev/null @@ -1,138 +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 -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "frc2/command/Command.h" -#include "frc2/command/CommandHelper.h" -#include "frc2/command/Requirements.h" - -namespace frc2 { -/** - * A command that uses a RAMSETE controller to follow a trajectory - * with a differential drive. - * - *

The command handles trajectory-following, PID calculations, and - * feedforwards internally. This is intended to be a more-or-less "complete - * solution" that can be used by teams without a great deal of controls - * expertise. - * - *

Advanced teams seeking more flexibility (for example, those who wish to - * use the onboard PID functionality of a "smart" motor controller) may use the - * secondary constructor that omits the PID and feedforward functionality, - * returning only the raw wheel speeds from the RAMSETE controller. - * - * This class is provided by the NewCommands VendorDep - * - * @see RamseteController - * @see Trajectory - */ -class RamseteCommand : public CommandHelper { - public: - /** - * Constructs a new RamseteCommand that, when executed, will follow the - * provided trajectory. PID control and feedforward are handled internally, - * and outputs are scaled -12 to 12 representing units of volts. - * - *

Note: The controller will *not* set the outputVolts to zero upon - * completion of the path - this is left to the user, since it is not - * appropriate for paths with nonstationary endstates. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose - use one of - * the odometry classes to provide this. - * @param controller The RAMSETE controller used to follow the - * trajectory. - * @param feedforward A component for calculating the feedforward for the - * drive. - * @param kinematics The kinematics for the robot drivetrain. - * @param wheelSpeeds A function that supplies the speeds of the left - * and right sides of the robot drive. - * @param leftController The PIDController for the left side of the robot - * drive. - * @param rightController The PIDController for the right side of the robot - * drive. - * @param output A function that consumes the computed left and right - * outputs (in volts) for the robot drive. - * @param requirements The subsystems to require. - * @deprecated Use LTVUnicycleController instead. - */ - [[deprecated("Use LTVUnicycleController instead.")]] - RamseteCommand(frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::SimpleMotorFeedforward feedforward, - frc::DifferentialDriveKinematics kinematics, - std::function wheelSpeeds, - frc::PIDController leftController, - frc::PIDController rightController, - std::function output, - Requirements requirements = {}); - - /** - * Constructs a new RamseteCommand that, when executed, will follow the - * provided trajectory. Performs no PID control and calculates no - * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller, - * and will need to be converted into a usable form by the user. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose - use one of - * the odometry classes to provide this. - * @param controller The RAMSETE controller used to follow the - * trajectory. - * @param kinematics The kinematics for the robot drivetrain. - * @param output A function that consumes the computed left and right - * wheel speeds. - * @param requirements The subsystems to require. - * @deprecated Use LTVUnicycleController instead. - */ - [[deprecated("Use LTVUnicycleController instead.")]] - RamseteCommand(frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::DifferentialDriveKinematics kinematics, - std::function output, - Requirements requirements = {}); - - void Initialize() override; - - void Execute() override; - - void End(bool interrupted) override; - - bool IsFinished() override; - - void InitSendable(wpi::SendableBuilder& builder) override; - - private: - frc::Trajectory m_trajectory; - std::function m_pose; - frc::RamseteController m_controller; - frc::SimpleMotorFeedforward m_feedforward; - frc::DifferentialDriveKinematics m_kinematics; - std::function m_speeds; - std::unique_ptr m_leftController; - std::unique_ptr m_rightController; - std::function m_outputVolts; - std::function - m_outputVel; - - frc::Timer m_timer; - units::second_t m_prevTime; - frc::DifferentialDriveWheelSpeeds m_prevSpeeds; - bool m_usePID; -}; -} // namespace frc2 diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index ea40670bb0e..c0354003f2b 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -759,11 +759,10 @@ }, { "name": "SimpleDifferentialDriveSimulation", - "description": "Simulate a differential drivetrain and follow trajectories with RamseteController (non-command-based).", + "description": "Simulate a differential drivetrain and follow trajectories with LTV unicycle controller (non-command-based).", "tags": [ "Differential Drive", "State-Space", - "Ramsete", "Path Following", "Trajectory", "Encoder", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index 3ed52e1bef9..d4b37d9d57f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -650,11 +650,10 @@ }, { "name": "SimpleDifferentialDriveSimulation", - "description": "Simulate a differential drivetrain and follow trajectories with RamseteController (non-command-based).", + "description": "Simulate a differential drivetrain and follow trajectories with LTV unicycle controller (non-command-based).", "tags": [ "Differential Drive", "State-Space", - "Ramsete", "Path Following", "Trajectory", "Encoder", diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java index c5b6defa0d1..f7ce380268f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java @@ -28,11 +28,10 @@ * state-space, then interpolate between them with a lookup table to save computational resources. * *

This controller has a flat hierarchy with pose and wheel velocity references and voltage - * outputs. This is different from a Ramsete controller's nested hierarchy where the top-level + * outputs. This is different from a unicycle controller's nested hierarchy where the top-level * controller has a pose reference and chassis velocity command outputs, and the low-level * controller has wheel velocity references and voltage outputs. Flat hierarchies are easier to tune - * in one shot. Furthermore, this controller is more optimal in the "least-squares error" sense than - * a controller based on Ramsete. + * in one shot. * *

See section 8.7 in Controls Engineering in FRC for a derivation of the control law we used * shown in theorem 8.7.4. diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java index 8437eafac8e..dbe05f73ff7 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java @@ -24,9 +24,6 @@ * compute the controller gain is the nonlinear unicycle model linearized around the drivetrain's * current state. * - *

This controller is a roughly drop-in replacement for {@link RamseteController} with more - * optimal feedback gains in the "least-squares error" sense. - * *

See section 8.9 in Controls Engineering in FRC for a derivation of the control law we used * shown in theorem 8.9.1. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java deleted file mode 100644 index 78e938fe8b1..00000000000 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java +++ /dev/null @@ -1,174 +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.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.Trajectory; - -/** - * Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model - * to a desired pose along a two-dimensional trajectory. Why would we need a nonlinear control law - * in addition to the linear ones we have used so far like PID? If we use the original approach with - * PID controllers for left and right position and velocity states, the controllers only deal with - * the local pose. If the robot deviates from the path, there is no way for the controllers to - * correct and the robot may not reach the desired global pose. This is due to multiple endpoints - * existing for the robot which have the same encoder path arc lengths. - * - *

Instead of using wheel path arc lengths (which are in the robot's local coordinate frame), - * nonlinear controllers like pure pursuit and Ramsete use global pose. The controller uses this - * extra information to guide a linear reference tracker like the PID controllers back in by - * adjusting the references of the PID controllers. - * - *

The paper "Control of Wheeled Mobile Robots: An Experimental Overview" describes a nonlinear - * controller for a wheeled vehicle with unicycle-like kinematics; a global pose consisting of x, y, - * and theta; and a desired pose consisting of x_d, y_d, and theta_d. We call it Ramsete because - * that's the acronym for the title of the book it came from in Italian ("Robotica Articolata e - * Mobile per i SErvizi e le TEcnologie"). - * - *

See Controls - * Engineering in the FIRST Robotics Competition section on Ramsete unicycle controller for a - * derivation and analysis. - */ -public class RamseteController { - private final double m_b; - - private final double m_zeta; - - private Pose2d m_poseError = Pose2d.kZero; - private Pose2d m_poseTolerance = Pose2d.kZero; - private boolean m_enabled = true; - - /** - * Construct a Ramsete unicycle controller. - * - * @param b Tuning parameter (b > 0 rad²/m²) for which larger values make convergence more - * aggressive like a proportional term. - * @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger values provide - * more damping in response. - * @deprecated Use LTVUnicycleController instead. - */ - @Deprecated(since = "2025", forRemoval = true) - public RamseteController(double b, double zeta) { - m_b = b; - m_zeta = zeta; - } - - /** - * Construct a Ramsete unicycle controller. The default arguments for b and zeta of 2.0 rad²/m² - * and 0.7 rad⁻¹ have been well-tested to produce desirable results. - * - * @deprecated Use LTVUnicycleController instead. - */ - @Deprecated(since = "2025", forRemoval = true) - public RamseteController() { - this(2.0, 0.7); - } - - /** - * 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_poseError.getRotation(); - 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 tolerable for use with atReference(). - * - * @param poseTolerance Pose error which is tolerable. - */ - public void setTolerance(Pose2d poseTolerance) { - m_poseTolerance = poseTolerance; - } - - /** - * Returns the next output of the Ramsete controller. - * - *

The reference pose, linear velocity, and angular velocity should come from a drivetrain - * trajectory. - * - * @param currentPose The current pose. - * @param poseRef The desired pose. - * @param linearVelocityRefMeters The desired linear velocity in meters per second. - * @param angularVelocityRefRadiansPerSecond The desired angular velocity in radians per second. - * @return The next controller output. - */ - public ChassisSpeeds calculate( - Pose2d currentPose, - Pose2d poseRef, - double linearVelocityRefMeters, - double angularVelocityRefRadiansPerSecond) { - if (!m_enabled) { - return new ChassisSpeeds(linearVelocityRefMeters, 0.0, angularVelocityRefRadiansPerSecond); - } - - m_poseError = poseRef.relativeTo(currentPose); - - // Aliases for equation readability - final double eX = m_poseError.getX(); - final double eY = m_poseError.getY(); - final double eTheta = m_poseError.getRotation().getRadians(); - final double vRef = linearVelocityRefMeters; - final double omegaRef = angularVelocityRefRadiansPerSecond; - - // k = 2ζ√(ω_ref² + b v_ref²) - double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2)); - - // v_cmd = v_ref cos(e_θ) + k e_x - // ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y - return new ChassisSpeeds( - vRef * m_poseError.getRotation().getCos() + k * eX, - 0.0, - omegaRef + k * eTheta + m_b * vRef * sinc(eTheta) * eY); - } - - /** - * Returns the next output of the Ramsete controller. - * - *

The reference pose, linear velocity, and angular velocity should come from a drivetrain - * trajectory. - * - * @param currentPose The current pose. - * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory. - * @return The next controller output. - */ - public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) { - return calculate( - currentPose, - desiredState.poseMeters, - desiredState.velocityMetersPerSecond, - desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter); - } - - /** - * Enables and disables the controller for troubleshooting purposes. - * - * @param enabled If the controller is enabled or not. - */ - public void setEnabled(boolean enabled) { - m_enabled = enabled; - } - - /** - * Returns sin(x) / x. - * - * @param x Value of which to take sinc(x). - */ - private static double sinc(double x) { - if (Math.abs(x) < 1e-9) { - return 1.0 - 1.0 / 6.0 * x * x; - } else { - return Math.sin(x) / x; - } - } -} diff --git a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h index c07c1cef782..59b99f31594 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h @@ -27,12 +27,11 @@ namespace frc { * between them with a lookup table to save computational resources. * * This controller has a flat hierarchy with pose and wheel velocity references - * and voltage outputs. This is different from a Ramsete controller's nested + * and voltage outputs. This is different from a unicycle controller's nested * hierarchy where the top-level controller has a pose reference and chassis * velocity command outputs, and the low-level controller has wheel velocity * references and voltage outputs. Flat hierarchies are easier to tune in one - * shot. Furthermore, this controller is more optimal in the "least-squares - * error" sense than a controller based on Ramsete. + * shot. * * See section 8.7 in Controls Engineering in FRC for a derivation of the * control law we used shown in theorem 8.7.4. diff --git a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h index 02f5271ad3e..c5825b3b30f 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h @@ -23,9 +23,6 @@ namespace frc { * but the model used to compute the controller gain is the nonlinear unicycle * model linearized around the drivetrain's current state. * - * This controller is a roughly drop-in replacement for RamseteController with - * more optimal feedback gains in the "least-squares error" sense. - * * See section 8.9 in Controls Engineering in FRC for a derivation of the * control law we used shown in theorem 8.9.1. */ diff --git a/wpimath/src/main/native/include/frc/controller/RamseteController.h b/wpimath/src/main/native/include/frc/controller/RamseteController.h deleted file mode 100644 index 1a31bc9ce1a..00000000000 --- a/wpimath/src/main/native/include/frc/controller/RamseteController.h +++ /dev/null @@ -1,193 +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 -#include - -#include "frc/geometry/Pose2d.h" -#include "frc/kinematics/ChassisSpeeds.h" -#include "frc/trajectory/Trajectory.h" -#include "units/angle.h" -#include "units/angular_velocity.h" -#include "units/length.h" -#include "units/math.h" -#include "units/velocity.h" - -namespace frc { - -/** - * Ramsete is a nonlinear time-varying feedback controller for unicycle models - * that drives the model to a desired pose along a two-dimensional trajectory. - * Why would we need a nonlinear control law in addition to the linear ones we - * have used so far like PID? If we use the original approach with PID - * controllers for left and right position and velocity states, the controllers - * only deal with the local pose. If the robot deviates from the path, there is - * no way for the controllers to correct and the robot may not reach the desired - * global pose. This is due to multiple endpoints existing for the robot which - * have the same encoder path arc lengths. - * - * Instead of using wheel path arc lengths (which are in the robot's local - * coordinate frame), nonlinear controllers like pure pursuit and Ramsete use - * global pose. The controller uses this extra information to guide a linear - * reference tracker like the PID controllers back in by adjusting the - * references of the PID controllers. - * - * The paper "Control of Wheeled Mobile Robots: An Experimental Overview" - * describes a nonlinear controller for a wheeled vehicle with unicycle-like - * kinematics; a global pose consisting of x, y, and theta; and a desired pose - * consisting of x_d, y_d, and theta_d. We call it Ramsete because that's the - * acronym for the title of the book it came from in Italian ("Robotica - * Articolata e Mobile per i SErvizi e le TEcnologie"). - * - * See section - * on Ramsete unicycle controller for a derivation and analysis. - */ -class WPILIB_DLLEXPORT RamseteController { - public: - using b_unit = - units::compound_unit, - units::inverse>>; - using zeta_unit = units::inverse; - - /** - * Construct a Ramsete unicycle controller. - * - * @param b Tuning parameter (b > 0 rad²/m²) for which larger values make - * convergence more aggressive like a proportional term. - * @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger - * values provide more damping in response. - * @deprecated Use LTVUnicycleController instead. - */ - [[deprecated("Use LTVUnicycleController instead.")]] - constexpr RamseteController(units::unit_t b, - units::unit_t zeta) - : m_b{b}, m_zeta{zeta} {} - - WPI_IGNORE_DEPRECATED - - /** - * Construct a Ramsete unicycle controller. The default arguments for - * b and zeta of 2.0 rad²/m² and 0.7 rad⁻¹ have been well-tested to produce - * desirable results. - * - * @deprecated Use LTVUnicycleController instead. - */ - [[deprecated("Use LTVUnicycleController instead.")]] - constexpr RamseteController() - : RamseteController{units::unit_t{2.0}, - units::unit_t{0.7}} {} - - WPI_UNIGNORE_DEPRECATED - - /** - * 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_poseError.Rotation(); - 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 poseTolerance Pose error which is tolerable. - */ - constexpr void SetTolerance(const Pose2d& poseTolerance) { - m_poseTolerance = poseTolerance; - } - - /** - * Returns the next output of the Ramsete controller. - * - * The reference pose, linear velocity, and angular velocity should come from - * a drivetrain trajectory. - * - * @param currentPose The current pose. - * @param poseRef The desired pose. - * @param linearVelocityRef The desired linear velocity. - * @param angularVelocityRef The desired angular velocity. - */ - constexpr ChassisSpeeds Calculate( - const Pose2d& currentPose, const Pose2d& poseRef, - units::meters_per_second_t linearVelocityRef, - units::radians_per_second_t angularVelocityRef) { - if (!m_enabled) { - return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef}; - } - - m_poseError = poseRef.RelativeTo(currentPose); - - // Aliases for equation readability - const auto& eX = m_poseError.X(); - const auto& eY = m_poseError.Y(); - const auto& eTheta = m_poseError.Rotation().Radians(); - const auto& vRef = linearVelocityRef; - const auto& omegaRef = angularVelocityRef; - - // k = 2ζ√(ω_ref² + b v_ref²) - auto k = 2.0 * m_zeta * - units::math::sqrt(units::math::pow<2>(omegaRef) + - m_b * units::math::pow<2>(vRef)); - - // v_cmd = v_ref cos(e_θ) + k e_x - // ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y - return ChassisSpeeds{ - vRef * m_poseError.Rotation().Cos() + k * eX, 0_mps, - omegaRef + k * eTheta + m_b * vRef * Sinc(eTheta) * eY}; - } - - /** - * Returns the next output of the Ramsete controller. - * - * The reference pose, linear velocity, and angular velocity should come from - * a drivetrain trajectory. - * - * @param currentPose The current pose. - * @param desiredState The desired pose, linear velocity, and angular velocity - * from a trajectory. - */ - constexpr ChassisSpeeds Calculate(const Pose2d& currentPose, - const Trajectory::State& desiredState) { - return Calculate(currentPose, desiredState.pose, desiredState.velocity, - desiredState.velocity * desiredState.curvature); - } - - /** - * Enables and disables the controller for troubleshooting purposes. - * - * @param enabled If the controller is enabled or not. - */ - constexpr void SetEnabled(bool enabled) { m_enabled = enabled; } - - private: - units::unit_t m_b; - units::unit_t m_zeta; - - Pose2d m_poseError; - Pose2d m_poseTolerance; - bool m_enabled = true; - - /** - * Returns sin(x) / x. - * - * @param x Value of which to take sinc(x). - */ - static constexpr decltype(1 / 1_rad) Sinc(units::radian_t x) { - if (units::math::abs(x) < 1e-9_rad) { - return decltype(1 / 1_rad){1.0 - 1.0 / 6.0 * x.value() * x.value()}; - } else { - return units::math::sin(x) / x; - } - } -}; - -} // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java deleted file mode 100644 index de76e528a09..00000000000 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java +++ /dev/null @@ -1,62 +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.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import java.util.ArrayList; -import org.junit.jupiter.api.Test; - -class RamseteControllerTest { - private static final double kTolerance = 1 / 12.0; - private static final double kAngularTolerance = Math.toRadians(2); - - @SuppressWarnings("removal") - @Test - void testReachesReference() { - final var controller = new RamseteController(2.0, 0.7); - var robotPose = new Pose2d(2.7, 23.0, Rotation2d.kZero); - - final var waypoints = new ArrayList(); - waypoints.add(new Pose2d(2.75, 22.521, Rotation2d.kZero)); - waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846))); - var config = new TrajectoryConfig(8.8, 0.1); - final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); - - final double kDt = 0.02; - final var totalTime = trajectory.getTotalTimeSeconds(); - for (int i = 0; i < (totalTime / kDt); ++i) { - var state = trajectory.sample(kDt * i); - - var output = controller.calculate(robotPose, state); - robotPose = - robotPose.exp( - new Twist2d(output.vxMetersPerSecond * kDt, 0, output.omegaRadiansPerSecond * kDt)); - } - - final var states = trajectory.getStates(); - final var endPose = states.get(states.size() - 1).poseMeters; - - // Java lambdas require local variables referenced from a lambda expression - // must be final or effectively final. - final var finalRobotPose = robotPose; - assertAll( - () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance), - () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance), - () -> - assertEquals( - 0.0, - MathUtil.angleModulus( - endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()), - kAngularTolerance)); - } -} diff --git a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp deleted file mode 100644 index 8c39cac31ba..00000000000 --- a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp +++ /dev/null @@ -1,50 +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 -#include - -#include "frc/MathUtil.h" -#include "frc/controller/RamseteController.h" -#include "frc/trajectory/TrajectoryGenerator.h" -#include "units/math.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}; - -WPI_IGNORE_DEPRECATED - -TEST(RamseteControllerTest, ReachesReference) { - frc::RamseteController controller{2.0 * 1_rad * 1_rad / (1_m * 1_m), - 0.7 / 1_rad}; - 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.8_mps, 0.1_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); - static_cast(vy); - - robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, 0_m, 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(endPose.Rotation().Radians() - - robotPose.Rotation().Radians()), - 0_rad, kAngularTolerance); -} - -WPI_UNIGNORE_DEPRECATED