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

[wpimath] Merge .inc files into headers (#7209)

Browse Source 
Splitting the files didn't help readability or save compilation time and
it confused contributors. Merging them is also in line with how C++
modules will be written.
This commit is contained in:
Tyler Veness
2024-10-14 16:08:10 -07:00
committed by GitHub
parent bedfc09268
commit ee281ea448
55 changed files with 1646 additions and 2531 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
View File

@@ -4,12 +4,22 @@
#pragma once
#include <stdexcept>
#include <string>
#include <Eigen/Cholesky>
#include <unsupported/Eigen/MatrixFunctions>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "frc/DARE.h"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/fmt/Eigen.h"
#include "frc/system/Discretization.h"
#include "frc/system/LinearSystem.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
namespace frc {
@@ -50,7 +60,8 @@ class LinearQuadraticRegulator {
template <int Outputs>
LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& Qelems, const InputArray& Relems,
units::second_t dt);
units::second_t dt)
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
/**
* Constructs a controller with the given coefficients and plant.
@@ -69,7 +80,9 @@ class LinearQuadraticRegulator {
LinearQuadraticRegulator(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const StateArray& Qelems, const InputArray& Relems,
units::second_t dt);
units::second_t dt)
: LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
/**
* Constructs a controller with the given coefficients and plant.
@@ -85,7 +98,30 @@ class LinearQuadraticRegulator {
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R,
units::second_t dt);
units::second_t dt) {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
if (!IsStabilizable<States, Inputs>(discA, discB)) {
std::string msg = fmt::format(
"The system passed to the LQR is unstabilizable!\n\nA =\n{}\nB "
"=\n{}\n",
discA, discB);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
m_K = (discB.transpose() * S * discB + R)
.llt()
.solve(discB.transpose() * S * discA);
Reset();
}
/**
* Constructs a controller with the given coefficients and plant.
@@ -103,7 +139,20 @@ class LinearQuadraticRegulator {
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R,
const Matrixd<States, Inputs>& N,
units::second_t dt);
units::second_t dt) {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
m_K = (discB.transpose() * S * discB + R)
.llt()
.solve(discB.transpose() * S * discA + N.transpose());
Reset();
}
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
@@ -166,7 +215,10 @@ class LinearQuadraticRegulator {
*
* @param x The current state x.
*/
InputVector Calculate(const StateVector& x);
InputVector Calculate(const StateVector& x) {
m_u = m_K * (m_r - x);
return m_u;
}
/**
* Returns the next output of the controller.
@@ -174,7 +226,10 @@ class LinearQuadraticRegulator {
* @param x The current state x.
* @param nextR The next reference vector r.
*/
InputVector Calculate(const StateVector& x, const StateVector& nextR);
InputVector Calculate(const StateVector& x, const StateVector& nextR) {
m_r = nextR;
return Calculate(x);
}
/**
* Adjusts LQR controller gain to compensate for a pure time delay in the
@@ -194,7 +249,13 @@ class LinearQuadraticRegulator {
*/
template <int Outputs>
void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
units::second_t dt, units::second_t inputDelay);
units::second_t dt, units::second_t inputDelay) {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
}
private:
// Current reference
@@ -215,5 +276,3 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
LinearQuadraticRegulator<2, 2>;
} // namespace frc
#include "LinearQuadraticRegulator.inc"

110
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
View File

