diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java new file mode 100644 index 0000000000..4d0c94f0be --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java @@ -0,0 +1,70 @@ +// 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.VecBuilder; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.LinearSystemId; + +/** A helper class which computes the feedforward outputs for a differential drive drivetrain. */ +public class DifferentialDriveFeedforward { + private final LinearSystem m_plant; + + /** + * Creates a new DifferentialDriveFeedforward with the specified parameters. + * + * @param kVLinear The linear velocity gain in volts per (meters per second). + * @param kALinear The linear acceleration gain in volts per (meters per second squared). + * @param kVAngular The angular velocity gain in volts per (radians per second). + * @param kAAngular The angular acceleration gain in volts per (radians per second squared). + * @param trackwidth The distance between the differential drive's left and right wheels, in + * meters. + */ + public DifferentialDriveFeedforward( + double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) { + m_plant = + LinearSystemId.identifyDrivetrainSystem( + kVLinear, kALinear, kVAngular, kAAngular, trackwidth); + } + + /** + * Creates a new DifferentialDriveFeedforward with the specified parameters. + * + * @param kVLinear The linear velocity gain in volts per (meters per second). + * @param kALinear The linear acceleration gain in volts per (meters per second squared). + * @param kVAngular The angular velocity gain in volts per (meters per second). + * @param kAAngular The angular acceleration gain in volts per (meters per second squared). + */ + public DifferentialDriveFeedforward( + double kVLinear, double kALinear, double kVAngular, double kAAngular) { + m_plant = LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular); + } + + /** + * Calculates the differential drive feedforward inputs given velocity setpoints. + * + * @param currentLeftVelocity The current left velocity of the differential drive in + * meters/second. + * @param nextLeftVelocity The next left velocity of the differential drive in meters/second. + * @param currentRightVelocity The current right velocity of the differential drive in + * meters/second. + * @param nextRightVelocity The next right velocity of the differential drive in meters/second. + * @param dtSeconds Discretization timestep. + * @return A DifferentialDriveWheelVoltages object containing the computed feedforward voltages. + */ + public DifferentialDriveWheelVoltages calculate( + double currentLeftVelocity, + double nextLeftVelocity, + double currentRightVelocity, + double nextRightVelocity, + double dtSeconds) { + var feedforward = new LinearPlantInversionFeedforward<>(m_plant, dtSeconds); + var r = VecBuilder.fill(currentLeftVelocity, currentRightVelocity); + var nextR = VecBuilder.fill(nextLeftVelocity, nextRightVelocity); + var u = feedforward.calculate(r, nextR); + return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0)); + } +} diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp new file mode 100644 index 0000000000..e1b2732642 --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp @@ -0,0 +1,37 @@ +// 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 "frc/controller/DifferentialDriveFeedforward.h" + +#include "frc/EigenCore.h" +#include "frc/controller/LinearPlantInversionFeedforward.h" +#include "frc/system/plant/LinearSystemId.h" + +using namespace frc; + +DifferentialDriveFeedforward::DifferentialDriveFeedforward( + decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, + decltype(1_V / 1_rad_per_s) kVAngular, + decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth) + : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem( + kVLinear, kALinear, kVAngular, kAAngular, trackwidth)} {} + +DifferentialDriveFeedforward::DifferentialDriveFeedforward( + decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, + decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) + : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem( + kVLinear, kALinear, kVAngular, kAAngular)} {} + +DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate( + units::meters_per_second_t currentLeftVelocity, + units::meters_per_second_t nextLeftVelocity, + units::meters_per_second_t currentRightVelocity, + units::meters_per_second_t nextRightVelocity, units::second_t dt) { + frc::LinearPlantInversionFeedforward<2, 2> feedforward{m_plant, dt}; + + frc::Vectord<2> r{currentLeftVelocity, currentRightVelocity}; + frc::Vectord<2> nextR{nextLeftVelocity, nextRightVelocity}; + auto u = feedforward.Calculate(r, nextR); + return {units::volt_t{u(0)}, units::volt_t{u(1)}}; +} diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h new file mode 100644 index 0000000000..716bc781ad --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h @@ -0,0 +1,83 @@ +// 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 "frc/controller/DifferentialDriveWheelVoltages.h" +#include "frc/system/LinearSystem.h" +#include "units/acceleration.h" +#include "units/angular_acceleration.h" +#include "units/angular_velocity.h" +#include "units/length.h" +#include "units/time.h" +#include "units/velocity.h" +#include "units/voltage.h" + +namespace frc { +/** + * A helper class which computes the feedforward outputs for a differential + * drive drivetrain. + */ +class WPILIB_DLLEXPORT DifferentialDriveFeedforward { + frc::LinearSystem<2, 2, 2> m_plant; + + public: + /** + * Creates a new DifferentialDriveFeedforward with the specified parameters. + * + * @param kVLinear The linear velocity gain in volts per (meters per second). + * @param kALinear The linear acceleration gain in volts per (meters per + * second squared). + * @param kVAngular The angular velocity gain in volts per (radians per + * second). + * @param kAAngular The angular acceleration gain in volts per (radians per + * second squared). + * @param trackwidth The distance between the differential drive's left and + * right wheels, in meters. + */ + DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear, + decltype(1_V / 1_mps_sq) kALinear, + decltype(1_V / 1_rad_per_s) kVAngular, + decltype(1_V / 1_rad_per_s_sq) kAAngular, + units::meter_t trackwidth); + + /** + * Creates a new DifferentialDriveFeedforward with the specified parameters. + * + * @param kVLinear The linear velocity gain in volts per (meters per second). + * @param kALinear The linear acceleration gain in volts per (meters per + * second squared). + * @param kVAngular The angular velocity gain in volts per (meters per + * second). + * @param kAAngular The angular acceleration gain in volts per (meters per + * second squared). + */ + DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear, + decltype(1_V / 1_mps_sq) kALinear, + decltype(1_V / 1_mps) kVAngular, + decltype(1_V / 1_mps_sq) kAAngular); + + /** + * Calculates the differential drive feedforward inputs given velocity + * setpoints. + * + * @param currentLeftVelocity The current left velocity of the differential + * drive in meters/second. + * @param nextLeftVelocity The next left velocity of the differential drive in + * meters/second. + * @param currentRightVelocity The current right velocity of the differential + * drive in meters/second. + * @param nextRightVelocity The next right velocity of the differential drive + * in meters/second. + * @param dt Discretization timestep. + */ + DifferentialDriveWheelVoltages Calculate( + units::meters_per_second_t currentLeftVelocity, + units::meters_per_second_t nextLeftVelocity, + units::meters_per_second_t currentRightVelocity, + units::meters_per_second_t nextRightVelocity, units::second_t dt); +}; +} // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java new file mode 100644 index 0000000000..fdf53eab74 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java @@ -0,0 +1,85 @@ +// 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.assertEquals; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.LinearSystemId; +import org.junit.jupiter.api.Test; + +class DifferentialDriveFeedforwardTest { + private static final double kVLinear = 1.0; + private static final double kALinear = 1.0; + private static final double kVAngular = 1.0; + private static final double kAAngular = 1.0; + private static final double trackwidth = 1.0; + private static final double dtSeconds = 0.02; + + @Test + void testCalculateWithTrackwidth() { + DifferentialDriveFeedforward differentialDriveFeedforward = + new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular, trackwidth); + LinearSystem plant = + LinearSystemId.identifyDrivetrainSystem( + kVLinear, kALinear, kVAngular, kAAngular, trackwidth); + for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) { + for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) { + for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) { + for (int nextRightVelocity = -4; nextRightVelocity <= 4; nextRightVelocity += 2) { + DifferentialDriveWheelVoltages u = + differentialDriveFeedforward.calculate( + currentLeftVelocity, + nextLeftVelocity, + currentRightVelocity, + nextRightVelocity, + dtSeconds); + Matrix nextX = + plant.calculateX( + VecBuilder.fill(currentLeftVelocity, currentRightVelocity), + VecBuilder.fill(u.left, u.right), + dtSeconds); + assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6); + assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6); + } + } + } + } + } + + @Test + void testCalculateWithoutTrackwidth() { + DifferentialDriveFeedforward differentialDriveFeedforward = + new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular); + LinearSystem plant = + LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular); + for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) { + for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) { + for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) { + for (int nextRightVelocity = -4; nextRightVelocity <= 4; nextRightVelocity += 2) { + DifferentialDriveWheelVoltages u = + differentialDriveFeedforward.calculate( + currentLeftVelocity, + nextLeftVelocity, + currentRightVelocity, + nextRightVelocity, + dtSeconds); + Matrix nextX = + plant.calculateX( + VecBuilder.fill(currentLeftVelocity, currentRightVelocity), + VecBuilder.fill(u.left, u.right), + dtSeconds); + assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6); + assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6); + } + } + } + } + } +} diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp new file mode 100644 index 0000000000..74d945c351 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp @@ -0,0 +1,84 @@ +// 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/EigenCore.h" +#include "frc/controller/DifferentialDriveFeedforward.h" +#include "frc/controller/LinearPlantInversionFeedforward.h" +#include "frc/system/plant/LinearSystemId.h" +#include "units/acceleration.h" +#include "units/length.h" +#include "units/time.h" + +TEST(DifferentialDriveFeedforwardTest, CalculateWithTrackwidth) { + constexpr auto kVLinear = 1_V / 1_mps; + constexpr auto kALinear = 1_V / 1_mps_sq; + constexpr auto kVAngular = 1_V / 1_rad_per_s; + constexpr auto kAAngular = 1_V / 1_rad_per_s_sq; + constexpr auto trackwidth = 1_m; + constexpr auto dt = 20_ms; + + frc::DifferentialDriveFeedforward differentialDriveFeedforward{ + kVLinear, kALinear, kVAngular, kAAngular, trackwidth}; + frc::LinearSystem<2, 2, 2> plant = + frc::LinearSystemId::IdentifyDrivetrainSystem( + kVLinear, kALinear, kVAngular, kAAngular, trackwidth); + for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps; + currentLeftVelocity += 2_mps) { + for (auto currentRightVelocity = -4_mps; currentRightVelocity <= 4_mps; + currentRightVelocity += 2_mps) { + for (auto nextLeftVelocity = -4_mps; nextLeftVelocity <= 4_mps; + nextLeftVelocity += 2_mps) { + for (auto nextRightVelocity = -4_mps; nextRightVelocity <= 4_mps; + nextRightVelocity += 2_mps) { + auto [left, right] = differentialDriveFeedforward.Calculate( + currentLeftVelocity, nextLeftVelocity, currentRightVelocity, + nextRightVelocity, dt); + frc::Matrixd<2, 1> nextX = plant.CalculateX( + frc::Vectord<2>{currentLeftVelocity, currentRightVelocity}, + frc::Vectord<2>{left, right}, dt); + EXPECT_NEAR(nextX(0), nextLeftVelocity.value(), 1e-6); + EXPECT_NEAR(nextX(1), nextRightVelocity.value(), 1e-6); + } + } + } + } +} + +TEST(DifferentialDriveFeedforwardTest, CalculateWithoutTrackwidth) { + constexpr auto kVLinear = 1_V / 1_mps; + constexpr auto kALinear = 1_V / 1_mps_sq; + constexpr auto kVAngular = 1_V / 1_mps; + constexpr auto kAAngular = 1_V / 1_mps_sq; + constexpr auto dt = 20_ms; + + frc::DifferentialDriveFeedforward differentialDriveFeedforward{ + kVLinear, kALinear, kVAngular, kAAngular}; + frc::LinearSystem<2, 2, 2> plant = + frc::LinearSystemId::IdentifyDrivetrainSystem(kVLinear, kALinear, + kVAngular, kAAngular); + for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps; + currentLeftVelocity += 2_mps) { + for (auto currentRightVelocity = -4_mps; currentRightVelocity <= 4_mps; + currentRightVelocity += 2_mps) { + for (auto nextLeftVelocity = -4_mps; nextLeftVelocity <= 4_mps; + nextLeftVelocity += 2_mps) { + for (auto nextRightVelocity = -4_mps; nextRightVelocity <= 4_mps; + nextRightVelocity += 2_mps) { + auto [left, right] = differentialDriveFeedforward.Calculate( + currentLeftVelocity, nextLeftVelocity, currentRightVelocity, + nextRightVelocity, dt); + frc::Matrixd<2, 1> nextX = plant.CalculateX( + frc::Vectord<2>{currentLeftVelocity, currentRightVelocity}, + frc::Vectord<2>{left, right}, dt); + EXPECT_NEAR(nextX(0), nextLeftVelocity.value(), 1e-6); + EXPECT_NEAR(nextX(1), nextRightVelocity.value(), 1e-6); + } + } + } + } +}