YAMS
Yet Another Mechanism System
Loading...
Searching...
No Matches
LQRConfig.hpp
Go to the documentation of this file.
1// Copyright (c) 2026 Yet Another Software Suite
2// SPDX-License-Identifier: LGPL-3.0-or-later
3
4#pragma once
5
6#include <memory>
7#include <optional>
8#include <variant>
9#include <vector>
10#include <wpi/math/controller/LinearQuadraticRegulator.hpp>
11#include <wpi/math/estimator/KalmanFilter.hpp>
12#include <wpi/math/system/DCMotor.hpp>
13#include <wpi/math/system/LinearSystem.hpp>
14#include <wpi/math/system/LinearSystemLoop.hpp>
15#include <wpi/math/system/Models.hpp>
16#include <wpi/units/time.hpp>
17#include <wpi/units/voltage.hpp>
18
19namespace yams::math {
20
28class LQRConfig {
29 public:
31 enum class LQRType {
35 };
36
38 using Loop1 = wpi::math::LinearSystemLoop<1, 1, 1>;
40 using Loop2 = wpi::math::LinearSystemLoop<2, 1, 1>;
41
49 template <int States>
50 struct LoopBundle {
51 wpi::math::LinearSystem<States, 1, 1> plant;
52 wpi::math::LinearQuadraticRegulator<States, 1> controller;
53 wpi::math::KalmanFilter<States, 1, 1> observer;
54 wpi::math::LinearSystemLoop<States, 1, 1> loop;
55
67 LoopBundle(const wpi::math::LinearSystem<States, 1, 1>& system, const wpi::util::array<double, States>& qElems,
68 const wpi::util::array<double, 1>& rElems, const wpi::util::array<double, States>& stateStdDevs,
69 const wpi::util::array<double, 1>& measStdDevs, wpi::units::volt_t maxV, wpi::units::second_t dt)
70 : plant{system},
71 controller{plant, qElems, rElems, dt},
72 observer{plant, stateStdDevs, measStdDevs, dt},
73 loop{plant, controller, observer, maxV, dt} {}
74
75 LoopBundle(const LoopBundle&) = delete;
76 LoopBundle& operator=(const LoopBundle&) = delete;
79 };
80
88
97 LQRConfig& WithFlywheelSystem(const wpi::math::DCMotor& motor, double momentOfInertia,
98 double gearing);
99
108 LQRConfig& WithArmSystem(const wpi::math::DCMotor& motor, double momentOfInertia, double gearing);
109
119 LQRConfig& WithElevatorSystem(const wpi::math::DCMotor& motor, double mass, double drumRadius,
120 double gearing);
121
130 LQRConfig& WithQElems(std::initializer_list<double> qElems);
131
141 LQRConfig& WithRElems(std::initializer_list<double> rElems);
142
149 LQRConfig& WithStateStdDevs(std::initializer_list<double> stateStdDevs);
150
157 LQRConfig& WithMeasurementStdDevs(std::initializer_list<double> measStdDevs);
158
165 LQRConfig& WithMaxVoltage(wpi::units::volt_t maxVoltage);
166
173 LQRConfig& WithPeriod(wpi::units::second_t period);
174
181
187 wpi::units::second_t GetPeriod() const;
188
194 wpi::units::volt_t GetMaxVoltage() const;
195
202 std::vector<double> GetQElems() const;
203
209 std::vector<double> GetRElems() const;
210
216 std::vector<double> GetStateStdDevs() const;
217
224 std::vector<double> GetMeasurementStdDevs() const;
225
233 LQRConfig& WithQElems(const std::vector<double>& qElems);
234
242 LQRConfig& WithRElems(const std::vector<double>& rElems);
243
251 LQRConfig& WithStateStdDevs(const std::vector<double>& stateStdDevs);
252
260 LQRConfig& WithMeasurementStdDevs(const std::vector<double>& measStdDevs);
261
267 std::variant<wpi::math::LinearSystem<1, 1, 1>, wpi::math::LinearSystem<2, 1, 1>> GetSystem()
268 const;
269
275 std::variant<std::unique_ptr<LoopBundle<1>>, std::unique_ptr<LoopBundle<2>>> GetLoop() const;
276
277 private:
278 friend class LQRController;
279
281 static std::vector<double> Checked(const std::vector<double>& values, size_t size, const char* name);
282
283 std::optional<LQRType> m_type;
284 std::optional<wpi::math::LinearSystem<1, 1, 1>> m_flywheelPlant;
285 std::optional<wpi::math::LinearSystem<2, 1, 1>> m_armElevatorPlant;
286
287 std::vector<double> m_qElems;
288 std::vector<double> m_rElems;
289 std::vector<double> m_stateStdDevs;
290 std::vector<double> m_measStdDevs;
291
292 wpi::units::volt_t m_maxVoltage{wpi::units::volt_t{12.0}};
293 wpi::units::second_t m_period{wpi::units::second_t{0.020}};
294};
295
296} // namespace yams::math
Definition LQRConfig.hpp:28
LQRConfig & WithStateStdDevs(std::initializer_list< double > stateStdDevs)
LQRConfig & WithElevatorSystem(const wpi::math::DCMotor &motor, double mass, double drumRadius, double gearing)
std::variant< std::unique_ptr< LoopBundle< 1 > >, std::unique_ptr< LoopBundle< 2 > > > GetLoop() const
LQRConfig & WithMaxVoltage(wpi::units::volt_t maxVoltage)
LQRConfig & WithQElems(std::initializer_list< double > qElems)
LQRConfig & WithArmSystem(const wpi::math::DCMotor &motor, double momentOfInertia, double gearing)
wpi::math::LinearSystemLoop< 2, 1, 1 > Loop2
Definition LQRConfig.hpp:40
LQRConfig & WithType(LQRType type)
std::vector< double > GetMeasurementStdDevs() const
wpi::units::second_t GetPeriod() const
LQRType
Definition LQRConfig.hpp:31
@ ELEVATOR
Elevator (2-state position+velocity model).
Definition LQRConfig.hpp:34
@ FLYWHEEL
Flywheel (1-state velocity model).
Definition LQRConfig.hpp:32
@ ARM
Single-jointed arm (2-state position+velocity model).
Definition LQRConfig.hpp:33
LQRConfig & WithRElems(std::initializer_list< double > rElems)
wpi::units::volt_t GetMaxVoltage() const
std::variant< wpi::math::LinearSystem< 1, 1, 1 >, wpi::math::LinearSystem< 2, 1, 1 > > GetSystem() const
LQRConfig & WithStateStdDevs(const std::vector< double > &stateStdDevs)
LQRConfig & WithMeasurementStdDevs(std::initializer_list< double > measStdDevs)
LQRConfig & WithMeasurementStdDevs(const std::vector< double > &measStdDevs)
std::vector< double > GetQElems() const
LQRConfig & WithQElems(const std::vector< double > &qElems)
std::vector< double > GetStateStdDevs() const
LQRConfig & WithPeriod(wpi::units::second_t period)
friend class LQRController
Definition LQRConfig.hpp:278
LQRType GetType() const
std::vector< double > GetRElems() const
wpi::math::LinearSystemLoop< 1, 1, 1 > Loop1
Definition LQRConfig.hpp:38
LQRConfig & WithFlywheelSystem(const wpi::math::DCMotor &motor, double momentOfInertia, double gearing)
LQRConfig & WithRElems(const std::vector< double > &rElems)
Definition GearBox.hpp:11
Definition ADRCConfig.hpp:24
LoopBundle(const LoopBundle &)=delete
LoopBundle(LoopBundle &&)=delete
wpi::math::LinearSystemLoop< States, 1, 1 > loop
Definition LQRConfig.hpp:54
wpi::math::KalmanFilter< States, 1, 1 > observer
Definition LQRConfig.hpp:53
LoopBundle(const wpi::math::LinearSystem< States, 1, 1 > &system, const wpi::util::array< double, States > &qElems, const wpi::util::array< double, 1 > &rElems, const wpi::util::array< double, States > &stateStdDevs, const wpi::util::array< double, 1 > &measStdDevs, wpi::units::volt_t maxV, wpi::units::second_t dt)
Definition LQRConfig.hpp:67
LoopBundle & operator=(LoopBundle &&)=delete
LoopBundle & operator=(const LoopBundle &)=delete
wpi::math::LinearQuadraticRegulator< States, 1 > controller
Definition LQRConfig.hpp:52
wpi::math::LinearSystem< States, 1, 1 > plant
Definition LQRConfig.hpp:51