@@ -1,110 +0,0 @@
// 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.
#pragma once
#include <stdexcept>
#include <string>
#include <Eigen/Cholesky>
#include <unsupported/Eigen/MatrixFunctions>
#include "frc/DARE.h"
#include "frc/StateSpaceUtil.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/fmt/Eigen.h"
#include "frc/system/Discretization.h"
#include "wpimath/MathShared.h"
namespace frc {
template <int States, int Inputs>
template <int Outputs>
LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
const LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& Qelems, const InputArray& Relems, units::second_t dt)
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
template <int States, int Inputs>
LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
const StateArray& Qelems, const InputArray& Relems, units::second_t dt)
: LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
template <int States, int Inputs>
LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
units::second_t dt) {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
if (!IsStabilizable<States, Inputs>(discA, discB)) {
std::string msg = fmt::format(
"The system passed to the LQR is unstabilizable!\n\nA =\n{}\nB =\n{}\n",
discA, discB);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
m_K = (discB.transpose() * S * discB + R)
.llt()
.solve(discB.transpose() * S * discA);
Reset();
}
template <int States, int Inputs>
LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
const Matrixd<States, Inputs>& N, units::second_t dt) {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
m_K = (discB.transpose() * S * discB + R)
.llt()
.solve(discB.transpose() * S * discA + N.transpose());
Reset();
}
template <int States, int Inputs>
typename LinearQuadraticRegulator<States, Inputs>::InputVector
LinearQuadraticRegulator<States, Inputs>::Calculate(const StateVector& x) {
m_u = m_K * (m_r - x);
return m_u;
}
template <int States, int Inputs>
typename LinearQuadraticRegulator<States, Inputs>::InputVector
LinearQuadraticRegulator<States, Inputs>::Calculate(const StateVector& x,
const StateVector& nextR) {
m_r = nextR;
return Calculate(x);
}
template <int States, int Inputs>
template <int Outputs>
void LinearQuadraticRegulator<States, Inputs>::LatencyCompensate(
const LinearSystem<States, Inputs, Outputs>& plant, units::second_t dt,
units::second_t inputDelay) {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
}
} // namespace frc

40
wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.h
View File

@@ -4,8 +4,10 @@
#pragma once
#include <wpi/ProtoHelper.h>
#include <wpi/protobuf/Protobuf.h>
#include "controller.pb.h"
#include "frc/controller/SimpleMotorFeedforward.h"
#include "units/length.h"
@@ -14,11 +16,35 @@
template <class Distance>
struct wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>> {
static google::protobuf::Message* New(google::protobuf::Arena* arena);
static frc::SimpleMotorFeedforward<Distance> Unpack(
const google::protobuf::Message& msg);
static void Pack(google::protobuf::Message* msg,
const frc::SimpleMotorFeedforward<Distance>& value);
};
static google::protobuf::Message* New(google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufSimpleMotorFeedforward>(
arena);
}
#include "frc/controller/proto/SimpleMotorFeedforwardProto.inc"
static frc::SimpleMotorFeedforward<Distance> Unpack(
const google::protobuf::Message& msg) {
auto m =
static_cast<const wpi::proto::ProtobufSimpleMotorFeedforward*>(&msg);
return {units::volt_t{m->ks()},
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
m->kv()},
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
m->ka()},
units::second_t{m->dt()}};
}
static void Pack(google::protobuf::Message* msg,
const frc::SimpleMotorFeedforward<Distance>& value) {
auto m = static_cast<wpi::proto::ProtobufSimpleMotorFeedforward*>(msg);
m->set_ks(value.GetKs().value());
m->set_kv(
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
value.GetKv()}
.value());
m->set_ka(
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
value.GetKa()}
.value());
m->set_dt(units::second_t{value.GetDt()}.value());
}
};

45
wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.inc
View File

@@ -1,45 +0,0 @@
// 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.
#pragma once
#include <wpi/ProtoHelper.h>
#include "controller.pb.h"
#include "frc/controller/proto/SimpleMotorFeedforwardProto.h"
template <class Distance>
google::protobuf::Message*
wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufSimpleMotorFeedforward>(arena);
}
template <class Distance>
frc::SimpleMotorFeedforward<Distance>
wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufSimpleMotorFeedforward*>(&msg);
return {units::volt_t{m->ks()},
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
m->kv()},
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
m->ka()},
units::second_t{m->dt()}};
}
template <class Distance>
void wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>>::Pack(
google::protobuf::Message* msg,
const frc::SimpleMotorFeedforward<Distance>& value) {
auto m = static_cast<wpi::proto::ProtobufSimpleMotorFeedforward*>(msg);
m->set_ks(value.GetKs().value());
m->set_kv(units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
value.GetKv()}
.value());
m->set_ka(units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
value.GetKa()}
.value());
m->set_dt(units::second_t{value.GetDt()}.value());
}

