Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Remove LUTs from LTV controllers (#7521)

Browse Source 
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
2024-12-07 23:00:15 -08:00
committed by GitHub
parent a7349f00ef
commit 4910436b10
6 changed files with 318 additions and 536 deletions
Download Patch File Download Diff File Expand all files Collapse all files

129
wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
View File

@@ -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;
}