System Identification

System Identification is the process of determining a mathematical model for the behavior of a system through statistical analysis of its inputs and outputs. A mathematical model is a simplified representation of the real system. It's important to model only those features of the system that are required to control it. The features that are important depend upon the application.

There are two ways to create a mathematical model of a system. If you know the system well enough then you can create the model using its underlying physics. This is referred to as first principals. This is often done in the case where a physical system is not yet available for testing. If you do have a physical system to test on and you don't know the system very well then you can use data to create the model. This is where System Identification techniques are required. What you do is send control inputs into the system and take a look how the it reacts. You'll be able to determine a relationship between the input and the output. You can then use that data to create a mathematical model of the observed relationship.

For more informataion view the System Identification videos by Brian Douglas.

System Identification Tool

The WPILib System Identification Tool (SysID) aids in identifying the characterics of our robot systems. The tool consists of an application that runs on the user’s PC and matching robot code that runs on the user’s robot. The PC application will send control signals to the robot over NetworkTables, while the robot sends data back to the application. On the Romi the robot code will run on the Simulator. The Simulator communicates with the SysID Tool over the NetworkTables. The application then processes the data and determines model parameters for the user’s robot mechanism, as well as producing diagnostic plots.

SysID Tool

Running the System Identification Tool

The System Identification Tool (SysId) can be opened from the Start Tool option in VS Code.

On MacOS you may need to start it from the terminal. Type python3 ~/wpilib/2023/tools/SysID.py in the terminal. If the SysID Tool is not there then you can run python3 ToolsUpdater.py from the ~/wpilib/2023/tools/ directory.

The SysID Tool display should look like this:

Configure SysID

Configuring the Project

The first step is to Configuring the Project for your specific mechanism. You'll need to know some details about your system, such as the motors, encoders, and gyro. You can get most of this information from the electrical team.

Creating the Identification Routine

See Creating an Identification Routine

There are four tests that you run to gather the data. The first two gradually accelerate the robot up to its maximum speed in both the forward and backward directions. This is designed to access voltage verses speed. The second two tests abuptly accelerate the robot up to full speed in the forward and backward directions.

Analysing the Data

The analysis will tell us how much Feedforward voltage is required to move the robot. It'll also tell us the maximum speed of the robot and suggest some starting PID values that we can try for our Feedback loops. See Analysing Data in the FRC documentation.

Lab - System Identification

In this lab we're going to run System Identification on the Romi. We'll analyze the data and use it to create a cascaded PID loop to drive the robot in a straight line.

In this lab you'll learn about the following Java programming concepts:

Java example of returning an object from a method and extracting its attributes.

There are four tasks for this lab:

Calibrate the gyro.
Edit the project code to create a System Identification Routine.
Log telemetry data on wheel speeds using the WPILib DifferentialDriveWheelSpeeds class.
Load the data into the SysId tool.
Analyze the data and update the project code with the feedforward and feedback parameter variables.
Use Cascade Control and the data that you get from System Identification to drive the Romi in a straight line.

Calibrate the Gyro

Ensure that the gyro has been calibrated using the web UI. Gyros usually have some sort of zero-offset error that causes them to drift even when the robot is standing still. With the Romi this is quite significant. Calibrating the gyro will prevent most of the drifting.

Once the calibration has been done this task is complete!

Create a System Identification Routine

To run the system identification tests we need to add some code to our project. See Creating a System Identification Routine in the FRC documentation. Add the followng code to the constructor of the Drivetrain.

new SysIdRoutine(
      // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
      new SysIdRoutine.Config(),
      new SysIdRoutine.Mechanism(
          // Tell SysId how to plumb the driving voltage to the motors.
          (Measure<Voltage> volts) -> {
            this.leftMotor.setVoltage(volts.in(Volts));
            this.rightMotor.setVoltage(volts.in(Volts));
          },
          // Tell SysId how to record a frame of data for each motor on the mechanism being
          // characterized.
          log -> {
            // Record a frame for the left motors.  Since these share an encoder, we consider
            // the entire group to be one motor.
            log.motor("drive-left")
                .voltage(this.appliedVoltage.mut_replace(
                        this.leftMotor.get() * RobotController.getBatteryVoltage(), Volts))
                .linearPosition(this.distance.mut_replace(this.leftEncoder.getDistance(), Meters))
                .linearVelocity(this.velocity.mut_replace(this.leftEncoder.getRate(), MetersPerSecond));
            // Record a frame for the right motors.  Since these share an encoder, we consider
            // the entire group to be one motor.
            log.motor("drive-right")
                .voltage(this.appliedVoltage.mut_replace(
                        this.rightMotor.get() * RobotController.getBatteryVoltage(), Volts))
                .linearPosition(this.distance.mut_replace(this.rightEncoder.getDistance(), Meters))
                .linearVelocity(this.velocity.mut_replace(this.rightEncoder.getRate(), MetersPerSecond));
          },
          // Tell SysId to make generated commands require this subsystem, suffix test state in
          // WPILog with this subsystem's name ("drive")
          this));

In the Drivetrain setup two routines to run the tests. The sysIdQuasistatic() returns a command to run a quasistatic test in the specified direction. In this test the robot will start out slowly and gradually pick up speed. The sysIdDynamic() returns a command to run a dynamic test in the specified direction. This test will immediatelly try to accelerate the robot to full speed.

public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
    return sysIdRoutine.quasistatic(direction);
}

public Command sysIdDynamic(SysIdRoutine.Direction direction) {
    return sysIdRoutine.dynamic(direction);
}

Finally, we need a way to run the test so add the following options the the SendableChooser in RobotContainer.