35
wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h
View File

@@ -24,14 +24,41 @@ struct wpi::Struct<frc::SimpleMotorFeedforward<Distance>> {
}
static frc::SimpleMotorFeedforward<Distance> Unpack(
std::span<const uint8_t> data);
std::span<const uint8_t> data) {
constexpr size_t kKsOff = 0;
constexpr size_t kKvOff = kKsOff + 8;
constexpr size_t kKaOff = kKvOff + 8;
constexpr size_t kDtOff = kKaOff + 8;
return {units::volt_t{wpi::UnpackStruct<double, kKsOff>(data)},
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
wpi::UnpackStruct<double, kKvOff>(data)},
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
wpi::UnpackStruct<double, kKaOff>(data)},
units::second_t{wpi::UnpackStruct<double, kDtOff>(data)}};
}
static void Pack(std::span<uint8_t> data,
const frc::SimpleMotorFeedforward<Distance>& value);
const frc::SimpleMotorFeedforward<Distance>& value) {
constexpr size_t kKsOff = 0;
constexpr size_t kKvOff = kKsOff + 8;
constexpr size_t kKaOff = kKvOff + 8;
constexpr size_t kDtOff = kKaOff + 8;
wpi::PackStruct<kKsOff>(data, value.GetKs().value());
wpi::PackStruct<kKvOff>(
data,
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
value.GetKv()}
.value());
wpi::PackStruct<kKaOff>(
data,
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
value.GetKa()}
.value());
wpi::PackStruct<kDtOff>(data, units::second_t{value.GetDt()}.value());
}
};
static_assert(
wpi::StructSerializable<frc::SimpleMotorFeedforward<units::meters>>);
static_assert(
wpi::StructSerializable<frc::SimpleMotorFeedforward<units::radians>>);
#include "frc/controller/struct/SimpleMotorFeedforwardStruct.inc"

45
wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.inc
View File

@@ -1,45 +0,0 @@
// 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.
#pragma once
#include "frc/controller/struct/SimpleMotorFeedforwardStruct.h"
template <class Distance>
frc::SimpleMotorFeedforward<Distance>
wpi::Struct<frc::SimpleMotorFeedforward<Distance>>::Unpack(
std::span<const uint8_t> data) {
constexpr size_t kKsOff = 0;
constexpr size_t kKvOff = kKsOff + 8;
constexpr size_t kKaOff = kKvOff + 8;
constexpr size_t kDtOff = kKaOff + 8;
return {units::volt_t{wpi::UnpackStruct<double, kKsOff>(data)},
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
wpi::UnpackStruct<double, kKvOff>(data)},
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
wpi::UnpackStruct<double, kKaOff>(data)},
units::second_t{wpi::UnpackStruct<double, kDtOff>(data)}};
}
template <class Distance>
void wpi::Struct<frc::SimpleMotorFeedforward<Distance>>::Pack(
std::span<uint8_t> data,
const frc::SimpleMotorFeedforward<Distance>& value) {
constexpr size_t kKsOff = 0;
constexpr size_t kKvOff = kKsOff + 8;
constexpr size_t kKaOff = kKvOff + 8;
constexpr size_t kDtOff = kKaOff + 8;
wpi::PackStruct<kKsOff>(data, value.GetKs().value());
wpi::PackStruct<kKvOff>(
data,
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
value.GetKv()}
.value());
wpi::PackStruct<kKaOff>(
data,
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
value.GetKa()}
.value());
wpi::PackStruct<kDtOff>(data, units::second_t{value.GetDt()}.value());
}