mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
[wpimath] Remove LUTs from LTV controllers (#7521)
The Raspberry Pi 5 is fast enough that we no longer need it. ``` Running ./build/DAREBench Run on (4 X 2400 MHz CPU s) CPU Caches: L1 Data 64 KiB (x4) L1 Instruction 64 KiB (x4) L2 Unified 512 KiB (x4) L3 Unified 2048 KiB (x1) Load Average: 0.47, 0.72, 0.45 ***WARNING*** CPU scaling is enabled, the benchmark real time measurements may be noisy and will incur extra overhead. ------------------------------------------------------------------------------- Benchmark Time CPU Iterations ------------------------------------------------------------------------------- DARE_WPIMath_Dynamic 34.4 us 34.4 us 20315 DARE_WPIMath_NoPrecondChecks_Dynamic 21.7 us 21.7 us 32266 DARE_WPIMath_Static 15.2 us 15.2 us 45878 DARE_WPIMath_NoPrecondChecks_Static 7.84 us 7.84 us 89316 DARE_SLICOT 79.4 us 79.4 us 8789 DARE_Drake 34.9 us 34.9 us 20074 ```
This commit is contained in:
Tyler Veness
@@ -5,177 +5,77 @@
|
||||
#include "frc/controller/LTVDifferentialDriveController.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
|
||||
#include <Eigen/Cholesky>
|
||||
|
||||
#include "frc/DARE.h"
|
||||
#include "frc/MathUtil.h"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/system/Discretization.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
namespace {
|
||||
|
||||
/**
|
||||
* States of the drivetrain system.
|
||||
*/
|
||||
class State {
|
||||
public:
|
||||
/// X position in global coordinate frame.
|
||||
[[maybe_unused]]
|
||||
static constexpr int kX = 0;
|
||||
|
||||
/// Y position in global coordinate frame.
|
||||
static constexpr int kY = 1;
|
||||
|
||||
/// Heading in global coordinate frame.
|
||||
static constexpr int kHeading = 2;
|
||||
|
||||
/// Left encoder velocity.
|
||||
[[maybe_unused]]
|
||||
static constexpr int kLeftVelocity = 3;
|
||||
|
||||
/// Right encoder velocity.
|
||||
[[maybe_unused]]
|
||||
static constexpr int kRightVelocity = 4;
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
LTVDifferentialDriveController::LTVDifferentialDriveController(
|
||||
const frc::LinearSystem<2, 2, 2>& plant, units::meter_t trackwidth,
|
||||
const wpi::array<double, 5>& Qelems, const wpi::array<double, 2>& Relems,
|
||||
units::second_t dt)
|
||||
: m_trackwidth{trackwidth} {
|
||||
// Control law derivation is in section 8.7 of
|
||||
// https://file.tavsys.net/control/controls-engineering-in-frc.pdf
|
||||
Matrixd<5, 5> A{
|
||||
{0.0, 0.0, 0.0, 0.5, 0.5},
|
||||
{0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, -1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()},
|
||||
{0.0, 0.0, 0.0, plant.A(0, 0), plant.A(0, 1)},
|
||||
{0.0, 0.0, 0.0, plant.A(1, 0), plant.A(1, 1)}};
|
||||
Matrixd<5, 2> B{{0.0, 0.0},
|
||||
{0.0, 0.0},
|
||||
{0.0, 0.0},
|
||||
{plant.B(0, 0), plant.B(0, 1)},
|
||||
{plant.B(1, 0), plant.B(1, 1)}};
|
||||
Matrixd<5, 5> Q = frc::MakeCostMatrix(Qelems);
|
||||
Matrixd<2, 2> R = frc::MakeCostMatrix(Relems);
|
||||
|
||||
// dx/dt = Ax + Bu
|
||||
// 0 = Ax + Bu
|
||||
// Ax = -Bu
|
||||
// x = -A⁻¹Bu
|
||||
units::meters_per_second_t maxV{
|
||||
// NOLINTNEXTLINE(clang-analyzer-unix.Malloc)
|
||||
-plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)};
|
||||
|
||||
if (maxV <= 0_mps) {
|
||||
throw std::domain_error(
|
||||
"Max velocity of plant with 12 V input must be greater than 0 m/s.");
|
||||
}
|
||||
if (maxV >= 15_mps) {
|
||||
throw std::domain_error(
|
||||
"Max velocity of plant with 12 V input must be less than 15 m/s.");
|
||||
}
|
||||
|
||||
auto R_llt = R.llt();
|
||||
|
||||
for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) {
|
||||
// The DARE is ill-conditioned if the velocity is close to zero, so don't
|
||||
// let the system stop.
|
||||
if (units::math::abs(velocity) < 1e-4_mps) {
|
||||
A(State::kY, State::kHeading) = 1e-4;
|
||||
} else {
|
||||
A(State::kY, State::kHeading) = velocity.value();
|
||||
}
|
||||
|
||||
Matrixd<5, 5> discA;
|
||||
Matrixd<5, 2> discB;
|
||||
DiscretizeAB(A, B, dt, &discA, &discB);
|
||||
|
||||
auto S = detail::DARE<5, 2>(discA, discB, Q, R_llt);
|
||||
|
||||
// K = (BᵀSB + R)⁻¹BᵀSA
|
||||
m_table.insert(velocity, (discB.transpose() * S * discB + R)
|
||||
.llt()
|
||||
.solve(discB.transpose() * S * discA));
|
||||
}
|
||||
}
|
||||
|
||||
bool LTVDifferentialDriveController::AtReference() const {
|
||||
return std::abs(m_error(0)) < m_tolerance(0) &&
|
||||
std::abs(m_error(1)) < m_tolerance(1) &&
|
||||
std::abs(m_error(2)) < m_tolerance(2) &&
|
||||
std::abs(m_error(3)) < m_tolerance(3) &&
|
||||
std::abs(m_error(4)) < m_tolerance(4);
|
||||
}
|
||||
|
||||
void LTVDifferentialDriveController::SetTolerance(
|
||||
const Pose2d& poseTolerance,
|
||||
units::meters_per_second_t leftVelocityTolerance,
|
||||
units::meters_per_second_t rightVelocityTolerance) {
|
||||
m_tolerance =
|
||||
Vectord<5>{poseTolerance.X().value(), poseTolerance.Y().value(),
|
||||
poseTolerance.Rotation().Radians().value(),
|
||||
leftVelocityTolerance.value(), rightVelocityTolerance.value()};
|
||||
}
|
||||
|
||||
DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate(
|
||||
const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
|
||||
units::meters_per_second_t rightVelocity, const Pose2d& poseRef,
|
||||
units::meters_per_second_t leftVelocityRef,
|
||||
units::meters_per_second_t rightVelocityRef) {
|
||||
// This implements the linear time-varying differential drive controller in
|
||||
// theorem 9.6.3 of https://tavsys.net/controls-in-frc.
|
||||
Vectord<5> x{currentPose.X().value(), currentPose.Y().value(),
|
||||
currentPose.Rotation().Radians().value(), leftVelocity.value(),
|
||||
rightVelocity.value()};
|
||||
|
||||
Matrixd<5, 5> inRobotFrame = Matrixd<5, 5>::Identity();
|
||||
inRobotFrame(0, 0) = std::cos(x(State::kHeading));
|
||||
inRobotFrame(0, 1) = std::sin(x(State::kHeading));
|
||||
inRobotFrame(1, 0) = -std::sin(x(State::kHeading));
|
||||
inRobotFrame(1, 1) = std::cos(x(State::kHeading));
|
||||
|
||||
Vectord<5> r{poseRef.X().value(), poseRef.Y().value(),
|
||||
poseRef.Rotation().Radians().value(), leftVelocityRef.value(),
|
||||
rightVelocityRef.value()};
|
||||
m_error = r - x;
|
||||
m_error(State::kHeading) =
|
||||
frc::AngleModulus(units::radian_t{m_error(State::kHeading)}).value();
|
||||
// theorem 8.7.4 of https://controls-in-frc.link/
|
||||
//
|
||||
// [x ]
|
||||
// [y ] [Vₗ]
|
||||
// x = [θ ] u = [Vᵣ]
|
||||
// [vₗ]
|
||||
// [vᵣ]
|
||||
|
||||
units::meters_per_second_t velocity{(leftVelocity + rightVelocity) / 2.0};
|
||||
const auto& K = m_table[velocity];
|
||||
|
||||
Vectord<2> u = K * inRobotFrame * m_error;
|
||||
// The DARE is ill-conditioned if the velocity is close to zero, so don't
|
||||
// let the system stop.
|
||||
if (units::math::abs(velocity) < 1e-4_mps) {
|
||||
velocity = 1e-4_mps;
|
||||
}
|
||||
|
||||
Eigen::Vector<double, 5> r{poseRef.X().value(), poseRef.Y().value(),
|
||||
poseRef.Rotation().Radians().value(),
|
||||
leftVelocityRef.value(), rightVelocityRef.value()};
|
||||
Eigen::Vector<double, 5> x{currentPose.X().value(), currentPose.Y().value(),
|
||||
currentPose.Rotation().Radians().value(),
|
||||
leftVelocity.value(), rightVelocity.value()};
|
||||
|
||||
m_error = r - x;
|
||||
m_error(2) = frc::AngleModulus(units::radian_t{m_error(2)}).value();
|
||||
|
||||
Eigen::Matrix<double, 5, 5> A{
|
||||
{0.0, 0.0, 0.0, 0.5, 0.5},
|
||||
{0.0, 0.0, velocity.value(), 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, -1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()},
|
||||
{0.0, 0.0, 0.0, m_A(0, 0), m_A(0, 1)},
|
||||
{0.0, 0.0, 0.0, m_A(1, 0), m_A(1, 1)}};
|
||||
Eigen::Matrix<double, 5, 2> B{{0.0, 0.0},
|
||||
{0.0, 0.0},
|
||||
{0.0, 0.0},
|
||||
{m_B(0, 0), m_B(0, 1)},
|
||||
{m_B(1, 0), m_B(1, 1)}};
|
||||
|
||||
Eigen::Matrix<double, 5, 5> discA;
|
||||
Eigen::Matrix<double, 5, 2> discB;
|
||||
DiscretizeAB(A, B, m_dt, &discA, &discB);
|
||||
|
||||
auto S = DARE<5, 2>(discA, discB, m_Q, m_R, false).value();
|
||||
|
||||
// K = (BᵀSB + R)⁻¹BᵀSA
|
||||
Eigen::Matrix<double, 2, 5> K = (discB.transpose() * S * discB + m_R)
|
||||
.llt()
|
||||
.solve(discB.transpose() * S * discA);
|
||||
|
||||
Eigen::Matrix<double, 5, 5> inRobotFrame{
|
||||
{std::cos(x(2)), std::sin(x(2)), 0.0, 0.0, 0.0},
|
||||
{-std::sin(x(2)), std::cos(x(2)), 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 1.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, 1.0, 0.0},
|
||||
{0.0, 0.0, 0.0, 0.0, 1.0}};
|
||||
|
||||
Eigen::Vector2d u = K * inRobotFrame * m_error;
|
||||
|
||||
return DifferentialDriveWheelVoltages{units::volt_t{u(0)},
|
||||
units::volt_t{u(1)}};
|
||||
}
|
||||
|
||||
DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate(
|
||||
const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
|
||||
units::meters_per_second_t rightVelocity,
|
||||
const Trajectory::State& desiredState) {
|
||||
// v = (v_r + v_l) / 2 (1)
|
||||
// w = (v_r - v_l) / (2r) (2)
|
||||
// k = w / v (3)
|
||||
//
|
||||
// v_l = v - wr
|
||||
// v_l = v - (vk)r
|
||||
// v_l = v(1 - kr)
|
||||
//
|
||||
// v_r = v + wr
|
||||
// v_r = v + (vk)r
|
||||
// v_r = v(1 + kr)
|
||||
return Calculate(
|
||||
currentPose, leftVelocity, rightVelocity, desiredState.pose,
|
||||
desiredState.velocity *
|
||||
(1 - (desiredState.curvature / 1_rad * m_trackwidth / 2.0)),
|
||||
desiredState.velocity *
|
||||
(1 + (desiredState.curvature / 1_rad * m_trackwidth / 2.0)));
|
||||
}
|
||||
|
||||
@@ -4,52 +4,16 @@
|
||||
|
||||
#include "frc/controller/LTVUnicycleController.h"
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
#include <Eigen/Cholesky>
|
||||
|
||||
#include "frc/DARE.h"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/system/Discretization.h"
|
||||
#include "units/math.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
namespace {
|
||||
|
||||
/**
|
||||
* States of the drivetrain system.
|
||||
*/
|
||||
class State {
|
||||
public:
|
||||
/// X position in global coordinate frame.
|
||||
[[maybe_unused]]
|
||||
static constexpr int kX = 0;
|
||||
|
||||
/// Y position in global coordinate frame.
|
||||
static constexpr int kY = 1;
|
||||
|
||||
/// Heading in global coordinate frame.
|
||||
static constexpr int kHeading = 2;
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
LTVUnicycleController::LTVUnicycleController(
|
||||
units::second_t dt, units::meters_per_second_t maxVelocity)
|
||||
: LTVUnicycleController{{0.0625, 0.125, 2.0}, {1.0, 2.0}, dt, maxVelocity} {
|
||||
}
|
||||
|
||||
LTVUnicycleController::LTVUnicycleController(
|
||||
const wpi::array<double, 3>& Qelems, const wpi::array<double, 2>& Relems,
|
||||
units::second_t dt, units::meters_per_second_t maxVelocity) {
|
||||
if (maxVelocity <= 0_mps) {
|
||||
throw std::domain_error("Max velocity must be greater than 0 m/s.");
|
||||
}
|
||||
if (maxVelocity >= 15_mps) {
|
||||
throw std::domain_error("Max velocity must be less than 15 m/s.");
|
||||
}
|
||||
|
||||
ChassisSpeeds LTVUnicycleController::Calculate(
|
||||
const Pose2d& currentPose, const Pose2d& poseRef,
|
||||
units::meters_per_second_t linearVelocityRef,
|
||||
units::radians_per_second_t angularVelocityRef) {
|
||||
// The change in global pose for a unicycle is defined by the following three
|
||||
// equations.
|
||||
//
|
||||
@@ -82,76 +46,39 @@ LTVUnicycleController::LTVUnicycleController(
|
||||
// [0 0 0] [1 0]
|
||||
// A = [0 0 v] B = [0 0]
|
||||
// [0 0 0] [0 1]
|
||||
Matrixd<3, 3> A = Matrixd<3, 3>::Zero();
|
||||
Matrixd<3, 2> B{{1.0, 0.0}, {0.0, 0.0}, {0.0, 1.0}};
|
||||
Matrixd<3, 3> Q = frc::MakeCostMatrix(Qelems);
|
||||
Matrixd<2, 2> R = frc::MakeCostMatrix(Relems);
|
||||
|
||||
auto R_llt = R.llt();
|
||||
|
||||
for (auto velocity = -maxVelocity; velocity < maxVelocity;
|
||||
velocity += 0.01_mps) {
|
||||
// The DARE is ill-conditioned if the velocity is close to zero, so don't
|
||||
// let the system stop.
|
||||
if (units::math::abs(velocity) < 1e-4_mps) {
|
||||
A(State::kY, State::kHeading) = 1e-4;
|
||||
} else {
|
||||
A(State::kY, State::kHeading) = velocity.value();
|
||||
}
|
||||
|
||||
Matrixd<3, 3> discA;
|
||||
Matrixd<3, 2> discB;
|
||||
DiscretizeAB(A, B, dt, &discA, &discB);
|
||||
|
||||
auto S = detail::DARE<3, 2>(discA, discB, Q, R_llt);
|
||||
|
||||
// K = (BᵀSB + R)⁻¹BᵀSA
|
||||
m_table.insert(velocity, (discB.transpose() * S * discB + R)
|
||||
.llt()
|
||||
.solve(discB.transpose() * S * discA));
|
||||
}
|
||||
}
|
||||
|
||||
bool LTVUnicycleController::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();
|
||||
}
|
||||
|
||||
void LTVUnicycleController::SetTolerance(const Pose2d& poseTolerance) {
|
||||
m_poseTolerance = poseTolerance;
|
||||
}
|
||||
|
||||
ChassisSpeeds LTVUnicycleController::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};
|
||||
}
|
||||
|
||||
// The DARE is ill-conditioned if the velocity is close to zero, so don't
|
||||
// let the system stop.
|
||||
if (units::math::abs(linearVelocityRef) < 1e-4_mps) {
|
||||
linearVelocityRef = 1e-4_mps;
|
||||
}
|
||||
|
||||
m_poseError = poseRef.RelativeTo(currentPose);
|
||||
|
||||
const auto& K = m_table[linearVelocityRef];
|
||||
Vectord<3> e{m_poseError.X().value(), m_poseError.Y().value(),
|
||||
m_poseError.Rotation().Radians().value()};
|
||||
Vectord<2> u = K * e;
|
||||
Eigen::Matrix<double, 3, 3> A{
|
||||
{0.0, 0.0, 0.0}, {0.0, 0.0, linearVelocityRef.value()}, {0.0, 0.0, 0.0}};
|
||||
constexpr Eigen::Matrix<double, 3, 2> B{{1.0, 0.0}, {0.0, 0.0}, {0.0, 1.0}};
|
||||
|
||||
Eigen::Matrix<double, 3, 3> discA;
|
||||
Eigen::Matrix<double, 3, 2> discB;
|
||||
DiscretizeAB(A, B, m_dt, &discA, &discB);
|
||||
|
||||
auto S = DARE<3, 2>(discA, discB, m_Q, m_R, false).value();
|
||||
|
||||
// K = (BᵀSB + R)⁻¹BᵀSA
|
||||
Eigen::Matrix<double, 2, 3> K = (discB.transpose() * S * discB + m_R)
|
||||
.llt()
|
||||
.solve(discB.transpose() * S * discA);
|
||||
|
||||
Eigen::Vector3d e{m_poseError.X().value(), m_poseError.Y().value(),
|
||||
m_poseError.Rotation().Radians().value()};
|
||||
Eigen::Vector2d u = K * e;
|
||||
|
||||
return ChassisSpeeds{linearVelocityRef + units::meters_per_second_t{u(0)},
|
||||
0_mps,
|
||||
angularVelocityRef + units::radians_per_second_t{u(1)}};
|
||||
}
|
||||
|
||||
ChassisSpeeds LTVUnicycleController::Calculate(
|
||||
const Pose2d& currentPose, const Trajectory::State& desiredState) {
|
||||
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
|
||||
desiredState.velocity * desiredState.curvature);
|
||||
}
|
||||
|
||||
void LTVUnicycleController::SetEnabled(bool enabled) {
|
||||
m_enabled = enabled;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user