001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj.simulation;
006
007import edu.wpi.first.math.Matrix;
008import edu.wpi.first.math.Num;
009import edu.wpi.first.math.StateSpaceUtil;
010import edu.wpi.first.math.numbers.N1;
011import edu.wpi.first.math.system.LinearSystem;
012import edu.wpi.first.wpilibj.RobotController;
013import org.ejml.MatrixDimensionException;
014import org.ejml.simple.SimpleMatrix;
015
016/**
017 * This class helps simulate linear systems. To use this class, do the following in the {@link
018 * edu.wpi.first.wpilibj.IterativeRobotBase#simulationPeriodic} method.
019 *
020 * <p>Call {@link #setInput(double...)} with the inputs to the system (usually voltage).
021 *
022 * <p>Call {@link #update} to update the simulation.
023 *
024 * <p>Set simulated sensor readings with the simulated positions in {@link #getOutput()}
025 *
026 * @param <States> Number of states of the system.
027 * @param <Inputs> Number of inputs to the system.
028 * @param <Outputs> Number of outputs of the system.
029 */
030public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs extends Num> {
031  /** The plant that represents the linear system. */
032  protected final LinearSystem<States, Inputs, Outputs> m_plant;
033
034  /** State vector. */
035  protected Matrix<States, N1> m_x;
036
037  /** Input vector. */
038  protected Matrix<Inputs, N1> m_u;
039
040  /** Output vector. */
041  protected Matrix<Outputs, N1> m_y;
042
043  /** The standard deviations of measurements, used for adding noise to the measurements. */
044  protected final Matrix<Outputs, N1> m_measurementStdDevs;
045
046  /**
047   * Creates a simulated generic linear system.
048   *
049   * @param system The system to simulate.
050   */
051  public LinearSystemSim(LinearSystem<States, Inputs, Outputs> system) {
052    this(system, null);
053  }
054
055  /**
056   * Creates a simulated generic linear system with measurement noise.
057   *
058   * @param system The system being controlled.
059   * @param measurementStdDevs Standard deviations of measurements. Can be null if no noise is
060   *     desired.
061   */
062  public LinearSystemSim(
063      LinearSystem<States, Inputs, Outputs> system, Matrix<Outputs, N1> measurementStdDevs) {
064    this.m_plant = system;
065    this.m_measurementStdDevs = measurementStdDevs;
066
067    m_x = new Matrix<>(new SimpleMatrix(system.getA().getNumRows(), 1));
068    m_u = new Matrix<>(new SimpleMatrix(system.getB().getNumCols(), 1));
069    m_y = new Matrix<>(new SimpleMatrix(system.getC().getNumRows(), 1));
070  }
071
072  /**
073   * Updates the simulation.
074   *
075   * @param dtSeconds The time between updates.
076   */
077  public void update(double dtSeconds) {
078    // Update X. By default, this is the linear system dynamics X = Ax + Bu
079    m_x = updateX(m_x, m_u, dtSeconds);
080
081    // y = cx + du
082    m_y = m_plant.calculateY(m_x, m_u);
083
084    // Add measurement noise.
085    if (m_measurementStdDevs != null) {
086      m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));
087    }
088  }
089
090  /**
091   * Returns the current output of the plant.
092   *
093   * @return The current output of the plant.
094   */
095  public Matrix<Outputs, N1> getOutput() {
096    return m_y;
097  }
098
099  /**
100   * Returns an element of the current output of the plant.
101   *
102   * @param row The row to return.
103   * @return An element of the current output of the plant.
104   */
105  public double getOutput(int row) {
106    return m_y.get(row, 0);
107  }
108
109  /**
110   * Sets the system inputs (usually voltages).
111   *
112   * @param u The system inputs.
113   */
114  public void setInput(Matrix<Inputs, N1> u) {
115    this.m_u = clampInput(u);
116  }
117
118  /**
119   * Sets the system inputs.
120   *
121   * @param row The row in the input matrix to set.
122   * @param value The value to set the row to.
123   */
124  public void setInput(int row, double value) {
125    m_u.set(row, 0, value);
126    m_u = clampInput(m_u);
127  }
128
129  /**
130   * Sets the system inputs.
131   *
132   * @param u An array of doubles that represent the inputs of the system.
133   */
134  public void setInput(double... u) {
135    if (u.length != m_u.getNumRows()) {
136      throw new MatrixDimensionException(
137          "Malformed input! Got " + u.length + " elements instead of " + m_u.getNumRows());
138    }
139    m_u = new Matrix<>(new SimpleMatrix(m_u.getNumRows(), 1, true, u));
140  }
141
142  /**
143   * Sets the system state.
144   *
145   * @param state The new state.
146   */
147  public void setState(Matrix<States, N1> state) {
148    m_x = state;
149  }
150
151  /**
152   * Returns the current drawn by this simulated system. Override this method to add a custom
153   * current calculation.
154   *
155   * @return The current drawn by this simulated mechanism.
156   */
157  public double getCurrentDrawAmps() {
158    return 0.0;
159  }
160
161  /**
162   * Updates the state estimate of the system.
163   *
164   * @param currentXhat The current state estimate.
165   * @param u The system inputs (usually voltage).
166   * @param dtSeconds The time difference between controller updates.
167   * @return The new state.
168   */
169  protected Matrix<States, N1> updateX(
170      Matrix<States, N1> currentXhat, Matrix<Inputs, N1> u, double dtSeconds) {
171    return m_plant.calculateX(currentXhat, u, dtSeconds);
172  }
173
174  /**
175   * Clamp the input vector such that no element exceeds the given voltage. If any does, the
176   * relative magnitudes of the input will be maintained.
177   *
178   * @param u The input vector.
179   * @return The normalized input.
180   */
181  protected Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> u) {
182    return StateSpaceUtil.desaturateInputVector(u, RobotController.getBatteryVoltage());
183  }
184}