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

[wpimath] LTVUnicycleController: Use LUT, provide default hyperparameters (#4213)

Browse Source
This commit is contained in:
Tyler Veness
2022-05-04 22:04:08 -07:00
committed by GitHub
parent d5456cf278
commit b1abf455c1
3 changed files with 106 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -37,12 +37,31 @@ class Input {
static constexpr int kAngularVelocity = 4;
};
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)
: m_dt{dt} {
m_Q = frc::MakeCostMatrix(Qelems);
m_R = frc::MakeCostMatrix(Relems);
units::second_t dt, units::meters_per_second_t maxVelocity) {
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);
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) {
m_table.insert(velocity, Matrixd<2, 3>::Zero());
} else {
A(State::kY, State::kHeading) = velocity.value();
m_table.insert(velocity,
frc::LinearQuadraticRegulator<3, 2>{A, B, Q, R, dt}.K());
}
}
}
bool LTVUnicycleController::AtReference() const {
@@ -69,15 +88,10 @@ ChassisSpeeds LTVUnicycleController::Calculate(
m_poseError = poseRef.RelativeTo(currentPose);
if (units::math::abs(linearVelocityRef) < 1e-4_mps) {
m_A(State::kY, State::kHeading) = 1e-4;
} else {
m_A(State::kY, State::kHeading) = linearVelocityRef.value();
}
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 =
frc::LinearQuadraticRegulator<3, 2>{m_A, m_B, m_Q, m_R, m_dt}.K() * e;
Vectord<2> u = K * e;
return ChassisSpeeds{linearVelocityRef + units::meters_per_second_t{u(0)},
0_mps,

26
wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
View File

@@ -6,6 +6,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include <wpi/interpolating_map.h>
#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
@@ -27,16 +28,30 @@ namespace frc {
*/
class WPILIB_DLLEXPORT LTVUnicycleController {
public:
/**
* Constructs a linear time-varying unicycle controller with default maximum
* desired error tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum
* desired control effort of (1 m/s, 2 rad/s).
*
* @param dt Discretization timestep.
* @param maxVelocity The maximum velocity for the controller gain lookup
* table.
*/
explicit LTVUnicycleController(
units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps);
/**
* Constructs a linear time-varying unicycle controller.
*
* @param Qelems The maximum desired error tolerance for each state.
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.
* @param maxVelocity The maximum velocity for the controller gain lookup
* table.
*/
LTVUnicycleController(const wpi::array<double, 3>& Qelems,
const wpi::array<double, 2>& Relems,
units::second_t dt);
const wpi::array<double, 2>& Relems, units::second_t dt,
units::meters_per_second_t maxVelocity = 9_mps);
/**
* Move constructor.
@@ -97,11 +112,8 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
void SetEnabled(bool enabled);
private:
Matrixd<3, 3> m_A = Matrixd<3, 3>::Zero();
Matrixd<3, 2> m_B{{1.0, 0.0}, {0.0, 0.0}, {0.0, 1.0}};
Matrixd<3, 3> m_Q;
Matrixd<2, 2> m_R;
units::second_t m_dt;
// LUT from drivetrain linear velocity to LQR gain
wpi::interpolating_map<units::meters_per_second_t, Matrixd<2, 3>> m_table;
Pose2d m_poseError;
Pose2d m_poseTolerance;