WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
LinearSystemSim.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <array>
8
9#include <units/current.h>
10#include <units/time.h>
11
12#include "frc/EigenCore.h"
13#include "frc/StateSpaceUtil.h"
15
16namespace frc::sim {
17/**
18 * This class helps simulate linear systems. To use this class, do the following
19 * in the simulationPeriodic() method.
20 *
21 * Call the SetInput() method with the inputs to your system (generally
22 * voltage). Call the Update() method to update the simulation. Set simulated
23 * sensor readings with the simulated positions in the GetOutput() method.
24 *
25 * @tparam States Number of states of the system.
26 * @tparam Inputs Number of inputs to the system.
27 * @tparam Outputs Number of outputs of the system.
28 */
29template <int States, int Inputs, int Outputs>
31 public:
32 /**
33 * Creates a simulated generic linear system.
34 *
35 * @param system The system to simulate.
36 * @param measurementStdDevs The standard deviations of the measurements.
37 */
40 const std::array<double, Outputs>& measurementStdDevs = {})
41 : m_plant(system), m_measurementStdDevs(measurementStdDevs) {
45 }
46
47 virtual ~LinearSystemSim() = default;
48
49 /**
50 * Updates the simulation.
51 *
52 * @param dt The time between updates.
53 */
54 void Update(units::second_t dt) {
55 // Update x. By default, this is the linear system dynamics x_k+1 = Ax_k +
56 // Bu_k
57 m_x = UpdateX(m_x, m_u, dt);
58
59 // y = Cx + Du
60 m_y = m_plant.CalculateY(m_x, m_u);
61
62 // Add noise. If the user did not pass a noise vector to the
63 // constructor, then this method will not do anything because
64 // the standard deviations default to zero.
66 }
67
68 /**
69 * Returns the current output of the plant.
70 *
71 * @return The current output of the plant.
72 */
73 const Vectord<Outputs>& GetOutput() const { return m_y; }
74
75 /**
76 * Returns an element of the current output of the plant.
77 *
78 * @param row The row to return.
79 * @return An element of the current output of the plant.
80 */
81 double GetOutput(int row) const { return m_y(row); }
82
83 /**
84 * Sets the system inputs (usually voltages).
85 *
86 * @param u The system inputs.
87 */
88 void SetInput(const Vectord<Inputs>& u) { m_u = u; }
89
90 /**
91 * Sets the system inputs.
92 *
93 * @param row The row in the input matrix to set.
94 * @param value The value to set the row to.
95 */
96 void SetInput(int row, double value) { m_u(row, 0) = value; }
97
98 /**
99 * Returns the current input of the plant.
100 *
101 * @return The current input of the plant.
102 */
103 const Vectord<Inputs>& GetInput() const { return m_u; }
104
105 /**
106 * Returns an element of the current input of the plant.
107 *
108 * @param row The row to return.
109 * @return An element of the current input of the plant.
110 */
111 double GetInput(int row) const { return m_u(row); }
112
113 /**
114 * Sets the system state.
115 *
116 * @param state The new state.
117 */
118 void SetState(const Vectord<States>& state) { m_x = state; }
119
120 protected:
121 /**
122 * Updates the state estimate of the system.
123 *
124 * @param currentXhat The current state estimate.
125 * @param u The system inputs (usually voltage).
126 * @param dt The time difference between controller updates.
127 */
128 virtual Vectord<States> UpdateX(const Vectord<States>& currentXhat,
129 const Vectord<Inputs>& u,
130 units::second_t dt) {
131 return m_plant.CalculateX(currentXhat, u, dt);
132 }
133
134 /**
135 * Clamp the input vector such that no element exceeds the given voltage. If
136 * any does, the relative magnitudes of the input will be maintained.
137 *
138 * @param maxInput The maximum magnitude of the input vector after clamping.
139 */
140 void ClampInput(double maxInput) {
142 }
143
144 /// The plant that represents the linear system.
146
147 /// State vector.
149
150 /// Input vector.
152
153 /// Output vector.
155
156 /// The standard deviations of measurements, used for adding noise to the
157 /// measurements.
158 std::array<double, Outputs> m_measurementStdDevs;
159};
160} // namespace frc::sim
A plant defined using state-space notation.
Definition LinearSystem.h:35
This class helps simulate linear systems.
Definition LinearSystemSim.h:30
double GetInput(int row) const
Returns an element of the current input of the plant.
Definition LinearSystemSim.h:111
double GetOutput(int row) const
Returns an element of the current output of the plant.
Definition LinearSystemSim.h:81
const Vectord< Outputs > & GetOutput() const
Returns the current output of the plant.
Definition LinearSystemSim.h:73
std::array< double, Outputs > m_measurementStdDevs
The standard deviations of measurements, used for adding noise to the measurements.
Definition LinearSystemSim.h:158
LinearSystemSim(const LinearSystem< States, Inputs, Outputs > &system, const std::array< double, Outputs > &measurementStdDevs={})
Creates a simulated generic linear system.
Definition LinearSystemSim.h:38
Vectord< Inputs > m_u
Input vector.
Definition LinearSystemSim.h:151
void ClampInput(double maxInput)
Clamp the input vector such that no element exceeds the given voltage.
Definition LinearSystemSim.h:140
void Update(units::second_t dt)
Updates the simulation.
Definition LinearSystemSim.h:54
void SetInput(const Vectord< Inputs > &u)
Sets the system inputs (usually voltages).
Definition LinearSystemSim.h:88
Vectord< States > m_x
State vector.
Definition LinearSystemSim.h:148
LinearSystem< States, Inputs, Outputs > m_plant
The plant that represents the linear system.
Definition LinearSystemSim.h:145
void SetState(const Vectord< States > &state)
Sets the system state.
Definition LinearSystemSim.h:118
const Vectord< Inputs > & GetInput() const
Returns the current input of the plant.
Definition LinearSystemSim.h:103
virtual Vectord< States > UpdateX(const Vectord< States > &currentXhat, const Vectord< Inputs > &u, units::second_t dt)
Updates the state estimate of the system.
Definition LinearSystemSim.h:128
Vectord< Outputs > m_y
Output vector.
Definition LinearSystemSim.h:154
virtual ~LinearSystemSim()=default
void SetInput(int row, double value)
Sets the system inputs.
Definition LinearSystemSim.h:96
Definition SimHooks.h:12
Eigen::Vector< double, Size > Vectord
Definition EigenCore.h:12
Vectord< sizeof...(Ts)> MakeWhiteNoiseVector(Ts... stdDevs)
Definition StateSpaceUtil.h:156
Vectord< Inputs > DesaturateInputVector(const Vectord< Inputs > &u, double maxMagnitude)
Renormalize all inputs if any exceeds the maximum magnitude.
Definition StateSpaceUtil.h:360