Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add ImplicitModelFollower (#4056)

Browse Source
This commit is contained in:
Tyler Veness
2022-03-20 00:36:12 -07:00
committed by GitHub
parent 78108c2aba
commit 8d79dc8738
4 changed files with 493 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

109
wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java Normal file
View File

@@ -0,0 +1,109 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.plant.LinearSystemId;
import org.junit.jupiter.api.Test;
class ImplicitModelFollowerTest {
@Test
@SuppressWarnings("LocalVariableName")
void testSameModel() {
final double dt = 0.005;
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plant, dt);
var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
// Forward
var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
assertEquals(x.get(0, 0), xImf.get(0, 0));
assertEquals(x.get(1, 0), xImf.get(1, 0));
}
// Backward
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
assertEquals(x.get(0, 0), xImf.get(0, 0));
assertEquals(x.get(1, 0), xImf.get(1, 0));
}
// Rotate CCW
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
assertEquals(x.get(0, 0), xImf.get(0, 0));
assertEquals(x.get(1, 0), xImf.get(1, 0));
}
}
@Test
@SuppressWarnings("LocalVariableName")
void testSlowerRefModel() {
final double dt = 0.005;
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
// Linear acceleration is slower, but angular acceleration is the same
var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0);
var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plantRef, dt);
var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
// Forward
var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
assertTrue(x.get(0, 0) >= xImf.get(0, 0));
assertTrue(x.get(1, 0) >= xImf.get(1, 0));
}
// Backward
x.fill(0.0);
xImf.fill(0.0);
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
assertTrue(x.get(0, 0) <= xImf.get(0, 0));
assertTrue(x.get(1, 0) <= xImf.get(1, 0));
}
// Rotate CCW
x.fill(0.0);
xImf.fill(0.0);
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
assertEquals(x.get(0, 0), xImf.get(0, 0), 1e-5);
assertEquals(x.get(1, 0), xImf.get(1, 0), 1e-5);
}
}
}