Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add RKF45 integration (#3047)

Browse Source 
This is more stable than Runge-Kutta for systems with large elements in their A or B matrices.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2021-01-06 21:40:25 -08:00
committed by GitHub
parent 278e0f126e
commit 85a0bd43c2
25 changed files with 560 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
View File

@@ -9,7 +9,7 @@
#include <utility>
#include "frc/RobotController.h"
#include "frc/system/RungeKutta.h"
#include "frc/system/NumericalIntegration.h"
using namespace frc;
using namespace frc::sim;
@@ -56,8 +56,7 @@ void DifferentialDrivetrainSim::SetGearing(double newGearing) {
}
void DifferentialDrivetrainSim::Update(units::second_t dt) {
m_x = RungeKutta([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x,
m_u, dt);
m_x = RK4([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
m_y = m_x + frc::MakeWhiteNoiseVector<7>(m_measurementStdDevs);
}

4
wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
View File

@@ -7,7 +7,7 @@
#include <wpi/MathExtras.h>
#include "frc/StateSpaceUtil.h"
#include "frc/system/RungeKutta.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/plant/LinearSystemId.h"
using namespace frc;
@@ -77,7 +77,7 @@ void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
Eigen::Matrix<double, 2, 1> ElevatorSim::UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
auto updatedXhat = RungeKutta(
auto updatedXhat = RKF45(
[&](const Eigen::Matrix<double, 2, 1>& x,
const Eigen::Matrix<double, 1, 1>& u_)
-> Eigen::Matrix<double, 2, 1> {

4
wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
View File

@@ -9,7 +9,7 @@
#include <units/voltage.h>
#include <wpi/MathExtras.h>
#include "frc/system/RungeKutta.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/plant/LinearSystemId.h"
using namespace frc;
@@ -82,7 +82,7 @@ Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
// We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
// std::cos(theta)]]
auto updatedXhat = RungeKutta(
Eigen::Matrix<double, 2, 1> updatedXhat = RKF45(
[&](const auto& x, const auto& u) -> Eigen::Matrix<double, 2, 1> {
Eigen::Matrix<double, 2, 1> xdot = m_plant.A() * x + m_plant.B() * u;

4
wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
View File

@@ -10,7 +10,7 @@
#include "frc/controller/RamseteController.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/simulation/DifferentialDrivetrainSim.h"
#include "frc/system/RungeKutta.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/system/plant/LinearSystemId.h"
#include "frc/trajectory/TrajectoryGenerator.h"
@@ -57,7 +57,7 @@ TEST(DifferentialDriveSim, Convergence) {
sim.Update(20_ms);
// Update ground truth.
groundTruthX = frc::RungeKutta(
groundTruthX = frc::RK4(
[&sim](const auto& x, const auto& u) -> Eigen::Matrix<double, 7, 1> {
return sim.Dynamics(x, u);
},

26
wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
View File

@@ -13,6 +13,7 @@
#include "frc/controller/PIDController.h"
#include "frc/simulation/ElevatorSim.h"
#include "frc/simulation/EncoderSim.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/system/plant/LinearSystemId.h"
#include "gtest/gtest.h"
@@ -64,3 +65,28 @@ TEST(ElevatorSim, MinMax) {
EXPECT_TRUE(height < 1.05_m);
}
}
TEST(ElevatorSim, Stability) {
static constexpr double kElevatorGearing = 100.0;
static constexpr units::meter_t kElevatorDrumRadius = 0.5_in;
static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem(
m_elevatorGearbox, kCarriageMass, kElevatorDrumRadius, kElevatorGearing);
Eigen::Matrix<double, 2, 1> x0 = frc::MakeMatrix<2, 1>(0.0, 0.0);
Eigen::Matrix<double, 1, 1> u0 = frc::MakeMatrix<1, 1>(12.0);
Eigen::Matrix<double, 2, 1> x1 = frc::MakeMatrix<2, 1>(0.0, 0.0);
for (size_t i = 0; i < 50; i++) {
x1 = frc::RKF45(
[&](Eigen::Matrix<double, 2, 1> x,
Eigen::Matrix<double, 1, 1> u) -> Eigen::Matrix<double, 2, 1> {
return system.A() * x + system.B() * u;
},
x1, u0, 0.020_s);
}
EXPECT_NEAR(x1(0), system.CalculateX(x0, u0, 1_s)(0), 0.1);
}