// Set up SysId routines
this.chooser.addOption(
    "Drive SysId (Quasistatic Forward)",
    this.drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
this.chooser.addOption(
    "Drive SysId (Quasistatic Reverse)",
    this.drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
this.chooser.addOption(
    "Drive SysId (Dynamic Forward)", this.drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
this.chooser.addOption(
    "Drive SysId (Dynamic Reverse)", this.drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));

Run the SysId Tests

Before running the tests place the Romi on the floor and make sure that you have at least 10 feet of space. Start with the Quasistatic forward test by selecting it from the chooser. Put the Simulator in Autonomous mode. Click Disabled before the Romi runs out of space. Run the other three tests in a similar manner.

After running the tests the results should appear in your project folder under logs. Start the SysId and load the log files.

Picture here...

Load the Data

AdvantageKit synchronizes all log updates to the robot loop cycle to enable replay. However, only changes to each field are recorded directly to the log file; by saving the timestamps of each loop cycle, the full set of timestamps where a field was originally recorded can be recreated during replay. This design was chosen because it significantly reduces file size, but it is not compatible with WPILib's SysId analyzer (where explicit updates are expected for every sample, regardless of whether the value changed).

To convert the AdvantageKit log file to a SysId compatible format, follow the instructions below:

Open the AdvantageKit log file in AdvantageScope. In the menu bar, go to "File" > "Export Data...".
Set the format to "WPILOG" and the timestamps to "AdvantageKit Cycles". For large log files, enter the prefixes for only the fields and tables necessary for SysId analysis (see the export options documentation for details).
Click the save icon and choose a location to save the log.
Open the SysId analyzer by searching for "WPILib: Start Tool" in the VSCode command palette and choosing "SysId" (or using the desktop launcher on Windows). Open the exported log file by clicking "Open data log file..."
Choose the fields to analyze as normal.

Analyze the Data

Let's examine the data starting with the Feedforward analysis. The Feedforward analysis gives you the Ks, Kv, and Ka voltage values required to drive the Romi forward. You'll use these values in your project (see the next task for this lab). These values will be called ksVolts, kvVoltSecondsPerMeter, and kaVoltSecondsSquaredPerMeter respectively in our project. These values are explained in the Feedforward Control module of the training guide.

The analysis gives you the readout for each wheel. This will be useful in getting the Romi to go straight, since the wheels are commonly not going to be exactly the same leading to the Romi curving either left or right.

Prior to viewing the data you may need to change the "Velocity Threashold" value. Changing this value excludes data that is below a certain velocity. See Improperly set Motion Threashold in the FRC documetation.

Romi Feedforward Analysis

Now we'll look at the Feedback analysis, which will give us the P and D gain values for the system. The I gain cannot be assessed with System Identification. The Gain Preset represents how the feedback gains are calculated for your specific controller. Since we're using the WPILib PIDController for the Romi you can leave the Gain Preset at Default. There are P and D gain values available for both a Position and Velocity loop. These values will be put into the Constants file of your project.

Romi Feedback Analysis

Another interesting chart is the "Dynamic Velocity vs. Time" analysis. This tells us the maximum velocity of the Romi. On a smooth surface, like a desktop, you'll find the maximum velocity to be approximately 0.6 meters per second. On a carpet that has more resistance the maximum velocity could be as low as 0.4 meters per second. How do we get that number? The chart shows that there are approximately 2.7 wheel rotations per second. The Romi's wheel diameter is 0.07 meters, so to get the distance travelled per wheel rotation do the following calculation:

    Meters per wheel rotation = 0.07 * Pi = 0.22 meters

Now multiple meters per wheel rotation by the number of wheel rotations per second shown in the graph to get meters per second:

    "Meters per second" = "Velocity (rot p/s)" * "Meters per wheel rotation (0.22)"

So in this example the maximum speed of the Romi is approximately 0.59 meters per second. Create a variable in the Constants file called kMaxSpeedMetersPerSecond and set it equal to that value. We'll use that as a speed constraint in some of our projects.

Romi Dynamic Analysis

Record all of the feedback values Ks, Kv, and Ka for the Combined, Left, and Right wheel. Also record the PID values kP, kD. You'll need all of these values for the next task.

That's all for this task!

Log Wheel Speeds

It's sometimes useful to have a single data structure that includes both the left and right wheel speeds of a differential drive robot. There's a WPILib data structure called DifferentialDriveWheelSpeeds that can be used for that purpose. See Robot Kinematics for more information. Create a method called getWheelSpeeds() that returns the left and right encoder rates in a single data structure.

/**
* Returns the current wheel speeds of the robot.
*
* @return The current wheel speeds
*/
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
    return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
}

In the Telemetry lab we had already logged output values for the wheel speeds. Replace these with the DifferentialDriveWheelSpeeds object returned from getWheelSpeeds().

    DifferentialDriveWheelSpeeds wheel_speeds = getWheelSpeeds();
    SmartDashboard.putNumber("Left Wheel Speed", wheel_speeds.leftMetersPerSecond);
    SmartDashboard.putNumber("Right Wheel Speed", wheel_speeds.rightMetersPerSecond);

Start the Simulator and create a plot to view the wheel speeds. Edit the plot by clicking on its window topbar and call it "Wheel Speeds". Set the Y axis range between -0.6 to 0.6, since our maximum velocity was assessed at around 0.6 meters per second during System Identification. Click on "Add Plot" and drag the Left and Right Wheel Speed telemetry values into the window. Run one of the Autonomous commands to test the plot output.

Simulator Wheel Speeds

You're now done with this task!

References

Videos System Identification by Brian Douglas
FRC Documentation - Robot Charactization
FRC Documentation - Feedforward Control in WPILib
Code Example - Romi Charaterization SysID