mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-30 02:31:44 +00:00
[wpimath] Add DifferentialDriveFeedforward classes which wrap LinearPlantInversionFeedforward (#4598)
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
CarloWoolsey
@@ -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<N2, N2, N2> 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));
|
||||
}
|
||||
}
|
||||
@@ -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)}};
|
||||
}
|
||||
@@ -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 <wpi/SymbolExports.h>
|
||||
|
||||
#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
|
||||
@@ -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<N2, N2, N2> 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<N2, N1> 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<N2, N2, N2> 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<N2, N1> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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 <gtest/gtest.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user