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

[wpimath] Remove LUTs from LTV controllers (#7521)

Browse Source 
The Raspberry Pi 5 is fast enough that we no longer need it.
```
Running ./build/DAREBench
Run on (4 X 2400 MHz CPU s)
CPU Caches:
  L1 Data 64 KiB (x4)
  L1 Instruction 64 KiB (x4)
  L2 Unified 512 KiB (x4)
  L3 Unified 2048 KiB (x1)
Load Average: 0.47, 0.72, 0.45
***WARNING*** CPU scaling is enabled, the benchmark real time measurements may be noisy and will incur extra overhead.
-------------------------------------------------------------------------------
Benchmark                                     Time             CPU   Iterations
-------------------------------------------------------------------------------
DARE_WPIMath_Dynamic                       34.4 us         34.4 us        20315
DARE_WPIMath_NoPrecondChecks_Dynamic       21.7 us         21.7 us        32266
DARE_WPIMath_Static                        15.2 us         15.2 us        45878
DARE_WPIMath_NoPrecondChecks_Static        7.84 us         7.84 us        89316
DARE_SLICOT                                79.4 us         79.4 us         8789
DARE_Drake                                 34.9 us         34.9 us        20074
```
This commit is contained in:
Tyler Veness
2024-12-07 23:00:15 -08:00
committed by GitHub
parent a7349f00ef
commit 4910436b10
6 changed files with 318 additions and 536 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
View File

@@ -4,11 +4,13 @@
#pragma once
#include <cmath>
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include <wpi/interpolating_map.h>
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/controller/DifferentialDriveWheelVoltages.h"
#include "frc/geometry/Pose2d.h"
#include "frc/system/LinearSystem.h"
@@ -52,14 +54,18 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController {
* @param Qelems The maximum desired error tolerance for each state.
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.
* @throws std::domain_error if max velocity of plant with 12 V input <= 0 m/s
* or >= 15 m/s.
*/
LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant,
units::meter_t trackwidth,
const wpi::array<double, 5>& Qelems,
const wpi::array<double, 2>& Relems,
units::second_t dt);
units::second_t dt)
: m_trackwidth{trackwidth},
m_A{plant.A()},
m_B{plant.B()},
m_Q{frc::MakeCostMatrix(Qelems)},
m_R{frc::MakeCostMatrix(Relems)},
m_dt{dt} {}
/**
* Move constructor.
@@ -75,7 +81,13 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController {
/**
* Returns true if the pose error is within tolerance of the reference.
*/
bool AtReference() const;
bool AtReference() const {
return std::abs(m_error(0)) < m_tolerance(0) &&
std::abs(m_error(1)) < m_tolerance(1) &&
std::abs(m_error(2)) < m_tolerance(2) &&
std::abs(m_error(3)) < m_tolerance(3) &&
std::abs(m_error(4)) < m_tolerance(4);
}
/**
* Sets the pose error which is considered tolerable for use with
@@ -87,7 +99,12 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController {
*/
void SetTolerance(const Pose2d& poseTolerance,
units::meters_per_second_t leftVelocityTolerance,
units::meters_per_second_t rightVelocityTolerance);
units::meters_per_second_t rightVelocityTolerance) {
m_tolerance = Eigen::Vector<double, 5>{
poseTolerance.X().value(), poseTolerance.Y().value(),
poseTolerance.Rotation().Radians().value(),
leftVelocityTolerance.value(), rightVelocityTolerance.value()};
}
/**
* Returns the left and right output voltages of the LTV controller.
@@ -123,16 +140,41 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController {
DifferentialDriveWheelVoltages Calculate(
const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
units::meters_per_second_t rightVelocity,
const Trajectory::State& desiredState);
const Trajectory::State& desiredState) {
// v = (v_r + v_l) / 2 (1)
// w = (v_r - v_l) / (2r) (2)
// k = w / v (3)
//
// v_l = v - wr
// v_l = v - (vk)r
// v_l = v(1 - kr)
//
// v_r = v + wr
// v_r = v + (vk)r
// v_r = v(1 + kr)
return Calculate(
currentPose, leftVelocity, rightVelocity, desiredState.pose,
desiredState.velocity *
(1 - (desiredState.curvature / 1_rad * m_trackwidth / 2.0)),
desiredState.velocity *
(1 + (desiredState.curvature / 1_rad * m_trackwidth / 2.0)));
}
private:
units::meter_t m_trackwidth;
// LUT from drivetrain linear velocity to LQR gain
wpi::interpolating_map<units::meters_per_second_t, Matrixd<2, 5>> m_table;
// Continuous velocity dynamics
Eigen::Matrix<double, 2, 2> m_A;
Eigen::Matrix<double, 2, 2> m_B;
Vectord<5> m_error;
Vectord<5> m_tolerance;
// LQR cost matrices
Eigen::Matrix<double, 5, 5> m_Q;
Eigen::Matrix<double, 2, 2> m_R;
units::second_t m_dt;
Eigen::Vector<double, 5> m_error;
Eigen::Vector<double, 5> m_tolerance;
};
} // namespace frc

49
wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
View File

@@ -4,15 +4,16 @@
#pragma once
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include <wpi/interpolating_map.h>
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/trajectory/Trajectory.h"
#include "units/angular_velocity.h"
#include "units/math.h"
#include "units/time.h"
#include "units/velocity.h"
@@ -37,12 +38,9 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
* desired control effort of (1 m/s, 2 rad/s).
*
* @param dt Discretization timestep.
* @param maxVelocity The maximum velocity for the controller gain lookup
* table.
* @throws std::domain_error if maxVelocity &lt;= 0.
*/
explicit LTVUnicycleController(
units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps);
explicit LTVUnicycleController(units::second_t dt)
: LTVUnicycleController{{0.0625, 0.125, 2.0}, {1.0, 2.0}, dt} {}
/**
* Constructs a linear time-varying unicycle controller.
@@ -54,13 +52,12 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
* @param Qelems The maximum desired error tolerance for each state.
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.
* @param maxVelocity The maximum velocity for the controller gain lookup
* table.
* @throws std::domain_error if maxVelocity <= 0 m/s or >= 15 m/s.
*/
LTVUnicycleController(const wpi::array<double, 3>& Qelems,
const wpi::array<double, 2>& Relems, units::second_t dt,
units::meters_per_second_t maxVelocity = 9_mps);
const wpi::array<double, 2>& Relems, units::second_t dt)
: m_Q{frc::MakeCostMatrix(Qelems)},
m_R{frc::MakeCostMatrix(Relems)},
m_dt{dt} {}
/**
* Move constructor.
@@ -75,7 +72,15 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
/**
* Returns true if the pose error is within tolerance of the reference.
*/
bool AtReference() const;
bool AtReference() const {
const auto& eTranslate = m_poseError.Translation();
const auto& eRotate = m_poseError.Rotation();
const auto& tolTranslate = m_poseTolerance.Translation();
const auto& tolRotate = m_poseTolerance.Rotation();
return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
units::math::abs(eRotate.Radians()) < tolRotate.Radians();
}
/**
* Sets the pose error which is considered tolerable for use with
@@ -83,7 +88,9 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
*
* @param poseTolerance Pose error which is tolerable.
*/
void SetTolerance(const Pose2d& poseTolerance);
void SetTolerance(const Pose2d& poseTolerance) {
m_poseTolerance = poseTolerance;
}
/**
* Returns the linear and angular velocity outputs of the LTV controller.
@@ -111,18 +118,24 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
* from a trajectory.
*/
ChassisSpeeds Calculate(const Pose2d& currentPose,
const Trajectory::State& desiredState);
const Trajectory::State& desiredState) {
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
desiredState.velocity * desiredState.curvature);
}
/**
* Enables and disables the controller for troubleshooting purposes.
*
* @param enabled If the controller is enabled or not.
*/
void SetEnabled(bool enabled);
void SetEnabled(bool enabled) { m_enabled = enabled; }
private:
// LUT from drivetrain linear velocity to LQR gain
wpi::interpolating_map<units::meters_per_second_t, Matrixd<2, 3>> m_table;
// LQR cost matrices
Eigen::Matrix<double, 3, 3> m_Q;
Eigen::Matrix<double, 2, 2> m_R;
units::second_t m_dt;
Pose2d m_poseError;
Pose2d m_poseTolerance;