Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Remove RamseteController and RamseteCommand (#7522)

Browse Source
This commit is contained in:
Tyler Veness
2024-12-07 23:38:35 -08:00
committed by GitHub
parent ae44295024
commit 220f4e1ba4
14 changed files with 8 additions and 996 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
shared/examplecheck.gradle
View File

@@ -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",

227
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
View File

@@ -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.
*
* <p>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.
*
* <p>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.
*
* <p>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<Pose2d> m_pose;
private final RamseteController m_follower;
private final SimpleMotorFeedforward m_feedforward;
private final DifferentialDriveKinematics m_kinematics;
private final Supplier<DifferentialDriveWheelSpeeds> m_speeds;
private final PIDController m_leftController;
private final PIDController m_rightController;
private final BiConsumer<Double, Double> 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.
*
* <p>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<Pose2d> pose,
RamseteController controller,
SimpleMotorFeedforward feedforward,
DifferentialDriveKinematics kinematics,
Supplier<DifferentialDriveWheelSpeeds> wheelSpeeds,
PIDController leftController,
PIDController rightController,
BiConsumer<Double, Double> 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<Pose2d> pose,
RamseteController follower,
DifferentialDriveKinematics kinematics,
BiConsumer<Double, Double> 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);
}
}

133
wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
View File

@@ -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 <utility>
#include <units/velocity.h>
#include <units/voltage.h>
#include <wpi/sendable/SendableBuilder.h>
using namespace frc2;
using kv_unit = units::compound_unit<units::volts,
units::inverse<units::meters_per_second>>;
RamseteCommand::RamseteCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::RamseteController controller,
frc::SimpleMotorFeedforward<units::meters> feedforward,
frc::DifferentialDriveKinematics kinematics,
std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
frc::PIDController leftController, frc::PIDController rightController,
std::function<void(units::volt_t, units::volt_t)> 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<frc::PIDController>(leftController)),
m_rightController(std::make_unique<frc::PIDController>(rightController)),
m_outputVolts(std::move(output)),
m_usePID(true) {
AddRequirements(requirements);
}
RamseteCommand::RamseteCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::RamseteController controller,
frc::DifferentialDriveKinematics kinematics,
std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
output,
Requirements requirements)
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_controller(controller),
m_feedforward(0_V, units::unit_t<kv_unit>{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);
}

138
wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
View File

@@ -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 <functional>
#include <memory>
#include <frc/Timer.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/RamseteController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/trajectory/Trajectory.h>
#include <units/length.h>
#include <units/voltage.h>
#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.
*
* <p>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.
*
* <p>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<Command, RamseteCommand> {
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.
*
* <p>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<frc::Pose2d()> pose,
frc::RamseteController controller,
frc::SimpleMotorFeedforward<units::meters> feedforward,
frc::DifferentialDriveKinematics kinematics,
std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
frc::PIDController leftController,
frc::PIDController rightController,
std::function<void(units::volt_t, units::volt_t)> 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<frc::Pose2d()> pose,
frc::RamseteController controller,
frc::DifferentialDriveKinematics kinematics,
std::function<void(units::meters_per_second_t,
units::meters_per_second_t)> 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<frc::Pose2d()> m_pose;
frc::RamseteController m_controller;
frc::SimpleMotorFeedforward<units::meters> m_feedforward;
frc::DifferentialDriveKinematics m_kinematics;
std::function<frc::DifferentialDriveWheelSpeeds()> m_speeds;
std::unique_ptr<frc::PIDController> m_leftController;
std::unique_ptr<frc::PIDController> m_rightController;
std::function<void(units::volt_t, units::volt_t)> m_outputVolts;
std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
m_outputVel;
frc::Timer m_timer;
units::second_t m_prevTime;
frc::DifferentialDriveWheelSpeeds m_prevSpeeds;
bool m_usePID;
};
} // namespace frc2

3
wpilibcExamples/src/main/cpp/examples/examples.json
View File

@@ -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",

3
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
View File

@@ -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",

5
wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
View File

@@ -27,11 +27,10 @@ import edu.wpi.first.math.trajectory.Trajectory;
* state-space, then interpolate between them with a lookup table to save computational resources.
*
* <p>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.
*
* <p>See section 8.7 in Controls Engineering in FRC for a derivation of the control law we used
* shown in theorem 8.7.4.

3
wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
View File

@@ -23,9 +23,6 @@ import edu.wpi.first.math.trajectory.Trajectory;
* compute the controller gain is the nonlinear unicycle model linearized around the drivetrain's
* current state.
*
* <p>This controller is a roughly drop-in replacement for {@link RamseteController} with more
* optimal feedback gains in the "least-squares error" sense.
*
* <p>See section 8.9 in Controls Engineering in FRC for a derivation of the control law we used
* shown in theorem 8.9.1.
*/

174
wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
View File

@@ -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.
*
* <p>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.
*
* <p>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").
*
* <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
* Engineering in the FIRST Robotics Competition</a> 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 &gt; 0 rad²/m²) for which larger values make convergence more
* aggressive like a proportional term.
* @param zeta Tuning parameter (0 rad⁻¹ &lt; zeta &lt; 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.
*
* <p>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.
*
* <p>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;
}
}
}

5
wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
View File

@@ -29,12 +29,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.

3
wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
View File

@@ -24,9 +24,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.
*/

193
wpimath/src/main/native/include/frc/controller/RamseteController.h
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/deprecated.h>
#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 <https://file.tavsys.net/control/controls-engineering-in-frc.pdf> section
* on Ramsete unicycle controller for a derivation and analysis.
*/
class WPILIB_DLLEXPORT RamseteController {
public:
using b_unit =
units::compound_unit<units::squared<units::radians>,
units::inverse<units::squared<units::meters>>>;
using zeta_unit = units::inverse<units::radians>;
/**
* 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_unit> b,
units::unit_t<zeta_unit> 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<b_unit>{2.0},
units::unit_t<zeta_unit>{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<b_unit> m_b;
units::unit_t<zeta_unit> 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

62
wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java
View File

@@ -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<Pose2d>();
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));
}
}

50
wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
View File

@@ -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 <gtest/gtest.h>
#include <wpi/deprecated.h>
#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<void>(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