mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
4910436b10
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 ```
85 lines
3.0 KiB
C++
85 lines
3.0 KiB
C++
// 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/LTVUnicycleController.h"
|
||
|
||
#include "frc/DARE.h"
|
||
#include "frc/system/Discretization.h"
|
||
#include "units/math.h"
|
||
|
||
using namespace frc;
|
||
|
||
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.
|
||
//
|
||
// ẋ = v cosθ
|
||
// ẏ = v sinθ
|
||
// θ̇ = ω
|
||
//
|
||
// Here's the model as a vector function where x = [x y θ]ᵀ and u = [v ω]ᵀ.
|
||
//
|
||
// [v cosθ]
|
||
// f(x, u) = [v sinθ]
|
||
// [ ω ]
|
||
//
|
||
// To create an LQR, we need to linearize this.
|
||
//
|
||
// [0 0 −v sinθ] [cosθ 0]
|
||
// ∂f(x, u)/∂x = [0 0 v cosθ] ∂f(x, u)/∂u = [sinθ 0]
|
||
// [0 0 0 ] [ 0 1]
|
||
//
|
||
// We're going to make a cross-track error controller, so we'll apply a
|
||
// clockwise rotation matrix to the global tracking error to transform it into
|
||
// the robot's coordinate frame. Since the cross-track error is always
|
||
// measured from the robot's coordinate frame, the model used to compute the
|
||
// LQR should be linearized around θ = 0 at all times.
|
||
//
|
||
// [0 0 −v sin0] [cos0 0]
|
||
// A = [0 0 v cos0] B = [sin0 0]
|
||
// [0 0 0 ] [ 0 1]
|
||
//
|
||
// [0 0 0] [1 0]
|
||
// A = [0 0 v] B = [0 0]
|
||
// [0 0 0] [0 1]
|
||
|
||
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);
|
||
|
||
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)}};
|
||
}
|