Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +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());
}

132
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -5,10 +5,17 @@
#pragma once
#include <functional>
#include <utility>
#include <Eigen/Cholesky>
#include <wpi/array.h>
#include "frc/DARE.h"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
#include "units/time.h"
namespace frc {
@@ -68,7 +75,38 @@ class ExtendedKalmanFilter {
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt);
units::second_t dt)
: m_f(std::move(f)), m_h(std::move(h)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_residualFuncY = [](const OutputVector& a,
const OutputVector& b) -> OutputVector {
return a - b;
};
m_addFuncX = [](const StateVector& a, const StateVector& b) -> StateVector {
return a + b;
};
m_dt = dt;
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, InputVector::Zero());
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
}
/**
* Constructs an extended Kalman filter.
@@ -96,7 +134,34 @@ class ExtendedKalmanFilter {
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX,
units::second_t dt);
units::second_t dt)
: m_f(std::move(f)),
m_h(std::move(h)),
m_residualFuncY(std::move(residualFuncY)),
m_addFuncX(std::move(addFuncX)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, InputVector::Zero());
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
}
/**
* Returns the error covariance matrix P.
@@ -159,7 +224,23 @@ class ExtendedKalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const InputVector& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt) {
// Find continuous A
StateMatrix contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
// Find discrete A and Q
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
m_xHat = RK4(m_f, m_xHat, u, dt);
// Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
m_P = discA * m_P * discA.transpose() + discQ;
m_dt = dt;
}
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -202,7 +283,16 @@ class ExtendedKalmanFilter {
void Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R);
const Matrixd<Rows, Rows>& R) {
auto residualFuncY = [](const Vectord<Rows>& a,
const Vectord<Rows>& b) -> Vectord<Rows> {
return a - b;
};
auto addFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector { return a + b; };
Correct<Rows>(u, y, std::move(h), R, std::move(residualFuncY),
std::move(addFuncX));
}
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -228,7 +318,37 @@ class ExtendedKalmanFilter {
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX);
addFuncX) {
const Matrixd<Rows, States> C =
NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Matrixd<Rows, Rows> S = C * m_P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Rows> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));
// Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
// Use Joseph form for numerical stability
m_P = (StateMatrix::Identity() - K * C) * m_P *
(StateMatrix::Identity() - K * C).transpose() +
K * discR * K.transpose();
}
private:
std::function<StateVector(const StateVector&, const InputVector&)> m_f;
@@ -246,5 +366,3 @@ class ExtendedKalmanFilter {
};
} // namespace frc
#include "ExtendedKalmanFilter.inc"

171
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
View File

@@ -1,171 +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 <functional>
#include <utility>
#include <Eigen/Cholesky>
#include "frc/DARE.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/ExtendedKalmanFilter.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
namespace frc {
template <int States, int Inputs, int Outputs>
ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt)
: m_f(std::move(f)), m_h(std::move(h)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_residualFuncY = [](const OutputVector& a,
const OutputVector& b) -> OutputVector { return a - b; };
m_addFuncX = [](const StateVector& a, const StateVector& b) -> StateVector {
return a + b;
};
m_dt = dt;
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, InputVector::Zero());
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
}
template <int States, int Inputs, int Outputs>
ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
std::function<OutputVector(const OutputVector&, const OutputVector&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
units::second_t dt)
: m_f(std::move(f)),
m_h(std::move(h)),
m_residualFuncY(std::move(residualFuncY)),
m_addFuncX(std::move(addFuncX)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, InputVector::Zero());
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
}
template <int States, int Inputs, int Outputs>
void ExtendedKalmanFilter<States, Inputs, Outputs>::Predict(
const InputVector& u, units::second_t dt) {
// Find continuous A
StateMatrix contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
// Find discrete A and Q
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
m_xHat = RK4(m_f, m_xHat, u, dt);
// Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
m_P = discA * m_P * discA.transpose() + discQ;
m_dt = dt;
}
template <int States, int Inputs, int Outputs>
template <int Rows>
void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R) {
auto residualFuncY = [](const Vectord<Rows>& a,
const Vectord<Rows>& b) -> Vectord<Rows> {
return a - b;
};
auto addFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector { return a + b; };
Correct<Rows>(u, y, std::move(h), R, std::move(residualFuncY),
std::move(addFuncX));
}
template <int States, int Inputs, int Outputs>
template <int Rows>
void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R,
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX) {
const Matrixd<Rows, States> C =
NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Matrixd<Rows, Rows> S = C * m_P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Rows> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));
// Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
// Use Joseph form for numerical stability
m_P = (StateMatrix::Identity() - K * C) * m_P *
(StateMatrix::Identity() - K * C).transpose() +
K * discR * K.transpose();
}
} // namespace frc

91
wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
View File

@@ -4,11 +4,21 @@
#pragma once
#include <cmath>
#include <stdexcept>
#include <string>
#include <Eigen/Cholesky>
#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 {
@@ -59,7 +69,37 @@ class KalmanFilter {
*/
KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& stateStdDevs,
const OutputArray& measurementStdDevs, units::second_t dt);
const OutputArray& measurementStdDevs, units::second_t dt) {
m_plant = &plant;
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
// Find discrete A and Q
Matrixd<States, States> discA;
Matrixd<States, States> discQ;
DiscretizeAQ<States>(plant.A(), m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
const auto& C = plant.C();
if (!IsDetectable<States, Outputs>(discA, C)) {
std::string msg = fmt::format(
"The system passed to the Kalman filter is undetectable!\n\n"
"A =\n{}\nC =\n{}\n",
discA, C);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
Reset();
}
/**
* Returns the error covariance matrix P.
@@ -122,7 +162,19 @@ class KalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const InputVector& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt) {
// Find discrete A and Q
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(m_plant->A(), m_contQ, dt, &discA, &discQ);
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
// Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
m_P = discA * m_P * discA.transpose() + discQ;
m_dt = dt;
}
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -144,7 +196,38 @@ class KalmanFilter {
* @param R Continuous measurement noise covariance matrix.
*/
void Correct(const InputVector& u, const OutputVector& y,
const Matrixd<Outputs, Outputs>& R);
const Matrixd<Outputs, Outputs>& R) {
const auto& C = m_plant->C();
const auto& D = m_plant->D();
const Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(R, m_dt);
Matrixd<Outputs, Outputs> S = C * m_P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Outputs> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += K * (y - (C * m_xHat + D * u));
// Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
// Use Joseph form for numerical stability
m_P = (StateMatrix::Identity() - K * C) * m_P *
(StateMatrix::Identity() - K * C).transpose() +
K * discR * K.transpose();
}
private:
LinearSystem<States, Inputs, Outputs>* m_plant;
@@ -163,5 +246,3 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
KalmanFilter<2, 1, 1>;
} // namespace frc
#include "KalmanFilter.inc"

110
wpimath/src/main/native/include/frc/estimator/KalmanFilter.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 <cmath>
#include <stdexcept>
#include <string>
#include <Eigen/Cholesky>
#include "frc/DARE.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/KalmanFilter.h"
#include "frc/fmt/Eigen.h"
#include "frc/system/Discretization.h"
#include "wpimath/MathShared.h"
namespace frc {
template <int States, int Inputs, int Outputs>
KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt) {
m_plant = &plant;
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
// Find discrete A and Q
Matrixd<States, States> discA;
Matrixd<States, States> discQ;
DiscretizeAQ<States>(plant.A(), m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
const auto& C = plant.C();
if (!IsDetectable<States, Outputs>(discA, C)) {
std::string msg = fmt::format(
"The system passed to the Kalman filter is undetectable!\n\n"
"A =\n{}\nC =\n{}\n",
discA, C);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
Reset();
}
template <int States, int Inputs, int Outputs>
void KalmanFilter<States, Inputs, Outputs>::Predict(const InputVector& u,
units::second_t dt) {
// Find discrete A and Q
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(m_plant->A(), m_contQ, dt, &discA, &discQ);
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
// Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
m_P = discA * m_P * discA.transpose() + discQ;
m_dt = dt;
}
template <int States, int Inputs, int Outputs>
void KalmanFilter<States, Inputs, Outputs>::Correct(
const InputVector& u, const OutputVector& y,
const Matrixd<Outputs, Outputs>& R) {
const auto& C = m_plant->C();
const auto& D = m_plant->D();
const Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(R, m_dt);
Matrixd<Outputs, Outputs> S = C * m_P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Outputs> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += K * (y - (C * m_xHat + D * u));
// Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
// Use Joseph form for numerical stability
m_P = (StateMatrix::Identity() - K * C) * m_P *
(StateMatrix::Identity() - K * C).transpose() +
K * discR * K.transpose();
}
} // namespace frc

206
wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
View File

@@ -6,7 +6,6 @@
#include <map>
#include <optional>
#include <utility>
#include <vector>
#include <Eigen/Core>
@@ -23,6 +22,7 @@
#include "wpimath/MathShared.h"
namespace frc {
/**
* This class wraps odometry to fuse latency-compensated
* vision measurements with encoder measurements. Robot code should not use this
@@ -59,7 +59,14 @@ class WPILIB_DLLEXPORT PoseEstimator {
PoseEstimator(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
Odometry<WheelSpeeds, WheelPositions>& odometry,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs);
const wpi::array<double, 3>& visionMeasurementStdDevs)
: m_odometry(odometry), m_poseEstimate(m_odometry.GetPose()) {
for (size_t i = 0; i < 3; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
/**
* Sets the pose estimator's trust in vision measurements. This might be used
@@ -72,7 +79,23 @@ class WPILIB_DLLEXPORT PoseEstimator {
* less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs);
const wpi::array<double, 3>& visionMeasurementStdDevs) {
wpi::array<double, 3> r{wpi::empty_array};
for (size_t i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
}
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
for (size_t row = 0; row < 3; ++row) {
if (m_q[row] == 0.0) {
m_visionK(row, row) = 0.0;
} else {
m_visionK(row, row) =
m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
}
}
}
/**
* Resets the robot's position on the field.
@@ -85,35 +108,50 @@ class WPILIB_DLLEXPORT PoseEstimator {
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions, const Pose2d& pose);
const WheelPositions& wheelPositions, const Pose2d& pose) {
// Reset state estimate and error covariance
m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();
}
/**
* Resets the robot's pose.
*
* @param pose The pose to reset to.
*/
void ResetPose(const Pose2d& pose);
void ResetPose(const Pose2d& pose) {
m_odometry.ResetPose(pose);
m_odometryPoseBuffer.Clear();
}
/**
* Resets the robot's translation.
*
* @param translation The pose to translation to.
*/
void ResetTranslation(const Translation2d& translation);
void ResetTranslation(const Translation2d& translation) {
m_odometry.ResetTranslation(translation);
m_odometryPoseBuffer.Clear();
}
/**
* Resets the robot's rotation.
*
* @param rotation The rotation to reset to.
*/
void ResetRotation(const Rotation2d& rotation);
void ResetRotation(const Rotation2d& rotation) {
m_odometry.ResetRotation(rotation);
m_odometryPoseBuffer.Clear();
}
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
Pose2d GetEstimatedPosition() const;
Pose2d GetEstimatedPosition() const { return m_poseEstimate; }
/**
* Return the pose at a given timestamp, if the buffer is not empty.
@@ -122,7 +160,47 @@ class WPILIB_DLLEXPORT PoseEstimator {
* @return The pose at the given timestamp (or std::nullopt if the buffer is
* empty).
*/
std::optional<Pose2d> SampleAt(units::second_t timestamp) const;
std::optional<Pose2d> SampleAt(units::second_t timestamp) const {
// Step 0: If there are no odometry updates to sample, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
return std::nullopt;
}
// Step 1: Make sure timestamp matches the sample from the odometry pose
// buffer. (When sampling, the buffer will always use a timestamp
// between the first and last timestamps)
units::second_t oldestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().front().first;
units::second_t newestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().back().first;
timestamp =
std::clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
// Step 2: If there are no applicable vision updates, use the odometry-only
// information.
if (m_visionUpdates.empty() || timestamp < m_visionUpdates.begin()->first) {
return m_odometryPoseBuffer.Sample(timestamp);
}
// Step 3: Get the latest vision update from before or at the timestamp to
// sample at.
// First, find the iterator past the sample timestamp, then go back one.
// Note that upper_bound() won't return begin() because we check begin()
// earlier.
auto floorIter = m_visionUpdates.upper_bound(timestamp);
--floorIter;
auto visionUpdate = floorIter->second;
// Step 4: Get the pose measured by odometry at the time of the sample.
auto odometryEstimate = m_odometryPoseBuffer.Sample(timestamp);
// Step 5: Apply the vision compensation to the odometry pose.
// TODO Replace with std::optional::transform() in C++23
if (odometryEstimate) {
return visionUpdate.Compensate(*odometryEstimate);
}
return std::nullopt;
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct
@@ -145,7 +223,63 @@ class WPILIB_DLLEXPORT PoseEstimator {
* frc::Timer::GetFPGATimestamp() as your time source in this case.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty() ||
m_odometryPoseBuffer.GetInternalBuffer().front().first -
kBufferDuration >
timestamp) {
return;
}
// Step 1: Clean up any old entries
CleanUpVisionUpdates();
// Step 2: Get the pose measured by odometry at the moment the vision
// measurement was made.
auto odometrySample = m_odometryPoseBuffer.Sample(timestamp);
if (!odometrySample) {
return;
}
// Step 3: Get the vision-compensated pose estimate at the moment the vision
// measurement was made.
auto visionSample = SampleAt(timestamp);
if (!visionSample) {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision
// pose.
auto twist = visionSample.value().Log(visionRobotPose);
// Step 5: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
// measurements compared to our current pose.
Eigen::Vector3d k_times_twist =
m_visionK * Eigen::Vector3d{twist.dx.value(), twist.dy.value(),
twist.dtheta.value()};
// Step 6: Convert back to Twist2d.
Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
units::meter_t{k_times_twist(1)},
units::radian_t{k_times_twist(2)}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample};
m_visionUpdates[timestamp] = visionUpdate;
// Step 8: Remove later vision measurements. (Matches previous behavior)
auto firstAfter = m_visionUpdates.upper_bound(timestamp);
m_visionUpdates.erase(firstAfter, m_visionUpdates.end());
// Step 9: Update latest pose estimate. Since we cleared all updates after
// this vision update, it's guaranteed to be the latest vision update.
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct
@@ -192,7 +326,10 @@ class WPILIB_DLLEXPORT PoseEstimator {
* @return The estimated pose of the robot in meters.
*/
Pose2d Update(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions);
const WheelPositions& wheelPositions) {
return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
wheelPositions);
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This
@@ -206,13 +343,53 @@ class WPILIB_DLLEXPORT PoseEstimator {
*/
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions);
const WheelPositions& wheelPositions) {
auto odometryEstimate = m_odometry.Update(gyroAngle, wheelPositions);
m_odometryPoseBuffer.AddSample(currentTime, odometryEstimate);
if (m_visionUpdates.empty()) {
m_poseEstimate = odometryEstimate;
} else {
auto visionUpdate = m_visionUpdates.rbegin()->second;
m_poseEstimate = visionUpdate.Compensate(odometryEstimate);
}
return GetEstimatedPosition();
}
private:
/**
* Removes stale vision updates that won't affect sampling.
*/
void CleanUpVisionUpdates();
void CleanUpVisionUpdates() {
// Step 0: If there are no odometry samples, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
return;
}
// Step 1: Find the oldest timestamp that needs a vision update.
units::second_t oldestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().front().first;
// Step 2: If there are no vision updates before that timestamp, skip.
if (m_visionUpdates.empty() ||
oldestOdometryTimestamp < m_visionUpdates.begin()->first) {
return;
}
// Step 3: Find the newest vision update timestamp before or at the oldest
// timestamp.
// First, find the iterator past the oldest odometry timestamp, then go
// back one. Note that upper_bound() won't return begin() because we check
// begin() earlier.
auto newestNeededVisionUpdate =
m_visionUpdates.upper_bound(oldestOdometryTimestamp);
--newestNeededVisionUpdate;
// Step 4: Remove all entries strictly before the newest timestamp we need.
m_visionUpdates.erase(m_visionUpdates.begin(), newestNeededVisionUpdate);
}
struct VisionUpdate {
// The vision-compensated pose estimate
@@ -250,6 +427,5 @@ class WPILIB_DLLEXPORT PoseEstimator {
Pose2d m_poseEstimate;
};
} // namespace frc
#include "frc/estimator/PoseEstimator.inc"
} // namespace frc

246
wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
View File

@@ -1,246 +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/estimator/PoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Translation2d.h"
namespace frc {
template <typename WheelSpeeds, typename WheelPositions>
PoseEstimator<WheelSpeeds, WheelPositions>::PoseEstimator(
Kinematics<WheelSpeeds, WheelPositions>& kinematics,
Odometry<WheelSpeeds, WheelPositions>& odometry,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
: m_odometry(odometry), m_poseEstimate(m_odometry.GetPose()) {
for (size_t i = 0; i < 3; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs) {
wpi::array<double, 3> r{wpi::empty_array};
for (size_t i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
}
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
for (size_t row = 0; row < 3; ++row) {
if (m_q[row] == 0.0) {
m_visionK(row, row) = 0.0;
} else {
m_visionK(row, row) =
m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
}
}
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
const Pose2d& pose) {
// Reset state estimate and error covariance
m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPose(const Pose2d& pose) {
m_odometry.ResetPose(pose);
m_odometryPoseBuffer.Clear();
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetTranslation(
const Translation2d& translation) {
m_odometry.ResetTranslation(translation);
m_odometryPoseBuffer.Clear();
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetRotation(
const Rotation2d& rotation) {
m_odometry.ResetRotation(rotation);
m_odometryPoseBuffer.Clear();
}
template <typename WheelSpeeds, typename WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
const {
return m_poseEstimate;
if (m_visionUpdates.empty()) {
return m_odometry.GetPose();
}
auto visionUpdate = m_visionUpdates.rbegin()->second;
return visionUpdate.Compensate(m_odometry.GetPose());
}
template <typename WheelSpeeds, typename WheelPositions>
std::optional<Pose2d> PoseEstimator<WheelSpeeds, WheelPositions>::SampleAt(
units::second_t timestamp) const {
// Step 0: If there are no odometry updates to sample, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
return std::nullopt;
}
// Step 1: Make sure timestamp matches the sample from the odometry pose
// buffer. (When sampling, the buffer will always use a timestamp
// between the first and last timestamps)
units::second_t oldestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().front().first;
units::second_t newestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().back().first;
timestamp =
std::clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
// Step 2: If there are no applicable vision updates, use the odometry-only
// information.
if (m_visionUpdates.empty() || timestamp < m_visionUpdates.begin()->first) {
return m_odometryPoseBuffer.Sample(timestamp);
}
// Step 3: Get the latest vision update from before or at the timestamp to
// sample at.
// First, find the iterator past the sample timestamp, then go back one. Note
// that upper_bound() won't return begin() because we check begin() earlier.
auto floorIter = m_visionUpdates.upper_bound(timestamp);
--floorIter;
auto visionUpdate = floorIter->second;
// Step 4: Get the pose measured by odometry at the time of the sample.
auto odometryEstimate = m_odometryPoseBuffer.Sample(timestamp);
// Step 5: Apply the vision compensation to the odometry pose.
// TODO Replace with std::optional::transform() in C++23
if (odometryEstimate) {
return visionUpdate.Compensate(*odometryEstimate);
}
return std::nullopt;
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::CleanUpVisionUpdates() {
// Step 0: If there are no odometry samples, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
return;
}
// Step 1: Find the oldest timestamp that needs a vision update.
units::second_t oldestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().front().first;
// Step 2: If there are no vision updates before that timestamp, skip.
if (m_visionUpdates.empty() ||
oldestOdometryTimestamp < m_visionUpdates.begin()->first) {
return;
}
// Step 3: Find the newest vision update timestamp before or at the oldest
// timestamp.
// First, find the iterator past the oldest odometry timestamp, then go
// back one. Note that upper_bound() won't return begin() because we check
// begin() earlier.
auto newestNeededVisionUpdate =
m_visionUpdates.upper_bound(oldestOdometryTimestamp);
--newestNeededVisionUpdate;
// Step 4: Remove all entries strictly before the newest timestamp we need.
m_visionUpdates.erase(m_visionUpdates.begin(), newestNeededVisionUpdate);
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty() ||
m_odometryPoseBuffer.GetInternalBuffer().front().first - kBufferDuration >
timestamp) {
return;
}
// Step 1: Clean up any old entries
CleanUpVisionUpdates();
// Step 2: Get the pose measured by odometry at the moment the vision
// measurement was made.
auto odometrySample = m_odometryPoseBuffer.Sample(timestamp);
if (!odometrySample) {
return;
}
// Step 3: Get the vision-compensated pose estimate at the moment the vision
// measurement was made.
auto visionSample = SampleAt(timestamp);
if (!visionSample) {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision
// pose.
auto twist = visionSample.value().Log(visionRobotPose);
// Step 5: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
// measurements compared to our current pose.
Eigen::Vector3d k_times_twist =
m_visionK *
Eigen::Vector3d{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
// Step 6: Convert back to Twist2d.
Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
units::meter_t{k_times_twist(1)},
units::radian_t{k_times_twist(2)}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample};
m_visionUpdates[timestamp] = visionUpdate;
// Step 8: Remove later vision measurements. (Matches previous behavior)
auto firstAfter = m_visionUpdates.upper_bound(timestamp);
m_visionUpdates.erase(firstAfter, m_visionUpdates.end());
// Step 9: Update latest pose estimate. Since we cleared all updates after
// this vision update, it's guaranteed to be the latest vision update.
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
}
template <typename WheelSpeeds, typename WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::Update(
const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
wheelPositions);
}
template <typename WheelSpeeds, typename WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions) {
auto odometryEstimate = m_odometry.Update(gyroAngle, wheelPositions);
m_odometryPoseBuffer.AddSample(currentTime, odometryEstimate);
if (m_visionUpdates.empty()) {
m_poseEstimate = odometryEstimate;
} else {
auto visionUpdate = m_visionUpdates.rbegin()->second;
m_poseEstimate = visionUpdate.Compensate(odometryEstimate);
}
return GetEstimatedPosition();
}
} // namespace frc

71
wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h
View File

@@ -4,12 +4,22 @@
#pragma once
#include <cmath>
#include <stdexcept>
#include <string>
#include <Eigen/Cholesky>
#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 {
@@ -63,7 +73,52 @@ class SteadyStateKalmanFilter {
SteadyStateKalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& stateStdDevs,
const OutputArray& measurementStdDevs,
units::second_t dt);
units::second_t dt) {
m_plant = &plant;
auto contQ = MakeCovMatrix(stateStdDevs);
auto contR = MakeCovMatrix(measurementStdDevs);
Matrixd<States, States> discA;
Matrixd<States, States> discQ;
DiscretizeAQ<States>(plant.A(), contQ, dt, &discA, &discQ);
auto discR = DiscretizeR<Outputs>(contR, dt);
const auto& C = plant.C();
if (!IsDetectable<States, Outputs>(discA, C)) {
std::string msg = fmt::format(
"The system passed to the Kalman filter is undetectable!\n\n"
"A =\n{}\nC =\n{}\n",
discA, C);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
Matrixd<States, States> P =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
// S = CPCᵀ + R
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
Reset();
}
SteadyStateKalmanFilter(SteadyStateKalmanFilter&&) = default;
SteadyStateKalmanFilter& operator=(SteadyStateKalmanFilter&&) = default;
@@ -119,7 +174,9 @@ class SteadyStateKalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const InputVector& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt) {
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
}
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -127,7 +184,13 @@ class SteadyStateKalmanFilter {
* @param u Same control input used in the last predict step.
* @param y Measurement vector.
*/
void Correct(const InputVector& u, const OutputVector& y);
void Correct(const InputVector& u, const OutputVector& y) {
const auto& C = m_plant->C();
const auto& D = m_plant->D();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += m_K * (y - (C * m_xHat + D * u));
}
private:
LinearSystem<States, Inputs, Outputs>* m_plant;
@@ -149,5 +212,3 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
SteadyStateKalmanFilter<2, 1, 1>;
} // namespace frc
#include "SteadyStateKalmanFilter.inc"

89
wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.inc
View File

@@ -1,89 +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 <cmath>
#include <stdexcept>
#include <string>
#include <Eigen/Cholesky>
#include "frc/DARE.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/SteadyStateKalmanFilter.h"
#include "frc/fmt/Eigen.h"
#include "frc/system/Discretization.h"
#include "wpimath/MathShared.h"
namespace frc {
template <int States, int Inputs, int Outputs>
SteadyStateKalmanFilter<States, Inputs, Outputs>::SteadyStateKalmanFilter(
LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt) {
m_plant = &plant;
auto contQ = MakeCovMatrix(stateStdDevs);
auto contR = MakeCovMatrix(measurementStdDevs);
Matrixd<States, States> discA;
Matrixd<States, States> discQ;
DiscretizeAQ<States>(plant.A(), contQ, dt, &discA, &discQ);
auto discR = DiscretizeR<Outputs>(contR, dt);
const auto& C = plant.C();
if (!IsDetectable<States, Outputs>(discA, C)) {
std::string msg = fmt::format(
"The system passed to the Kalman filter is undetectable!\n\n"
"A =\n{}\nC =\n{}\n",
discA, C);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
Matrixd<States, States> P =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
// S = CPCᵀ + R
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
Reset();
}
template <int States, int Inputs, int Outputs>
void SteadyStateKalmanFilter<States, Inputs, Outputs>::Predict(
const InputVector& u, units::second_t dt) {
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
}
template <int States, int Inputs, int Outputs>
void SteadyStateKalmanFilter<States, Inputs, Outputs>::Correct(
const InputVector& u, const OutputVector& y) {
const auto& C = m_plant->C();
const auto& D = m_plant->D();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += m_K * (y - (C * m_xHat + D * u));
}
} // namespace frc

145
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -5,12 +5,19 @@
#pragma once
#include <functional>
#include <utility>
#include <Eigen/Cholesky>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/MerweScaledSigmaPoints.h"
#include "frc/estimator/UnscentedTransform.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
#include "units/time.h"
namespace frc {
@@ -74,7 +81,31 @@ class UnscentedKalmanFilter {
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt);
units::second_t dt)
: m_f(std::move(f)), m_h(std::move(h)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_meanFuncX = [](const Matrixd<States, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm) -> StateVector {
return sigmas * Wm;
};
m_meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wc) -> OutputVector {
return sigmas * Wc;
};
m_residualFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector { return a - b; };
m_residualFuncY = [](const OutputVector& a,
const OutputVector& b) -> OutputVector {
return a - b;
};
m_addFuncX = [](const StateVector& a, const StateVector& b) -> StateVector {
return a + b;
};
m_dt = dt;
Reset();
}
/**
* Constructs an unscented Kalman filter with custom mean, residual, and
@@ -120,7 +151,20 @@ class UnscentedKalmanFilter {
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX,
units::second_t dt);
units::second_t dt)
: m_f(std::move(f)),
m_h(std::move(h)),
m_meanFuncX(std::move(meanFuncX)),
m_meanFuncY(std::move(meanFuncY)),
m_residualFuncX(std::move(residualFuncX)),
m_residualFuncY(std::move(residualFuncY)),
m_addFuncX(std::move(addFuncX)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
Reset();
}
/**
* Returns the square-root error covariance matrix S.
@@ -197,7 +241,31 @@ class UnscentedKalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const InputVector& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt) {
m_dt = dt;
// Discretize Q before projecting mean and covariance forward
StateMatrix contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, m_dt, &discA, &discQ);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
StateVector x = sigmas.template block<States, 1>(0, i);
m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
}
auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
discQ.template triangularView<Eigen::Lower>());
m_xHat = xHat;
m_S = S;
}
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -242,7 +310,25 @@ class UnscentedKalmanFilter {
void Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R);
const Matrixd<Rows, Rows>& R) {
auto meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wc) -> Vectord<Rows> {
return sigmas * Wc;
};
auto residualFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector {
return a - b;
};
auto residualFuncY = [](const Vectord<Rows>& a,
const Vectord<Rows>& b) -> Vectord<Rows> {
return a - b;
};
auto addFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector { return a + b; };
Correct<Rows>(u, y, std::move(h), R, std::move(meanFuncY),
std::move(residualFuncY), std::move(residualFuncX),
std::move(addFuncX));
}
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -277,7 +363,54 @@ class UnscentedKalmanFilter {
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX);
addFuncX) {
Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);
// Transform sigma points into measurement space
Matrixd<Rows, 2 * States + 1> sigmasH;
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
sigmasH.template block<Rows, 1>(0, i) =
h(sigmas.template block<States, 1>(0, i), u);
}
// Mean and covariance of prediction passed through UT
auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
discR.template triangularView<Eigen::Lower>());
// Compute cross covariance of the state and the measurements
Matrixd<States, Rows> Pxy;
Pxy.setZero();
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
// Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
Pxy +=
m_pts.Wc(i) *
(residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
(residualFuncY(sigmasH.template block<Rows, 1>(0, i), yHat))
.transpose();
}
// K = (P_{xy} / S_yᵀ) / S_y
// K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
// K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
Matrixd<States, Rows> K =
Sy.transpose()
.fullPivHouseholderQr()
.solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
.transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
Matrixd<States, Rows> U = K * Sy;
for (int i = 0; i < Rows; i++) {
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
m_S, U.template block<States, 1>(0, i), -1);
}
}
private:
std::function<StateVector(const StateVector&, const InputVector&)> m_f;
@@ -309,5 +442,3 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<5, 3, 3>;
} // namespace frc
#include "UnscentedKalmanFilter.inc"

196
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
View File

@@ -1,196 +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 <functional>
#include <utility>
#include <Eigen/Cholesky>
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/estimator/UnscentedTransform.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
namespace frc {
template <int States, int Inputs, int Outputs>
UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt)
: m_f(std::move(f)), m_h(std::move(h)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_meanFuncX = [](const Matrixd<States, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm) -> StateVector {
return sigmas * Wm;
};
m_meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wc) -> OutputVector {
return sigmas * Wc;
};
m_residualFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector { return a - b; };
m_residualFuncY = [](const OutputVector& a,
const OutputVector& b) -> OutputVector { return a - b; };
m_addFuncX = [](const StateVector& a, const StateVector& b) -> StateVector {
return a + b;
};
m_dt = dt;
Reset();
}
template <int States, int Inputs, int Outputs>
UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncX,
std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<OutputVector(const OutputVector&, const OutputVector&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
units::second_t dt)
: m_f(std::move(f)),
m_h(std::move(h)),
m_meanFuncX(std::move(meanFuncX)),
m_meanFuncY(std::move(meanFuncY)),
m_residualFuncX(std::move(residualFuncX)),
m_residualFuncY(std::move(residualFuncY)),
m_addFuncX(std::move(addFuncX)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
Reset();
}
template <int States, int Inputs, int Outputs>
void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
const InputVector& u, units::second_t dt) {
m_dt = dt;
// Discretize Q before projecting mean and covariance forward
StateMatrix contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, m_dt, &discA, &discQ);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
StateVector x = sigmas.template block<States, 1>(0, i);
m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
}
auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
discQ.template triangularView<Eigen::Lower>());
m_xHat = xHat;
m_S = S;
}
template <int States, int Inputs, int Outputs>
template <int Rows>
void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R) {
auto meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wc) -> Vectord<Rows> {
return sigmas * Wc;
};
auto residualFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector {
return a - b;
};
auto residualFuncY = [](const Vectord<Rows>& a,
const Vectord<Rows>& b) -> Vectord<Rows> {
return a - b;
};
auto addFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector { return a + b; };
Correct<Rows>(u, y, std::move(h), R, std::move(meanFuncY),
std::move(residualFuncY), std::move(residualFuncX),
std::move(addFuncX));
}
template <int States, int Inputs, int Outputs>
template <int Rows>
void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R,
std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncY,
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX) {
Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);
// Transform sigma points into measurement space
Matrixd<Rows, 2 * States + 1> sigmasH;
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
sigmasH.template block<Rows, 1>(0, i) =
h(sigmas.template block<States, 1>(0, i), u);
}
// Mean and covariance of prediction passed through UT
auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
discR.template triangularView<Eigen::Lower>());
// Compute cross covariance of the state and the measurements
Matrixd<States, Rows> Pxy;
Pxy.setZero();
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
// Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
Pxy += m_pts.Wc(i) *
(residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
(residualFuncY(sigmasH.template block<Rows, 1>(0, i), yHat))
.transpose();
}
// K = (P_{xy} / S_yᵀ) / S_y
// K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
// K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
Matrixd<States, Rows> K =
Sy.transpose()
.fullPivHouseholderQr()
.solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
.transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
Matrixd<States, Rows> U = K * Sy;
for (int i = 0; i < Rows; i++) {
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
m_S, U.template block<States, 1>(0, i), -1);
}
}
} // namespace frc

31
wpimath/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -6,6 +6,7 @@
#include <initializer_list>
#include <span>
#include <utility>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
@@ -14,6 +15,7 @@
#include "frc/geometry/Transform2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Twist2d.h"
#include "units/length.h"
namespace frc {
@@ -33,7 +35,9 @@ class WPILIB_DLLEXPORT Pose2d {
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
constexpr Pose2d(Translation2d translation, Rotation2d rotation);
constexpr Pose2d(Translation2d translation, Rotation2d rotation)
: m_translation{std::move(translation)},
m_rotation{std::move(rotation)} {}
/**
* Constructs a pose with x and y translations instead of a separate
@@ -43,7 +47,8 @@ class WPILIB_DLLEXPORT Pose2d {
* @param y The y component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
/**
* Transforms the pose by the given transformation and returns the new
@@ -59,7 +64,9 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The transformed pose.
*/
constexpr Pose2d operator+(const Transform2d& other) const;
constexpr Pose2d operator+(const Transform2d& other) const {
return TransformBy(other);
}
/**
* Returns the Transform2d that maps the one pose to another.
@@ -109,7 +116,9 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The new scaled Pose2d.
*/
constexpr Pose2d operator*(double scalar) const;
constexpr Pose2d operator*(double scalar) const {
return Pose2d{m_translation * scalar, m_rotation * scalar};
}
/**
* Divides the current pose by a scalar.
@@ -118,7 +127,9 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The new scaled Pose2d.
*/
constexpr Pose2d operator/(double scalar) const;
constexpr Pose2d operator/(double scalar) const {
return *this * (1.0 / scalar);
}
/**
* Rotates the pose around the origin and returns the new pose.
@@ -127,7 +138,9 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The rotated pose.
*/
constexpr Pose2d RotateBy(const Rotation2d& other) const;
constexpr Pose2d RotateBy(const Rotation2d& other) const {
return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
}
/**
* Transforms the pose by the given transformation and returns the new pose.
@@ -137,7 +150,10 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The transformed pose.
*/
constexpr Pose2d TransformBy(const Transform2d& other) const;
constexpr Pose2d TransformBy(const Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
other.Rotation() + m_rotation};
}
/**
* Returns the current pose relative to the given pose.
@@ -217,4 +233,3 @@ void from_json(const wpi::json& json, Pose2d& pose);
#include "frc/geometry/proto/Pose2dProto.h"
#endif
#include "frc/geometry/struct/Pose2dStruct.h"
#include "frc/geometry/Pose2d.inc"

43
wpimath/src/main/native/include/frc/geometry/Pose2d.inc
View File

@@ -1,43 +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 <utility>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "units/length.h"
namespace frc {
constexpr Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
: m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
constexpr Pose2d::Pose2d(units::meter_t x, units::meter_t y,
Rotation2d rotation)
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
constexpr Pose2d Pose2d::operator+(const Transform2d& other) const {
return TransformBy(other);
}
constexpr Pose2d Pose2d::operator*(double scalar) const {
return Pose2d{m_translation * scalar, m_rotation * scalar};
}
constexpr Pose2d Pose2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
constexpr Pose2d Pose2d::RotateBy(const Rotation2d& other) const {
return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
}
constexpr Pose2d Pose2d::TransformBy(const Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
other.Rotation() + m_rotation};
}
} // namespace frc

55
wpimath/src/main/native/include/frc/geometry/Rotation2d.h
View File

@@ -4,10 +4,15 @@
#pragma once
#include <type_traits>
#include <gcem.hpp>
#include <wpi/StackTrace.h>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#include "units/angle.h"
#include "wpimath/MathShared.h"
namespace frc {
@@ -32,7 +37,10 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @param value The value of the angle.
*/
constexpr Rotation2d(units::angle_unit auto value); // NOLINT
constexpr Rotation2d(units::angle_unit auto value) // NOLINT
: m_value{value},
m_cos{gcem::cos(value.template convert<units::radian>().value())},
m_sin{gcem::sin(value.template convert<units::radian>().value())} {}
/**
* Constructs a Rotation2d with the given x and y (cosine and sine)
@@ -41,7 +49,22 @@ class WPILIB_DLLEXPORT Rotation2d {
* @param x The x component or cosine of the rotation.
* @param y The y component or sine of the rotation.
*/
constexpr Rotation2d(double x, double y);
constexpr Rotation2d(double x, double y) {
double magnitude = gcem::hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
if (!std::is_constant_evaluated()) {
wpi::math::MathSharedStore::ReportError(
"x and y components of Rotation2d are zero\n{}",
wpi::GetStackTrace(1));
}
}
m_value = units::radian_t{gcem::atan2(m_sin, m_cos)};
}
/**
* Adds two rotations together, with the result being bounded between -pi and
@@ -54,7 +77,9 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The sum of the two rotations.
*/
constexpr Rotation2d operator+(const Rotation2d& other) const;
constexpr Rotation2d operator+(const Rotation2d& other) const {
return RotateBy(other);
}
/**
* Subtracts the new rotation from the current rotation and returns the new
@@ -67,7 +92,9 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The difference between the two rotations.
*/
constexpr Rotation2d operator-(const Rotation2d& other) const;
constexpr Rotation2d operator-(const Rotation2d& other) const {
return *this + -other;
}
/**
* Takes the inverse of the current rotation. This is simply the negative of
@@ -75,7 +102,7 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The inverse of the current rotation.
*/
constexpr Rotation2d operator-() const;
constexpr Rotation2d operator-() const { return Rotation2d{-m_value}; }
/**
* Multiplies the current rotation by a scalar.
@@ -84,7 +111,9 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The new scaled Rotation2d.
*/
constexpr Rotation2d operator*(double scalar) const;
constexpr Rotation2d operator*(double scalar) const {
return Rotation2d{m_value * scalar};
}
/**
* Divides the current rotation by a scalar.
@@ -93,7 +122,9 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The new scaled Rotation2d.
*/
constexpr Rotation2d operator/(double scalar) const;
constexpr Rotation2d operator/(double scalar) const {
return *this * (1.0 / scalar);
}
/**
* Checks equality between this Rotation2d and another object.
@@ -101,7 +132,9 @@ class WPILIB_DLLEXPORT Rotation2d {
* @param other The other object.
* @return Whether the two objects are equal.
*/
constexpr bool operator==(const Rotation2d& other) const;
constexpr bool operator==(const Rotation2d& other) const {
return gcem::hypot(Cos() - other.Cos(), Sin() - other.Sin()) < 1E-9;
}
/**
* Adds the new rotation to the current rotation using a rotation matrix.
@@ -116,7 +149,10 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The new rotated Rotation2d.
*/
constexpr Rotation2d RotateBy(const Rotation2d& other) const;
constexpr Rotation2d RotateBy(const Rotation2d& other) const {
return {Cos() * other.Cos() - Sin() * other.Sin(),
Cos() * other.Sin() + Sin() * other.Cos()};
}
/**
* Returns the radian value of the rotation.
@@ -173,4 +209,3 @@ void from_json(const wpi::json& json, Rotation2d& rotation);
#include "frc/geometry/proto/Rotation2dProto.h"
#endif
#include "frc/geometry/struct/Rotation2dStruct.h"
#include "frc/geometry/Rotation2d.inc"

70
wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
View File

@@ -1,70 +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 <string>
#include <type_traits>
#include <gcem.hpp>
#include <wpi/StackTrace.h>
#include "frc/geometry/Rotation2d.h"
#include "units/angle.h"
#include "wpimath/MathShared.h"
namespace frc {
constexpr Rotation2d::Rotation2d(units::angle_unit auto value)
: m_value{value},
m_cos{gcem::cos(value.template convert<units::radian>().value())},
m_sin{gcem::sin(value.template convert<units::radian>().value())} {}
constexpr Rotation2d::Rotation2d(double x, double y) {
double magnitude = gcem::hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
if (!std::is_constant_evaluated()) {
wpi::math::MathSharedStore::ReportError(
"x and y components of Rotation2d are zero\n{}",
wpi::GetStackTrace(1));
}
}
m_value = units::radian_t{gcem::atan2(m_sin, m_cos)};
}
constexpr Rotation2d Rotation2d::operator-() const {
return Rotation2d{-m_value};
}
constexpr Rotation2d Rotation2d::operator*(double scalar) const {
return Rotation2d{m_value * scalar};
}
constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
return RotateBy(other);
}
constexpr Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
return *this + -other;
}
constexpr Rotation2d Rotation2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
constexpr bool Rotation2d::operator==(const Rotation2d& other) const {
return gcem::hypot(Cos() - other.Cos(), Sin() - other.Sin()) < 1E-9;
}
constexpr Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
return {Cos() * other.Cos() - Sin() * other.Sin(),
Cos() * other.Sin() + Sin() * other.Cos()};
}
} // namespace frc

19
wpimath/src/main/native/include/frc/geometry/Transform2d.h
View File

@@ -4,8 +4,11 @@
#pragma once
#include <utility>
#include <wpi/SymbolExports.h>
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
namespace frc {
@@ -31,7 +34,9 @@ class WPILIB_DLLEXPORT Transform2d {
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
constexpr Transform2d(Translation2d translation, Rotation2d rotation);
constexpr Transform2d(Translation2d translation, Rotation2d rotation)
: m_translation{std::move(translation)},
m_rotation{std::move(rotation)} {}
/**
* Constructs a transform with x and y translations instead of a separate
@@ -41,8 +46,8 @@ class WPILIB_DLLEXPORT Transform2d {
* @param y The y component of the translational component of the transform.
* @param rotation The rotational component of the transform.
*/
constexpr Transform2d(units::meter_t x, units::meter_t y,
Rotation2d rotation);
constexpr Transform2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
/**
* Constructs the identity transform -- maps an initial pose to itself.
@@ -82,7 +87,12 @@ class WPILIB_DLLEXPORT Transform2d {
*
* @return The inverted transformation.
*/
constexpr Transform2d Inverse() const;
constexpr Transform2d Inverse() const {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
/**
* Multiplies the transform by the scalar.
@@ -128,4 +138,3 @@ class WPILIB_DLLEXPORT Transform2d {
#include "frc/geometry/proto/Transform2dProto.h"
#endif
#include "frc/geometry/struct/Transform2dStruct.h"
#include "frc/geometry/Transform2d.inc"

30
wpimath/src/main/native/include/frc/geometry/Transform2d.inc
View File

@@ -1,30 +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 <utility>
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Transform2d.h"
#include "frc/geometry/Translation2d.h"
namespace frc {
constexpr Transform2d::Transform2d(Translation2d translation,
Rotation2d rotation)
: m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
constexpr Transform2d::Transform2d(units::meter_t x, units::meter_t y,
Rotation2d rotation)
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
constexpr Transform2d Transform2d::Inverse() const {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
} // namespace frc

50
wpimath/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -39,7 +39,8 @@ class WPILIB_DLLEXPORT Translation2d {
* @param x The x component of the translation.
* @param y The y component of the translation.
*/
constexpr Translation2d(units::meter_t x, units::meter_t y);
constexpr Translation2d(units::meter_t x, units::meter_t y)
: m_x{x}, m_y{y} {}
/**
* Constructs a Translation2d with the provided distance and angle. This is
@@ -48,7 +49,8 @@ class WPILIB_DLLEXPORT Translation2d {
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
constexpr Translation2d(units::meter_t distance, const Rotation2d& angle);
constexpr Translation2d(units::meter_t distance, const Rotation2d& angle)
: m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {}
/**
* Constructs a Translation2d from the provided translation vector's X and Y
@@ -90,7 +92,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return A Vector representation of this translation.
*/
constexpr Eigen::Vector2d ToVector() const;
constexpr Eigen::Vector2d ToVector() const {
return Eigen::Vector2d{{m_x.value(), m_y.value()}};
}
/**
* Returns the norm, or distance from the origin to the translation.
@@ -104,7 +108,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The angle of the translation
*/
constexpr Rotation2d Angle() const;
constexpr Rotation2d Angle() const {
return Rotation2d{m_x.value(), m_y.value()};
}
/**
* Applies a rotation to the translation in 2D space.
@@ -124,7 +130,10 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The new rotated translation.
*/
constexpr Translation2d RotateBy(const Rotation2d& other) const;
constexpr Translation2d RotateBy(const Rotation2d& other) const {
return {m_x * other.Cos() - m_y * other.Sin(),
m_x * other.Sin() + m_y * other.Cos()};
}
/**
* Rotates this translation around another translation in 2D space.
@@ -139,7 +148,12 @@ class WPILIB_DLLEXPORT Translation2d {
* @return The new rotated translation.
*/
constexpr Translation2d RotateAround(const Translation2d& other,
const Rotation2d& rot) const;
const Rotation2d& rot) const {
return {(m_x - other.X()) * rot.Cos() - (m_y - other.Y()) * rot.Sin() +
other.X(),
(m_x - other.X()) * rot.Sin() + (m_y - other.Y()) * rot.Cos() +
other.Y()};
}
/**
* Returns the sum of two translations in 2D space.
@@ -151,7 +165,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The sum of the translations.
*/
constexpr Translation2d operator+(const Translation2d& other) const;
constexpr Translation2d operator+(const Translation2d& other) const {
return {X() + other.X(), Y() + other.Y()};
}
/**
* Returns the difference between two translations.
@@ -163,7 +179,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The difference between the two translations.
*/
constexpr Translation2d operator-(const Translation2d& other) const;
constexpr Translation2d operator-(const Translation2d& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current translation. This is equivalent to
@@ -172,7 +190,7 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The inverse of the current translation.
*/
constexpr Translation2d operator-() const;
constexpr Translation2d operator-() const { return {-m_x, -m_y}; }
/**
* Returns the translation multiplied by a scalar.
@@ -183,7 +201,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The scaled translation.
*/
constexpr Translation2d operator*(double scalar) const;
constexpr Translation2d operator*(double scalar) const {
return {scalar * m_x, scalar * m_y};
}
/**
* Returns the translation divided by a scalar.
@@ -194,7 +214,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The scaled translation.
*/
constexpr Translation2d operator/(double scalar) const;
constexpr Translation2d operator/(double scalar) const {
return operator*(1.0 / scalar);
}
/**
* Checks equality between this Translation2d and another object.
@@ -202,7 +224,10 @@ class WPILIB_DLLEXPORT Translation2d {
* @param other The other object.
* @return Whether the two objects are equal.
*/
constexpr bool operator==(const Translation2d& other) const;
constexpr bool operator==(const Translation2d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m;
}
/**
* Returns the nearest Translation2d from a collection of translations
@@ -236,4 +261,3 @@ void from_json(const wpi::json& json, Translation2d& state);
#include "frc/geometry/proto/Translation2dProto.h"
#endif
#include "frc/geometry/struct/Translation2dStruct.h"
#include "frc/geometry/Translation2d.inc"

68
wpimath/src/main/native/include/frc/geometry/Translation2d.inc
View File

@@ -1,68 +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/geometry/Translation2d.h"
#include "units/length.h"
#include "units/math.h"
namespace frc {
constexpr Translation2d::Translation2d(units::meter_t x, units::meter_t y)
: m_x{x}, m_y{y} {}
constexpr Translation2d::Translation2d(units::meter_t distance,
const Rotation2d& angle)
: m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {}
constexpr Eigen::Vector2d Translation2d::ToVector() const {
return Eigen::Vector2d{{m_x.value(), m_y.value()}};
}
constexpr Rotation2d Translation2d::Angle() const {
return Rotation2d{m_x.value(), m_y.value()};
}
constexpr Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
return {m_x * other.Cos() - m_y * other.Sin(),
m_x * other.Sin() + m_y * other.Cos()};
}
constexpr Translation2d Translation2d::RotateAround(
const Translation2d& other, const Rotation2d& rot) const {
return {
(m_x - other.X()) * rot.Cos() - (m_y - other.Y()) * rot.Sin() + other.X(),
(m_x - other.X()) * rot.Sin() + (m_y - other.Y()) * rot.Cos() +
other.Y()};
}
constexpr Translation2d Translation2d::operator+(
const Translation2d& other) const {
return {X() + other.X(), Y() + other.Y()};
}
constexpr Translation2d Translation2d::operator-(
const Translation2d& other) const {
return *this + -other;
}
constexpr Translation2d Translation2d::operator-() const {
return {-m_x, -m_y};
}
constexpr Translation2d Translation2d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y};
}
constexpr Translation2d Translation2d::operator/(double scalar) const {
return operator*(1.0 / scalar);
}
constexpr bool Translation2d::operator==(const Translation2d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m;
}
} // namespace frc

66
wpimath/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -11,6 +11,7 @@
#include "frc/geometry/Rotation3d.h"
#include "frc/geometry/Translation2d.h"
#include "units/length.h"
#include "units/math.h"
namespace frc {
@@ -37,7 +38,8 @@ class WPILIB_DLLEXPORT Translation3d {
* @param y The y component of the translation.
* @param z The z component of the translation.
*/
constexpr Translation3d(units::meter_t x, units::meter_t y, units::meter_t z);
constexpr Translation3d(units::meter_t x, units::meter_t y, units::meter_t z)
: m_x{x}, m_y{y}, m_z{z} {}
/**
* Constructs a Translation3d with the provided distance and angle. This is
@@ -46,7 +48,12 @@ class WPILIB_DLLEXPORT Translation3d {
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
Translation3d(units::meter_t distance, const Rotation3d& angle);
Translation3d(units::meter_t distance, const Rotation3d& angle) {
auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
m_x = rectangular.X();
m_y = rectangular.Y();
m_z = rectangular.Z();
}
/**
* Constructs a Translation3d from the provided translation vector's X, Y, and
@@ -54,7 +61,10 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @param vector The translation vector to represent.
*/
explicit Translation3d(const Eigen::Vector3d& vector);
explicit Translation3d(const Eigen::Vector3d& vector)
: m_x{units::meter_t{vector.x()}},
m_y{units::meter_t{vector.y()}},
m_z{units::meter_t{vector.z()}} {}
/**
* Calculates the distance between two translations in 3D space.
@@ -66,7 +76,11 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The distance between the two translations.
*/
units::meter_t Distance(const Translation3d& other) const;
units::meter_t Distance(const Translation3d& other) const {
return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
units::math::pow<2>(other.m_y - m_y) +
units::math::pow<2>(other.m_z - m_z));
}
/**
* Returns the X component of the translation.
@@ -94,14 +108,18 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return A Vector representation of this translation.
*/
constexpr Eigen::Vector3d ToVector() const;
constexpr Eigen::Vector3d ToVector() const {
return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}};
}
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
units::meter_t Norm() const;
units::meter_t Norm() const {
return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
/**
* Applies a rotation to the translation in 3D space.
@@ -113,13 +131,20 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The new rotated translation.
*/
Translation3d RotateBy(const Rotation3d& other) const;
Translation3d RotateBy(const Rotation3d& other) const {
Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
units::meter_t{qprime.Z()}};
}
/**
* Returns a Translation2d representing this Translation3d projected into the
* X-Y plane.
*/
constexpr Translation2d ToTranslation2d() const;
constexpr Translation2d ToTranslation2d() const {
return Translation2d{m_x, m_y};
}
/**
* Returns the sum of two translations in 3D space.
@@ -131,7 +156,9 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The sum of the translations.
*/
constexpr Translation3d operator+(const Translation3d& other) const;
constexpr Translation3d operator+(const Translation3d& other) const {
return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
}
/**
* Returns the difference between two translations.
@@ -143,7 +170,9 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The difference between the two translations.
*/
constexpr Translation3d operator-(const Translation3d& other) const;
constexpr Translation3d operator-(const Translation3d& other) const {
return operator+(-other);
}
/**
* Returns the inverse of the current translation. This is equivalent to
@@ -151,7 +180,7 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The inverse of the current translation.
*/
constexpr Translation3d operator-() const;
constexpr Translation3d operator-() const { return {-m_x, -m_y, -m_z}; }
/**
* Returns the translation multiplied by a scalar.
@@ -163,7 +192,9 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The scaled translation.
*/
constexpr Translation3d operator*(double scalar) const;
constexpr Translation3d operator*(double scalar) const {
return {scalar * m_x, scalar * m_y, scalar * m_z};
}
/**
* Returns the translation divided by a scalar.
@@ -175,7 +206,9 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The scaled translation.
*/
constexpr Translation3d operator/(double scalar) const;
constexpr Translation3d operator/(double scalar) const {
return operator*(1.0 / scalar);
}
/**
* Checks equality between this Translation3d and another object.
@@ -183,7 +216,11 @@ class WPILIB_DLLEXPORT Translation3d {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Translation3d& other) const;
constexpr bool operator==(const Translation3d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m &&
units::math::abs(m_z - other.m_z) < 1E-9_m;
}
private:
units::meter_t m_x = 0_m;
@@ -203,4 +240,3 @@ void from_json(const wpi::json& json, Translation3d& state);
#include "frc/geometry/proto/Translation3dProto.h"
#endif
#include "frc/geometry/struct/Translation3dStruct.h"
#include "frc/geometry/Translation3d.inc"

48
wpimath/src/main/native/include/frc/geometry/Translation3d.inc
View File

@@ -1,48 +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/geometry/Translation2d.h"
#include "frc/geometry/Translation3d.h"
#include "units/length.h"
#include "units/math.h"
namespace frc {
constexpr Translation3d::Translation3d(units::meter_t x, units::meter_t y,
units::meter_t z)
: m_x{x}, m_y{y}, m_z{z} {}
constexpr Translation2d Translation3d::ToTranslation2d() const {
return Translation2d{m_x, m_y};
}
constexpr Eigen::Vector3d Translation3d::ToVector() const {
return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}};
}
constexpr Translation3d Translation3d::operator+(
const Translation3d& other) const {
return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
}
constexpr Translation3d Translation3d::operator-(
const Translation3d& other) const {
return operator+(-other);
}
constexpr Translation3d Translation3d::operator-() const {
return {-m_x, -m_y, -m_z};
}
constexpr Translation3d Translation3d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y, scalar * m_z};
}
constexpr Translation3d Translation3d::operator/(double scalar) const {
return operator*(1.0 / scalar);
}
} // namespace frc

28
wpimath/src/main/native/include/frc/kinematics/Odometry.h
View File

@@ -12,6 +12,7 @@
#include "frc/kinematics/Kinematics.h"
namespace frc {
/**
* Class for odometry. Robot code should not use this directly- Instead, use the
* particular type for your drivetrain (e.g., DifferentialDriveOdometry).
@@ -39,7 +40,13 @@ class WPILIB_DLLEXPORT Odometry {
explicit Odometry(const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions,
const Pose2d& initialPose = Pose2d{});
const Pose2d& initialPose = Pose2d{})
: m_kinematics(kinematics),
m_pose(initialPose),
m_previousWheelPositions(wheelPositions) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
/**
* Resets the robot's position on the field.
@@ -108,7 +115,21 @@ class WPILIB_DLLEXPORT Odometry {
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions);
const WheelPositions& wheelPositions) {
auto angle = gyroAngle + m_gyroOffset;
auto twist =
m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
twist.dtheta = (angle - m_previousAngle).Radians();
auto newPose = m_pose.Exp(twist);
m_previousAngle = angle;
m_previousWheelPositions = wheelPositions;
m_pose = {newPose.Translation(), angle};
return m_pose;
}
private:
const Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
@@ -118,6 +139,5 @@ class WPILIB_DLLEXPORT Odometry {
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
};
} // namespace frc
#include "Odometry.inc"
} // namespace frc

38
wpimath/src/main/native/include/frc/kinematics/Odometry.inc
View File

@@ -1,38 +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/kinematics/Odometry.h"
namespace frc {
template <typename WheelSpeeds, typename WheelPositions>
Odometry<WheelSpeeds, WheelPositions>::Odometry(
const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
const Pose2d& initialPose)
: m_kinematics(kinematics),
m_pose(initialPose),
m_previousWheelPositions(wheelPositions) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
template <typename WheelSpeeds, typename WheelPositions>
const Pose2d& Odometry<WheelSpeeds, WheelPositions>::Update(
const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
auto angle = gyroAngle + m_gyroOffset;
auto twist = m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
twist.dtheta = (angle - m_previousAngle).Radians();
auto newPose = m_pose.Exp(twist);
m_previousAngle = angle;
m_previousWheelPositions = wheelPositions;
m_pose = {newPose.Translation(), angle};
return m_pose;
}
} // namespace frc

148
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
View File

@@ -4,6 +4,7 @@
#pragma once
#include <algorithm>
#include <concepts>
#include <cstddef>
@@ -19,6 +20,7 @@
#include "frc/kinematics/Kinematics.h"
#include "frc/kinematics/SwerveModulePosition.h"
#include "frc/kinematics/SwerveModuleState.h"
#include "units/math.h"
#include "units/velocity.h"
#include "wpimath/MathShared.h"
@@ -116,7 +118,11 @@ class SwerveDriveKinematics
* @param moduleHeadings The swerve module headings. The order of the module
* headings should be same as passed into the constructor of this class.
*/
void ResetHeadings(wpi::array<Rotation2d, NumModules> moduleHeadings);
void ResetHeadings(wpi::array<Rotation2d, NumModules> moduleHeadings) {
for (size_t i = 0; i < NumModules; i++) {
m_moduleHeadings[i] = moduleHeadings[i];
}
}
/**
* Performs inverse kinematics to return the module states from a desired
@@ -151,7 +157,52 @@ class SwerveDriveKinematics
*/
wpi::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation = Translation2d{}) const;
const Translation2d& centerOfRotation = Translation2d{}) const {
wpi::array<SwerveModuleState, NumModules> moduleStates(wpi::empty_array);
if (chassisSpeeds.vx == 0_mps && chassisSpeeds.vy == 0_mps &&
chassisSpeeds.omega == 0_rad_per_s) {
for (size_t i = 0; i < NumModules; i++) {
moduleStates[i] = {0_mps, m_moduleHeadings[i]};
}
return moduleStates;
}
// We have a new center of rotation. We need to compute the matrix again.
if (centerOfRotation != m_previousCoR) {
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) =
Matrixd<2, 3>{
{1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).value()},
{0, 1, (+m_modules[i].X() - centerOfRotation.X()).value()}};
// clang-format on
}
m_previousCoR = centerOfRotation;
}
Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(),
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
Matrixd<NumModules * 2, 1> moduleStateMatrix =
m_inverseKinematics * chassisSpeedsVector;
for (size_t i = 0; i < NumModules; i++) {
units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)};
units::meters_per_second_t y{moduleStateMatrix(i * 2 + 1, 0)};
auto speed = units::math::hypot(x, y);
auto rotation = speed > 1e-6_mps ? Rotation2d{x.value(), y.value()}
: m_moduleHeadings[i];
moduleStates[i] = {speed, rotation};
m_moduleHeadings[i] = rotation;
}
return moduleStates;
}
wpi::array<SwerveModuleState, NumModules> ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const override {
@@ -191,7 +242,23 @@ class SwerveDriveKinematics
* @return The resulting chassis speed.
*/
ChassisSpeeds ToChassisSpeeds(const wpi::array<SwerveModuleState, NumModules>&
moduleStates) const override;
moduleStates) const override {
Matrixd<NumModules * 2, 1> moduleStateMatrix;
for (size_t i = 0; i < NumModules; ++i) {
SwerveModuleState module = moduleStates[i];
moduleStateMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
moduleStateMatrix(i * 2 + 1, 0) =
module.speed.value() * module.angle.Sin();
}
Eigen::Vector3d chassisSpeedsVector =
m_forwardKinematics.solve(moduleStateMatrix);
return {units::meters_per_second_t{chassisSpeedsVector(0)},
units::meters_per_second_t{chassisSpeedsVector(1)},
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
/**
* Performs forward kinematics to return the resulting Twist2d from the
@@ -227,7 +294,24 @@ class SwerveDriveKinematics
* @return The resulting Twist2d.
*/
Twist2d ToTwist2d(
wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const;
wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const {
Matrixd<NumModules * 2, 1> moduleDeltaMatrix;
for (size_t i = 0; i < NumModules; ++i) {
SwerveModulePosition module = moduleDeltas[i];
moduleDeltaMatrix(i * 2, 0) =
module.distance.value() * module.angle.Cos();
moduleDeltaMatrix(i * 2 + 1, 0) =
module.distance.value() * module.angle.Sin();
}
Eigen::Vector3d chassisDeltaVector =
m_forwardKinematics.solve(moduleDeltaMatrix);
return {units::meter_t{chassisDeltaVector(0)},
units::meter_t{chassisDeltaVector(1)},
units::radian_t{chassisDeltaVector(2)}};
}
Twist2d ToTwist2d(
const wpi::array<SwerveModulePosition, NumModules>& start,
@@ -257,7 +341,22 @@ class SwerveDriveKinematics
*/
static void DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed);
units::meters_per_second_t attainableMaxSpeed) {
auto& states = *moduleStates;
auto realMaxSpeed =
units::math::abs(std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return units::math::abs(a.speed) <
units::math::abs(b.speed);
})
->speed);
if (realMaxSpeed > attainableMaxSpeed) {
for (auto& module : states) {
module.speed = module.speed / realMaxSpeed * attainableMaxSpeed;
}
}
}
/**
* Renormalizes the wheel speeds if any individual speed is above the
@@ -285,7 +384,38 @@ class SwerveDriveKinematics
ChassisSpeeds desiredChassisSpeed,
units::meters_per_second_t attainableMaxModuleSpeed,
units::meters_per_second_t attainableMaxRobotTranslationSpeed,
units::radians_per_second_t attainableMaxRobotRotationSpeed);
units::radians_per_second_t attainableMaxRobotRotationSpeed) {
auto& states = *moduleStates;
auto realMaxSpeed =
units::math::abs(std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return units::math::abs(a.speed) <
units::math::abs(b.speed);
})
->speed);
if (attainableMaxRobotTranslationSpeed == 0_mps ||
attainableMaxRobotRotationSpeed == 0_rad_per_s ||
realMaxSpeed == 0_mps) {
return;
}
auto translationalK =
units::math::hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy) /
attainableMaxRobotTranslationSpeed;
auto rotationalK = units::math::abs(desiredChassisSpeed.omega) /
attainableMaxRobotRotationSpeed;
auto k = units::math::max(translationalK, rotationalK);
auto scale = units::math::min(k * attainableMaxModuleSpeed / realMaxSpeed,
units::scalar_t{1});
for (auto& module : states) {
module.speed = module.speed * scale;
}
}
wpi::array<SwerveModulePosition, NumModules> Interpolate(
const wpi::array<SwerveModulePosition, NumModules>& start,
@@ -312,9 +442,11 @@ class SwerveDriveKinematics
mutable Translation2d m_previousCoR;
};
template <typename ModuleTranslation, typename... ModuleTranslations>
SwerveDriveKinematics(ModuleTranslation, ModuleTranslations...)
-> SwerveDriveKinematics<1 + sizeof...(ModuleTranslations)>;
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
SwerveDriveKinematics<4>;
} // namespace frc
#include "SwerveDriveKinematics.inc"

176
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
View File

@@ -1,176 +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 <algorithm>
#include <utility>
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "units/math.h"
namespace frc {
template <typename ModuleTranslation, typename... ModuleTranslations>
SwerveDriveKinematics(ModuleTranslation, ModuleTranslations...)
-> SwerveDriveKinematics<1 + sizeof...(ModuleTranslations)>;
template <size_t NumModules>
void SwerveDriveKinematics<NumModules>::ResetHeadings(
wpi::array<Rotation2d, NumModules> moduleHeadings) {
for (size_t i = 0; i < NumModules; i++) {
m_moduleHeadings[i] = moduleHeadings[i];
}
}
template <size_t NumModules>
wpi::array<SwerveModuleState, NumModules>
SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation) const {
wpi::array<SwerveModuleState, NumModules> moduleStates(wpi::empty_array);
if (chassisSpeeds.vx == 0_mps && chassisSpeeds.vy == 0_mps &&
chassisSpeeds.omega == 0_rad_per_s) {
for (size_t i = 0; i < NumModules; i++) {
moduleStates[i] = {0_mps, m_moduleHeadings[i]};
}
return moduleStates;
}
// We have a new center of rotation. We need to compute the matrix again.
if (centerOfRotation != m_previousCoR) {
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) =
Matrixd<2, 3>{
{1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).value()},
{0, 1, (+m_modules[i].X() - centerOfRotation.X()).value()}};
// clang-format on
}
m_previousCoR = centerOfRotation;
}
Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(),
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
Matrixd<NumModules * 2, 1> moduleStateMatrix =
m_inverseKinematics * chassisSpeedsVector;
for (size_t i = 0; i < NumModules; i++) {
units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)};
units::meters_per_second_t y{moduleStateMatrix(i * 2 + 1, 0)};
auto speed = units::math::hypot(x, y);
auto rotation = speed > 1e-6_mps ? Rotation2d{x.value(), y.value()}
: m_moduleHeadings[i];
moduleStates[i] = {speed, rotation};
m_moduleHeadings[i] = rotation;
}
return moduleStates;
}
template <size_t NumModules>
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
const wpi::array<SwerveModuleState, NumModules>& moduleStates) const {
Matrixd<NumModules * 2, 1> moduleStateMatrix;
for (size_t i = 0; i < NumModules; ++i) {
SwerveModuleState module = moduleStates[i];
moduleStateMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
moduleStateMatrix(i * 2 + 1, 0) = module.speed.value() * module.angle.Sin();
}
Eigen::Vector3d chassisSpeedsVector =
m_forwardKinematics.solve(moduleStateMatrix);
return {units::meters_per_second_t{chassisSpeedsVector(0)},
units::meters_per_second_t{chassisSpeedsVector(1)},
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
template <size_t NumModules>
Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const {
Matrixd<NumModules * 2, 1> moduleDeltaMatrix;
for (size_t i = 0; i < NumModules; ++i) {
SwerveModulePosition module = moduleDeltas[i];
moduleDeltaMatrix(i * 2, 0) = module.distance.value() * module.angle.Cos();
moduleDeltaMatrix(i * 2 + 1, 0) =
module.distance.value() * module.angle.Sin();
}
Eigen::Vector3d chassisDeltaVector =
m_forwardKinematics.solve(moduleDeltaMatrix);
return {units::meter_t{chassisDeltaVector(0)},
units::meter_t{chassisDeltaVector(1)},
units::radian_t{chassisDeltaVector(2)}};
}
template <size_t NumModules>
void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed) {
auto& states = *moduleStates;
auto realMaxSpeed =
units::math::abs(std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return units::math::abs(a.speed) <
units::math::abs(b.speed);
})
->speed);
if (realMaxSpeed > attainableMaxSpeed) {
for (auto& module : states) {
module.speed = module.speed / realMaxSpeed * attainableMaxSpeed;
}
}
}
template <size_t NumModules>
void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates,
ChassisSpeeds desiredChassisSpeed,
units::meters_per_second_t attainableMaxModuleSpeed,
units::meters_per_second_t attainableMaxRobotTranslationSpeed,
units::radians_per_second_t attainableMaxRobotRotationSpeed) {
auto& states = *moduleStates;
auto realMaxSpeed =
units::math::abs(std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return units::math::abs(a.speed) <
units::math::abs(b.speed);
})
->speed);
if (attainableMaxRobotTranslationSpeed == 0_mps ||
attainableMaxRobotRotationSpeed == 0_rad_per_s || realMaxSpeed == 0_mps) {
return;
}
auto translationalK =
units::math::hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy) /
attainableMaxRobotTranslationSpeed;
auto rotationalK = units::math::abs(desiredChassisSpeed.omega) /
attainableMaxRobotRotationSpeed;
auto k = units::math::max(translationalK, rotationalK);
auto scale = units::math::min(k * attainableMaxModuleSpeed / realMaxSpeed,
units::scalar_t{1});
for (auto& module : states) {
module.speed = module.speed * scale;
}
}
} // namespace frc

14
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
View File

@@ -4,7 +4,6 @@
#pragma once
#include <chrono>
#include <cstddef>
#include <ctime>
@@ -16,7 +15,7 @@
#include "SwerveModulePosition.h"
#include "SwerveModuleState.h"
#include "frc/geometry/Pose2d.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
namespace frc {
@@ -45,7 +44,14 @@ class SwerveDriveOdometry
SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose = Pose2d{});
const Pose2d& initialPose = Pose2d{})
: Odometry<wpi::array<SwerveModuleState, NumModules>,
wpi::array<SwerveModulePosition, NumModules>>(
m_kinematicsImpl, gyroAngle, modulePositions, initialPose),
m_kinematicsImpl(kinematics) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
}
private:
SwerveDriveKinematics<NumModules> m_kinematicsImpl;
@@ -55,5 +61,3 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
SwerveDriveOdometry<4>;
} // namespace frc
#include "SwerveDriveOdometry.inc"

23
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
View File

@@ -1,23 +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/kinematics/SwerveDriveOdometry.h"
#include "wpimath/MathShared.h"
namespace frc {
template <size_t NumModules>
SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose)
: Odometry<wpi::array<SwerveModuleState, NumModules>,
wpi::array<SwerveModulePosition, NumModules>>(
m_kinematicsImpl, gyroAngle, modulePositions, initialPose),
m_kinematicsImpl(kinematics) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
}
} // namespace frc

36
wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.h
View File

@@ -4,17 +4,39 @@
#pragma once
#include <stdexcept>
#include <fmt/format.h>
#include <wpi/ProtoHelper.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "kinematics.pb.h"
template <size_t NumModules>
struct wpi::Protobuf<frc::SwerveDriveKinematics<NumModules>> {
static google::protobuf::Message* New(google::protobuf::Arena* arena);
static frc::SwerveDriveKinematics<NumModules> Unpack(
const google::protobuf::Message& msg);
static void Pack(google::protobuf::Message* msg,
const frc::SwerveDriveKinematics<NumModules>& value);
};
static google::protobuf::Message* New(google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufSwerveDriveKinematics>(arena);
}
#include "frc/kinematics/proto/SwerveDriveKinematicsProto.inc"
static frc::SwerveDriveKinematics<NumModules> Unpack(
const google::protobuf::Message& msg) {
auto m =
static_cast<const wpi::proto::ProtobufSwerveDriveKinematics*>(&msg);
if (m->modules_size() != NumModules) {
throw std::invalid_argument(fmt::format(
"Tried to unpack message with {} elements in modules into "
"SwerveDriveKinematics with {} modules",
m->modules_size(), NumModules));
}
return frc::SwerveDriveKinematics<NumModules>{
wpi::UnpackProtobufArray<wpi::proto::ProtobufTranslation2d,
frc::Translation2d, NumModules>(m->modules())};
}
static void Pack(google::protobuf::Message* msg,
const frc::SwerveDriveKinematics<NumModules>& value) {
auto m = static_cast<wpi::proto::ProtobufSwerveDriveKinematics*>(msg);
wpi::PackProtobufArray(m->mutable_modules(), value.GetModules());
}
};

44
wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.inc
View File

@@ -1,44 +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 <fmt/format.h>
#include <wpi/ProtoHelper.h>
#include "frc/kinematics/proto/SwerveDriveKinematicsProto.h"
#include "kinematics.pb.h"
template <size_t NumModules>
google::protobuf::Message*
wpi::Protobuf<frc::SwerveDriveKinematics<NumModules>>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufSwerveDriveKinematics>(arena);
}
template <size_t NumModules>
frc::SwerveDriveKinematics<NumModules>
wpi::Protobuf<frc::SwerveDriveKinematics<NumModules>>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufSwerveDriveKinematics*>(&msg);
if (m->modules_size() != NumModules) {
throw std::invalid_argument(
fmt::format("Tried to unpack message with {} elements in modules into "
"SwerveDriveKinematics with {} modules",
m->modules_size(), NumModules));
}
return frc::SwerveDriveKinematics<NumModules>{
wpi::UnpackProtobufArray<wpi::proto::ProtobufTranslation2d,
frc::Translation2d, NumModules>(m->modules())};
}
template <size_t NumModules>
void wpi::Protobuf<frc::SwerveDriveKinematics<NumModules>>::Pack(
google::protobuf::Message* msg,
const frc::SwerveDriveKinematics<NumModules>& value) {
auto m = static_cast<wpi::proto::ProtobufSwerveDriveKinematics*>(msg);
wpi::PackProtobufArray(m->mutable_modules(), value.GetModules());
}

16
wpimath/src/main/native/include/frc/kinematics/struct/SwerveDriveKinematicsStruct.h
View File

@@ -24,9 +24,19 @@ struct wpi::Struct<frc::SwerveDriveKinematics<NumModules>> {
static constexpr std::string_view GetSchema() { return kSchema; }
static frc::SwerveDriveKinematics<NumModules> Unpack(
std::span<const uint8_t> data);
std::span<const uint8_t> data) {
constexpr size_t kModulesOff = 0;
return frc::SwerveDriveKinematics<NumModules>{
wpi::UnpackStructArray<frc::Translation2d, kModulesOff, NumModules>(
data)};
}
static void Pack(std::span<uint8_t> data,
const frc::SwerveDriveKinematics<NumModules>& value);
const frc::SwerveDriveKinematics<NumModules>& value) {
constexpr size_t kModulesOff = 0;
wpi::PackStructArray<kModulesOff, NumModules>(data, value.GetModules());
}
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
@@ -37,5 +47,3 @@ static_assert(wpi::StructSerializable<frc::SwerveDriveKinematics<4>>);
static_assert(wpi::HasNestedStruct<frc::SwerveDriveKinematics<4>>);
static_assert(wpi::StructSerializable<frc::SwerveDriveKinematics<3>>);
static_assert(wpi::HasNestedStruct<frc::SwerveDriveKinematics<3>>);
#include "frc/kinematics/struct/SwerveDriveKinematicsStruct.inc"

25
wpimath/src/main/native/include/frc/kinematics/struct/SwerveDriveKinematicsStruct.inc
View File

@@ -1,25 +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/kinematics/struct/SwerveDriveKinematicsStruct.h"
template <size_t NumModules>
frc::SwerveDriveKinematics<NumModules>
wpi::Struct<frc::SwerveDriveKinematics<NumModules>>::Unpack(
std::span<const uint8_t> data) {
constexpr size_t kModulesOff = 0;
return frc::SwerveDriveKinematics<NumModules>{
wpi::UnpackStructArray<frc::Translation2d, kModulesOff, NumModules>(
data)};
}
template <size_t NumModules>
void wpi::Struct<frc::SwerveDriveKinematics<NumModules>>::Pack(
std::span<uint8_t> data,
const frc::SwerveDriveKinematics<NumModules>& value) {
constexpr size_t kModulesOff = 0;
wpi::PackStructArray<kModulesOff, NumModules>(data, value.GetModules());
}

44
wpimath/src/main/native/include/frc/proto/MatrixProto.h
View File

@@ -4,19 +4,53 @@
#pragma once
#include <stdexcept>
#include <fmt/format.h>
#include <wpi/ProtoHelper.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/EigenCore.h"
#include "wpimath.pb.h"
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
struct wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
static google::protobuf::Message* New(google::protobuf::Arena* arena);
static google::protobuf::Message* New(google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufMatrix>(arena);
}
static frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> Unpack(
const google::protobuf::Message& msg);
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufMatrix*>(&msg);
if (m->num_rows() != Rows || m->num_cols() != Cols) {
throw std::invalid_argument(fmt::format(
"Tried to unpack message with {} rows and {} columns into "
"Matrix with {} rows and {} columns",
m->num_rows(), m->num_cols(), Rows, Cols));
}
if (m->data_size() != Rows * Cols) {
throw std::invalid_argument(
fmt::format("Tried to unpack message with {} elements in data into "
"Matrix with {} elements",
m->data_size(), Rows * Cols));
}
frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> mat;
for (int i = 0; i < Rows * Cols; i++) {
mat(i) = m->data(i);
}
return mat;
}
static void Pack(
google::protobuf::Message* msg,
const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value);
const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value) {
auto m = static_cast<wpi::proto::ProtobufMatrix*>(msg);
m->set_num_rows(Rows);
m->set_num_cols(Cols);
m->clear_data();
for (int i = 0; i < Rows * Cols; i++) {
m->add_data(value(i));
}
}
};
#include "frc/proto/MatrixProto.inc"

60
wpimath/src/main/native/include/frc/proto/MatrixProto.inc
View File

@@ -1,60 +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 <fmt/format.h>
#include <wpi/ProtoHelper.h>
#include "frc/proto/MatrixProto.h"
#include "wpimath.pb.h"
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
google::protobuf::Message*
wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufMatrix>(arena);
}
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>
wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufMatrix*>(&msg);
if (m->num_rows() != Rows || m->num_cols() != Cols) {
throw std::invalid_argument(
fmt::format("Tried to unpack message with {} rows and {} columns into "
"Matrix with {} rows and {} columns",
m->num_rows(), m->num_cols(), Rows, Cols));
}
if (m->data_size() != Rows * Cols) {
throw std::invalid_argument(
fmt::format("Tried to unpack message with {} elements in data into "
"Matrix with {} elements",
m->data_size(), Rows * Cols));
}
frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> mat;
for (int i = 0; i < Rows * Cols; i++) {
mat(i) = m->data(i);
}
return mat;
}
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
void wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Pack(
google::protobuf::Message* msg,
const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value) {
auto m = static_cast<wpi::proto::ProtobufMatrix*>(msg);
m->set_num_rows(Rows);
m->set_num_cols(Cols);
m->clear_data();
for (int i = 0; i < Rows * Cols; i++) {
m->add_data(value(i));
}
}

36
wpimath/src/main/native/include/frc/proto/VectorProto.h
View File

@@ -4,18 +4,44 @@
#pragma once
#include <stdexcept>
#include <fmt/format.h>
#include <wpi/ProtoHelper.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/EigenCore.h"
#include "wpimath.pb.h"
template <int Size, int Options, int MaxRows, int MaxCols>
struct wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>> {
static google::protobuf::Message* New(google::protobuf::Arena* arena);
static google::protobuf::Message* New(google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufVector>(arena);
}
static frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> Unpack(
const google::protobuf::Message& msg);
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufVector*>(&msg);
if (m->rows_size() != Size) {
throw std::invalid_argument(
fmt::format("Tried to unpack message with {} elements in rows into "
"Vector with {} rows",
m->rows_size(), Size));
}
frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> vec;
for (int i = 0; i < Size; i++) {
vec(i) = m->rows(i);
}
return vec;
}
static void Pack(
google::protobuf::Message* msg,
const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value);
const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value) {
auto m = static_cast<wpi::proto::ProtobufVector*>(msg);
m->clear_rows();
for (int i = 0; i < Size; i++) {
m->add_rows(value(i));
}
}
};
#include "frc/proto/VectorProto.inc"

49
wpimath/src/main/native/include/frc/proto/VectorProto.inc
View File

@@ -1,49 +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 <fmt/format.h>
#include <wpi/ProtoHelper.h>
#include "frc/proto/VectorProto.h"
#include "wpimath.pb.h"
template <int Size, int Options, int MaxRows, int MaxCols>
google::protobuf::Message*
wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufVector>(arena);
}
template <int Size, int Options, int MaxRows, int MaxCols>
frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>
wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufVector*>(&msg);
if (m->rows_size() != Size) {
throw std::invalid_argument(
fmt::format("Tried to unpack message with {} elements in rows into "
"Vector with {} rows",
m->rows_size(), Size));
}
frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> vec;
for (int i = 0; i < Size; i++) {
vec(i) = m->rows(i);
}
return vec;
}
template <int Size, int Options, int MaxRows, int MaxCols>
void wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Pack(
google::protobuf::Message* msg,
const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value) {
auto m = static_cast<wpi::proto::ProtobufVector*>(msg);
m->clear_rows();
for (int i = 0; i < Size; i++) {
m->add_rows(value(i));
}
}

23
wpimath/src/main/native/include/frc/struct/MatrixStruct.h
View File

@@ -24,13 +24,28 @@ struct wpi::Struct<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
static constexpr std::string_view GetSchema() { return kSchema; }
static frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> Unpack(
std::span<const uint8_t> data);
std::span<const uint8_t> data) {
constexpr size_t kDataOff = 0;
wpi::array<double, Rows * Cols> mat_data =
wpi::UnpackStructArray<double, kDataOff, Rows * Cols>(data);
frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> mat;
for (int i = 0; i < Rows * Cols; i++) {
mat(i) = mat_data[i];
}
return mat;
}
static void Pack(
std::span<uint8_t> data,
const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value);
const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value) {
constexpr size_t kDataOff = 0;
wpi::array<double, Rows * Cols> mat_data(wpi::empty_array);
for (int i = 0; i < Rows * Cols; i++) {
mat_data[i] = value(i);
}
wpi::PackStructArray<kDataOff, Rows * Cols>(data, mat_data);
}
};
static_assert(wpi::StructSerializable<frc::Matrixd<1, 2>>);
static_assert(wpi::StructSerializable<frc::Matrixd<3, 3>>);
#include "frc/struct/MatrixStruct.inc"

35
wpimath/src/main/native/include/frc/struct/MatrixStruct.inc
View File

@@ -1,35 +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/struct/MatrixStruct.h"
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>
wpi::Struct<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Unpack(
std::span<const uint8_t> data) {
constexpr size_t kDataOff = 0;
wpi::array<double, Rows * Cols> mat_data =
wpi::UnpackStructArray<double, kDataOff, Rows * Cols>(data);
frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> mat;
for (int i = 0; i < Rows * Cols; i++) {
mat(i) = mat_data[i];
}
return mat;
}
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
void wpi::Struct<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Pack(
std::span<uint8_t> data,
const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value) {
constexpr size_t kDataOff = 0;
wpi::array<double, Rows * Cols> mat_data(wpi::empty_array);
for (int i = 0; i < Rows * Cols; i++) {
mat_data[i] = value(i);
}
wpi::PackStructArray<kDataOff, Rows * Cols>(data, mat_data);
}

23
wpimath/src/main/native/include/frc/struct/VectorStruct.h
View File

@@ -21,13 +21,28 @@ struct wpi::Struct<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>> {
static constexpr std::string_view GetSchema() { return kSchema; }
static frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> Unpack(
std::span<const uint8_t> data);
std::span<const uint8_t> data) {
constexpr size_t kDataOff = 0;
wpi::array<double, Size> vec_data =
wpi::UnpackStructArray<double, kDataOff, Size>(data);
frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> vec;
for (int i = 0; i < Size; i++) {
vec(i) = vec_data[i];
}
return vec;
}
static void Pack(
std::span<uint8_t> data,
const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value);
const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value) {
constexpr size_t kDataOff = 0;
wpi::array<double, Size> vec_data(wpi::empty_array);
for (int i = 0; i < Size; i++) {
vec_data[i] = value(i);
}
wpi::PackStructArray<kDataOff, Size>(data, vec_data);
}
};
static_assert(wpi::StructSerializable<frc::Vectord<1>>);
static_assert(wpi::StructSerializable<frc::Vectord<3>>);
#include "frc/struct/VectorStruct.inc"

33
wpimath/src/main/native/include/frc/struct/VectorStruct.inc
View File

@@ -1,33 +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/struct/VectorStruct.h"
template <int Size, int Options, int MaxRows, int MaxCols>
frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>
wpi::Struct<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Unpack(
std::span<const uint8_t> data) {
constexpr size_t kDataOff = 0;
wpi::array<double, Size> vec_data =
wpi::UnpackStructArray<double, kDataOff, Size>(data);
frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> vec;
for (int i = 0; i < Size; i++) {
vec(i) = vec_data[i];
}
return vec;
}
template <int Size, int Options, int MaxRows, int MaxCols>
void wpi::Struct<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Pack(
std::span<uint8_t> data,
const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value) {
constexpr size_t kDataOff = 0;
wpi::array<double, Size> vec_data(wpi::empty_array);
for (int i = 0; i < Size; i++) {
vec_data[i] = value(i);
}
wpi::PackStructArray<kDataOff, Size>(data, vec_data);
}

46
wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.h
View File

@@ -4,17 +4,49 @@
#pragma once
#include <stdexcept>
#include <fmt/format.h>
#include <wpi/ProtoHelper.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/proto/MatrixProto.h"
#include "frc/system/LinearSystem.h"
#include "system.pb.h"
template <int States, int Inputs, int Outputs>
struct wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>> {
static google::protobuf::Message* New(google::protobuf::Arena* arena);
static frc::LinearSystem<States, Inputs, Outputs> Unpack(
const google::protobuf::Message& msg);
static void Pack(google::protobuf::Message* msg,
const frc::LinearSystem<States, Inputs, Outputs>& value);
};
static google::protobuf::Message* New(google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufLinearSystem>(arena);
}
#include "frc/system/proto/LinearSystemProto.inc"
static frc::LinearSystem<States, Inputs, Outputs> Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufLinearSystem*>(&msg);
if (m->num_states() != States || m->num_inputs() != Inputs ||
m->num_outputs() != Outputs) {
throw std::invalid_argument(fmt::format(
"Tried to unpack message with {} states and {} inputs and {} outputs "
"into LinearSystem with {} states and {} inputs and {} outputs",
m->num_states(), m->num_inputs(), m->num_outputs(), States, Inputs,
Outputs));
}
return frc::LinearSystem<States, Inputs, Outputs>{
wpi::UnpackProtobuf<frc::Matrixd<States, States>>(m->wpi_a()),
wpi::UnpackProtobuf<frc::Matrixd<States, Inputs>>(m->wpi_b()),
wpi::UnpackProtobuf<frc::Matrixd<Outputs, States>>(m->wpi_c()),
wpi::UnpackProtobuf<frc::Matrixd<Outputs, Inputs>>(m->wpi_d())};
}
static void Pack(google::protobuf::Message* msg,
const frc::LinearSystem<States, Inputs, Outputs>& value) {
auto m = static_cast<wpi::proto::ProtobufLinearSystem*>(msg);
m->set_num_states(States);
m->set_num_inputs(Inputs);
m->set_num_outputs(Outputs);
wpi::PackProtobuf(m->mutable_a(), value.A());
wpi::PackProtobuf(m->mutable_b(), value.B());
wpi::PackProtobuf(m->mutable_c(), value.C());
wpi::PackProtobuf(m->mutable_d(), value.D());
}
};

55
wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.inc
View File

@@ -1,55 +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 <fmt/format.h>
#include <wpi/ProtoHelper.h>
#include "frc/proto/MatrixProto.h"
#include "frc/system/proto/LinearSystemProto.h"
#include "system.pb.h"
template <int States, int Inputs, int Outputs>
google::protobuf::Message*
wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufLinearSystem>(arena);
}
template <int States, int Inputs, int Outputs>
frc::LinearSystem<States, Inputs, Outputs>
wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufLinearSystem*>(&msg);
if (m->num_states() != States || m->num_inputs() != Inputs ||
m->num_outputs() != Outputs) {
throw std::invalid_argument(fmt::format(
"Tried to unpack message with {} states and {} inputs and {} outputs "
"into LinearSystem with {} states and {} inputs and {} outputs",
m->num_states(), m->num_inputs(), m->num_outputs(), States, Inputs,
Outputs));
}
return frc::LinearSystem<States, Inputs, Outputs>{
wpi::UnpackProtobuf<frc::Matrixd<States, States>>(m->wpi_a()),
wpi::UnpackProtobuf<frc::Matrixd<States, Inputs>>(m->wpi_b()),
wpi::UnpackProtobuf<frc::Matrixd<Outputs, States>>(m->wpi_c()),
wpi::UnpackProtobuf<frc::Matrixd<Outputs, Inputs>>(m->wpi_d())};
}
template <int States, int Inputs, int Outputs>
void wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>>::Pack(
google::protobuf::Message* msg,
const frc::LinearSystem<States, Inputs, Outputs>& value) {
auto m = static_cast<wpi::proto::ProtobufLinearSystem*>(msg);
m->set_num_states(States);
m->set_num_inputs(Inputs);
m->set_num_outputs(Outputs);
wpi::PackProtobuf(m->mutable_a(), value.A());
wpi::PackProtobuf(m->mutable_b(), value.B());
wpi::PackProtobuf(m->mutable_c(), value.C());
wpi::PackProtobuf(m->mutable_d(), value.D());
}

33
wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.h
View File

@@ -32,9 +32,36 @@ struct wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>> {
static constexpr std::string_view GetSchema() { return kSchema; }
static frc::LinearSystem<States, Inputs, Outputs> Unpack(
std::span<const uint8_t> data);
std::span<const uint8_t> data) {
constexpr size_t kAOff = 0;
constexpr size_t kBOff =
kAOff + wpi::GetStructSize<frc::Matrixd<States, States>>();
constexpr size_t kCOff =
kBOff + wpi::GetStructSize<frc::Matrixd<States, Inputs>>();
constexpr size_t kDOff =
kCOff + wpi::GetStructSize<frc::Matrixd<Outputs, States>>();
return frc::LinearSystem<States, Inputs, Outputs>{
wpi::UnpackStruct<frc::Matrixd<States, States>, kAOff>(data),
wpi::UnpackStruct<frc::Matrixd<States, Inputs>, kBOff>(data),
wpi::UnpackStruct<frc::Matrixd<Outputs, States>, kCOff>(data),
wpi::UnpackStruct<frc::Matrixd<Outputs, Inputs>, kDOff>(data)};
}
static void Pack(std::span<uint8_t> data,
const frc::LinearSystem<States, Inputs, Outputs>& value);
const frc::LinearSystem<States, Inputs, Outputs>& value) {
constexpr size_t kAOff = 0;
constexpr size_t kBOff =
kAOff + wpi::GetStructSize<frc::Matrixd<States, States>>();
constexpr size_t kCOff =
kBOff + wpi::GetStructSize<frc::Matrixd<States, Inputs>>();
constexpr size_t kDOff =
kCOff + wpi::GetStructSize<frc::Matrixd<Outputs, States>>();
wpi::PackStruct<kAOff>(data, value.A());
wpi::PackStruct<kBOff>(data, value.B());
wpi::PackStruct<kCOff>(data, value.C());
wpi::PackStruct<kDOff>(data, value.D());
}
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Matrixd<States, States>>(fn);
@@ -48,5 +75,3 @@ static_assert(wpi::StructSerializable<frc::LinearSystem<4, 3, 2>>);
static_assert(wpi::HasNestedStruct<frc::LinearSystem<4, 3, 2>>);
static_assert(wpi::StructSerializable<frc::LinearSystem<2, 3, 4>>);
static_assert(wpi::HasNestedStruct<frc::LinearSystem<2, 3, 4>>);
#include "frc/system/struct/LinearSystemStruct.inc"

42
wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.inc
View File

@@ -1,42 +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/system/struct/LinearSystemStruct.h"
template <int States, int Inputs, int Outputs>
frc::LinearSystem<States, Inputs, Outputs>
wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>>::Unpack(
std::span<const uint8_t> data) {
constexpr size_t kAOff = 0;
constexpr size_t kBOff =
kAOff + wpi::GetStructSize<frc::Matrixd<States, States>>();
constexpr size_t kCOff =
kBOff + wpi::GetStructSize<frc::Matrixd<States, Inputs>>();
constexpr size_t kDOff =
kCOff + wpi::GetStructSize<frc::Matrixd<Outputs, States>>();
return frc::LinearSystem<States, Inputs, Outputs>{
wpi::UnpackStruct<frc::Matrixd<States, States>, kAOff>(data),
wpi::UnpackStruct<frc::Matrixd<States, Inputs>, kBOff>(data),
wpi::UnpackStruct<frc::Matrixd<Outputs, States>, kCOff>(data),
wpi::UnpackStruct<frc::Matrixd<Outputs, Inputs>, kDOff>(data)};
}
template <int States, int Inputs, int Outputs>
void wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>>::Pack(
std::span<uint8_t> data,
const frc::LinearSystem<States, Inputs, Outputs>& value) {
constexpr size_t kAOff = 0;
constexpr size_t kBOff =
kAOff + wpi::GetStructSize<frc::Matrixd<States, States>>();
constexpr size_t kCOff =
kBOff + wpi::GetStructSize<frc::Matrixd<States, Inputs>>();
constexpr size_t kDOff =
kCOff + wpi::GetStructSize<frc::Matrixd<Outputs, States>>();
wpi::PackStruct<kAOff>(data, value.A());
wpi::PackStruct<kBOff>(data, value.B());
wpi::PackStruct<kCOff>(data, value.C());
wpi::PackStruct<kDOff>(data, value.D());
}

208
wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h
View File

@@ -4,8 +4,8 @@
#pragma once
#include "units/math.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
namespace frc {
@@ -134,7 +134,8 @@ class ExponentialProfile {
*
* @param constraints The constraints on the profile, like maximum input.
*/
explicit ExponentialProfile(Constraints constraints);
explicit ExponentialProfile(Constraints constraints)
: m_constraints(constraints) {}
ExponentialProfile(const ExponentialProfile&) = default;
ExponentialProfile& operator=(const ExponentialProfile&) = default;
@@ -152,7 +153,25 @@ class ExponentialProfile {
* @return The position and velocity of the profile at time t.
*/
State Calculate(const units::second_t& t, const State& current,
const State& goal) const;
const State& goal) const {
auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
auto u = direction * m_constraints.maxInput;
auto inflectionPoint = CalculateInflectionPoint(current, goal, u);
auto timing = CalculateProfileTiming(current, inflectionPoint, goal, u);
if (t < 0_s) {
return current;
} else if (t < timing.inflectionTime) {
return {ComputeDistanceFromTime(t, u, current),
ComputeVelocityFromTime(t, u, current)};
} else if (t < timing.totalTime) {
return {ComputeDistanceFromTime(t - timing.totalTime, -u, goal),
ComputeVelocityFromTime(t - timing.totalTime, -u, goal)};
} else {
return goal;
}
}
/**
* Calculates the point after which the fastest way to reach the goal state is
@@ -162,7 +181,13 @@ class ExponentialProfile {
* @param goal The desired state when the profile is complete.
* @return The position and velocity of the profile at the inflection point.
*/
State CalculateInflectionPoint(const State& current, const State& goal) const;
State CalculateInflectionPoint(const State& current,
const State& goal) const {
auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
auto u = direction * m_constraints.maxInput;
return CalculateInflectionPoint(current, goal, u);
}
/**
* Calculates the time it will take for this profile to reach the goal state.
@@ -171,7 +196,11 @@ class ExponentialProfile {
* @param goal The desired state when the profile is complete.
* @return The total duration of this profile.
*/
units::second_t TimeLeftUntil(const State& current, const State& goal) const;
units::second_t TimeLeftUntil(const State& current, const State& goal) const {
auto timing = CalculateProfileTiming(current, goal);
return timing.totalTime;
}
/**
* Calculates the time it will take for this profile to reach the inflection
@@ -182,7 +211,13 @@ class ExponentialProfile {
* @return The timing information for this profile.
*/
ProfileTiming CalculateProfileTiming(const State& current,
const State& goal) const;
const State& goal) const {
auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
auto u = direction * m_constraints.maxInput;
auto inflectionPoint = CalculateInflectionPoint(current, goal, u);
return CalculateProfileTiming(current, inflectionPoint, goal, u);
}
private:
/**
@@ -196,7 +231,19 @@ class ExponentialProfile {
* @return The position and velocity of the profile at the inflection point.
*/
State CalculateInflectionPoint(const State& current, const State& goal,
const Input_t& input) const;
const Input_t& input) const {
auto u = input;
if (current == goal) {
return current;
}
auto inflectionVelocity = SolveForInflectionVelocity(u, current, goal);
auto inflectionPosition =
ComputeDistanceFromVelocity(inflectionVelocity, -u, goal);
return {inflectionPosition, inflectionVelocity};
}
/**
* Calculates the time it will take for this profile to reach the inflection
@@ -212,7 +259,59 @@ class ExponentialProfile {
ProfileTiming CalculateProfileTiming(const State& current,
const State& inflectionPoint,
const State& goal,
const Input_t& input) const;
const Input_t& input) const {
auto u = input;
auto u_dir = units::math::abs(u) / u;
units::second_t inflectionT_forward;
// We need to handle 5 cases here:
//
// - Approaching -maxVelocity from below
// - Approaching -maxVelocity from above
// - Approaching maxVelocity from below
// - Approaching maxVelocity from above
// - At +-maxVelocity
//
// For cases 1 and 3, we want to subtract epsilon from the inflection point
// velocity For cases 2 and 4, we want to add epsilon to the inflection
// point velocity. For case 5, we have reached inflection point velocity.
auto epsilon = Velocity_t(1e-9);
if (units::math::abs(u_dir * m_constraints.MaxVelocity() -
inflectionPoint.velocity) < epsilon) {
auto solvableV = inflectionPoint.velocity;
units::second_t t_to_solvable_v;
Distance_t x_at_solvable_v;
if (units::math::abs(current.velocity - inflectionPoint.velocity) <
epsilon) {
t_to_solvable_v = 0_s;
x_at_solvable_v = current.position;
} else {
if (units::math::abs(current.velocity) > m_constraints.MaxVelocity()) {
solvableV += u_dir * epsilon;
} else {
solvableV -= u_dir * epsilon;
}
t_to_solvable_v =
ComputeTimeFromVelocity(solvableV, u, current.velocity);
x_at_solvable_v = ComputeDistanceFromVelocity(solvableV, u, current);
}
inflectionT_forward =
t_to_solvable_v + u_dir *
(inflectionPoint.position - x_at_solvable_v) /
m_constraints.MaxVelocity();
} else {
inflectionT_forward = ComputeTimeFromVelocity(inflectionPoint.velocity, u,
current.velocity);
}
auto inflectionT_backward =
ComputeTimeFromVelocity(inflectionPoint.velocity, -u, goal.velocity);
return {inflectionT_forward, inflectionT_forward - inflectionT_backward};
}
/**
* Calculates the position reached after t seconds when applying an input from
@@ -226,7 +325,16 @@ class ExponentialProfile {
*/
Distance_t ComputeDistanceFromTime(const units::second_t& time,
const Input_t& input,
const State& initial) const;
const State& initial) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
return initial.position +
(-B * u * time +
(initial.velocity + B * u / A) * (units::math::exp(A * time) - 1)) /
A;
}
/**
* Calculates the velocity reached after t seconds when applying an input from
@@ -240,7 +348,14 @@ class ExponentialProfile {
*/
Velocity_t ComputeVelocityFromTime(const units::second_t& time,
const Input_t& input,
const State& initial) const;
const State& initial) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
return (initial.velocity + B * u / A) * units::math::exp(A * time) -
B * u / A;
}
/**
* Calculates the time required to reach a specified velocity given the
@@ -254,7 +369,13 @@ class ExponentialProfile {
*/
units::second_t ComputeTimeFromVelocity(const Velocity_t& velocity,
const Input_t& input,
const Velocity_t& initial) const;
const Velocity_t& initial) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
return units::math::log((A * velocity + B * u) / (A * initial + B * u)) / A;
}
/**
* Calculates the distance reached at the same time as the given velocity when
@@ -268,7 +389,16 @@ class ExponentialProfile {
*/
Distance_t ComputeDistanceFromVelocity(const Velocity_t& velocity,
const Input_t& input,
const State& initial) const;
const State& initial) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
return initial.position + (velocity - initial.velocity) / A -
B * u / (A * A) *
units::math::log((A * velocity + B * u) /
(A * initial.velocity + B * u));
}
/**
* Calculates the velocity at which input should be reversed in order to reach
@@ -282,7 +412,30 @@ class ExponentialProfile {
*/
Velocity_t SolveForInflectionVelocity(const Input_t& input,
const State& current,
const State& goal) const;
const State& goal) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
auto u_dir = u / units::math::abs(u);
auto position_delta = goal.position - current.position;
auto velocity_delta = goal.velocity - current.velocity;
auto scalar = (A * current.velocity + B * u) * (A * goal.velocity - B * u);
auto power = -A / B / u * (A * position_delta - velocity_delta);
auto a = -A * A;
auto c = B * B * u * u + scalar * units::math::exp(power);
if (-1e-9 < c.value() && c.value() < 0) {
// numeric instability - the heuristic gets it right but c is around
// -1e-13
return Velocity_t(0);
}
return u_dir * units::math::sqrt(-c / a);
}
/**
* Returns true if the profile should be inverted.
@@ -293,10 +446,33 @@ class ExponentialProfile {
* @param current The initial state (usually the current state).
* @param goal The desired state when the profile is complete.
*/
bool ShouldFlipInput(const State& current, const State& goal) const;
bool ShouldFlipInput(const State& current, const State& goal) const {
auto u = m_constraints.maxInput;
auto v0 = current.velocity;
auto xf = goal.position;
auto vf = goal.velocity;
auto x_forward = ComputeDistanceFromVelocity(vf, u, current);
auto x_reverse = ComputeDistanceFromVelocity(vf, -u, current);
if (v0 >= m_constraints.MaxVelocity()) {
return xf < x_reverse;
}
if (v0 <= -m_constraints.MaxVelocity()) {
return xf < x_forward;
}
auto a = v0 >= Velocity_t(0);
auto b = vf >= Velocity_t(0);
auto c = xf >= x_forward;
auto d = xf >= x_reverse;
return (a && !d) || (b && !c) || (!c && !d);
}
Constraints m_constraints;
};
} // namespace frc
#include "ExponentialProfile.inc"
} // namespace frc

251
wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc
View File

@@ -1,251 +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 <algorithm>
#include "frc/trajectory/ExponentialProfile.h"
#include "units/math.h"
namespace frc {
template <class Distance, class Input>
ExponentialProfile<Distance, Input>::ExponentialProfile(Constraints constraints)
: m_constraints(constraints) {}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::State
ExponentialProfile<Distance, Input>::Calculate(const units::second_t& t,
const State& current,
const State& goal) const {
auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
auto u = direction * m_constraints.maxInput;
auto inflectionPoint = CalculateInflectionPoint(current, goal, u);
auto timing = CalculateProfileTiming(current, inflectionPoint, goal, u);
if (t < 0_s) {
return current;
} else if (t < timing.inflectionTime) {
return {ComputeDistanceFromTime(t, u, current),
ComputeVelocityFromTime(t, u, current)};
} else if (t < timing.totalTime) {
return {ComputeDistanceFromTime(t - timing.totalTime, -u, goal),
ComputeVelocityFromTime(t - timing.totalTime, -u, goal)};
} else {
return goal;
}
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::State
ExponentialProfile<Distance, Input>::CalculateInflectionPoint(
const State& current, const State& goal) const {
auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
auto u = direction * m_constraints.maxInput;
return CalculateInflectionPoint(current, goal, u);
}
template <class Distance, class Input>
units::second_t ExponentialProfile<Distance, Input>::TimeLeftUntil(
const State& current, const State& goal) const {
auto timing = CalculateProfileTiming(current, goal);
return timing.totalTime;
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::ProfileTiming
ExponentialProfile<Distance, Input>::CalculateProfileTiming(
const State& current, const State& goal) const {
auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
auto u = direction * m_constraints.maxInput;
auto inflectionPoint = CalculateInflectionPoint(current, goal, u);
return CalculateProfileTiming(current, inflectionPoint, goal, u);
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::State
ExponentialProfile<Distance, Input>::CalculateInflectionPoint(
const State& current, const State& goal, const Input_t& input) const {
auto u = input;
if (current == goal) {
return current;
}
auto inflectionVelocity = SolveForInflectionVelocity(u, current, goal);
auto inflectionPosition =
ComputeDistanceFromVelocity(inflectionVelocity, -u, goal);
return {inflectionPosition, inflectionVelocity};
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::ProfileTiming
ExponentialProfile<Distance, Input>::CalculateProfileTiming(
const State& current, const State& inflectionPoint, const State& goal,
const Input_t& input) const {
auto u = input;
auto u_dir = units::math::abs(u) / u;
units::second_t inflectionT_forward;
// We need to handle 5 cases here:
//
// - Approaching -maxVelocity from below
// - Approaching -maxVelocity from above
// - Approaching maxVelocity from below
// - Approaching maxVelocity from above
// - At +-maxVelocity
//
// For cases 1 and 3, we want to subtract epsilon from the inflection point
// velocity For cases 2 and 4, we want to add epsilon to the inflection point
// velocity. For case 5, we have reached inflection point velocity.
auto epsilon = Velocity_t(1e-9);
if (units::math::abs(u_dir * m_constraints.MaxVelocity() -
inflectionPoint.velocity) < epsilon) {
auto solvableV = inflectionPoint.velocity;
units::second_t t_to_solvable_v;
Distance_t x_at_solvable_v;
if (units::math::abs(current.velocity - inflectionPoint.velocity) <
epsilon) {
t_to_solvable_v = 0_s;
x_at_solvable_v = current.position;
} else {
if (units::math::abs(current.velocity) > m_constraints.MaxVelocity()) {
solvableV += u_dir * epsilon;
} else {
solvableV -= u_dir * epsilon;
}
t_to_solvable_v = ComputeTimeFromVelocity(solvableV, u, current.velocity);
x_at_solvable_v = ComputeDistanceFromVelocity(solvableV, u, current);
}
inflectionT_forward =
t_to_solvable_v + u_dir * (inflectionPoint.position - x_at_solvable_v) /
m_constraints.MaxVelocity();
} else {
inflectionT_forward =
ComputeTimeFromVelocity(inflectionPoint.velocity, u, current.velocity);
}
auto inflectionT_backward =
ComputeTimeFromVelocity(inflectionPoint.velocity, -u, goal.velocity);
return {inflectionT_forward, inflectionT_forward - inflectionT_backward};
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::Distance_t
ExponentialProfile<Distance, Input>::ComputeDistanceFromTime(
const units::second_t& time, const Input_t& input,
const State& initial) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
return initial.position +
(-B * u * time +
(initial.velocity + B * u / A) * (units::math::exp(A * time) - 1)) /
A;
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::Velocity_t
ExponentialProfile<Distance, Input>::ComputeVelocityFromTime(
const units::second_t& time, const Input_t& input,
const State& initial) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
return (initial.velocity + B * u / A) * units::math::exp(A * time) -
B * u / A;
}
template <class Distance, class Input>
units::second_t ExponentialProfile<Distance, Input>::ComputeTimeFromVelocity(
const Velocity_t& velocity, const Input_t& input,
const Velocity_t& initial) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
return units::math::log((A * velocity + B * u) / (A * initial + B * u)) / A;
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::Distance_t
ExponentialProfile<Distance, Input>::ComputeDistanceFromVelocity(
const Velocity_t& velocity, const Input_t& input,
const State& initial) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
return initial.position + (velocity - initial.velocity) / A -
B * u / (A * A) *
units::math::log((A * velocity + B * u) /
(A * initial.velocity + B * u));
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::Velocity_t
ExponentialProfile<Distance, Input>::SolveForInflectionVelocity(
const Input_t& input, const State& current, const State& goal) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
auto u_dir = u / units::math::abs(u);
auto position_delta = goal.position - current.position;
auto velocity_delta = goal.velocity - current.velocity;
auto scalar = (A * current.velocity + B * u) * (A * goal.velocity - B * u);
auto power = -A / B / u * (A * position_delta - velocity_delta);
auto a = -A * A;
auto c = B * B * u * u + scalar * units::math::exp(power);
if (-1e-9 < c.value() && c.value() < 0) {
// numeric instability - the heuristic gets it right but c is around -1e-13
return Velocity_t(0);
}
return u_dir * units::math::sqrt(-c / a);
}
template <class Distance, class Input>
bool ExponentialProfile<Distance, Input>::ShouldFlipInput(
const State& current, const State& goal) const {
auto u = m_constraints.maxInput;
auto v0 = current.velocity;
auto xf = goal.position;
auto vf = goal.velocity;
auto x_forward = ComputeDistanceFromVelocity(vf, u, current);
auto x_reverse = ComputeDistanceFromVelocity(vf, -u, current);
if (v0 >= m_constraints.MaxVelocity()) {
return xf < x_reverse;
}
if (v0 <= -m_constraints.MaxVelocity()) {
return xf < x_forward;
}
auto a = v0 >= Velocity_t(0);
auto b = vf >= Velocity_t(0);
auto c = xf >= x_forward;
auto d = xf >= x_reverse;
return (a && !d) || (b && !c) || (!c && !d);
}
} // namespace frc

143
wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
View File

@@ -4,6 +4,7 @@
#pragma once
#include "units/math.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
@@ -101,7 +102,9 @@ class TrapezoidProfile {
*
* @param constraints The constraints on the profile, like maximum velocity.
*/
TrapezoidProfile(Constraints constraints); // NOLINT
TrapezoidProfile(Constraints constraints) // NOLINT
: m_constraints(constraints) {}
TrapezoidProfile(const TrapezoidProfile&) = default;
TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
TrapezoidProfile(TrapezoidProfile&&) = default;
@@ -117,7 +120,74 @@ class TrapezoidProfile {
* @param goal The desired state when the profile is complete.
* @return The position and velocity of the profile at time t.
*/
State Calculate(units::second_t t, State current, State goal);
State Calculate(units::second_t t, State current, State goal) {
m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1;
m_current = Direct(current);
goal = Direct(goal);
if (m_current.velocity > m_constraints.maxVelocity) {
m_current.velocity = m_constraints.maxVelocity;
}
// Deal with a possibly truncated motion profile (with nonzero initial or
// final velocity) by calculating the parameters as if the profile began and
// ended at zero velocity
units::second_t cutoffBegin =
m_current.velocity / m_constraints.maxAcceleration;
Distance_t cutoffDistBegin =
cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
units::second_t cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
Distance_t cutoffDistEnd =
cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
// Now we can calculate the parameters as if it was a full trapezoid instead
// of a truncated one
Distance_t fullTrapezoidDist =
cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
units::second_t accelerationTime =
m_constraints.maxVelocity / m_constraints.maxAcceleration;
Distance_t fullSpeedDist =
fullTrapezoidDist -
accelerationTime * accelerationTime * m_constraints.maxAcceleration;
// Handle the case where the profile never reaches full speed
if (fullSpeedDist < Distance_t{0}) {
accelerationTime =
units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
fullSpeedDist = Distance_t{0};
}
m_endAccel = accelerationTime - cutoffBegin;
m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
m_endDecel = m_endFullSpeed + accelerationTime - cutoffEnd;
State result = m_current;
if (t < m_endAccel) {
result.velocity += t * m_constraints.maxAcceleration;
result.position +=
(m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
} else if (t < m_endFullSpeed) {
result.velocity = m_constraints.maxVelocity;
result.position += (m_current.velocity +
m_endAccel * m_constraints.maxAcceleration / 2.0) *
m_endAccel +
m_constraints.maxVelocity * (t - m_endAccel);
} else if (t <= m_endDecel) {
result.velocity =
goal.velocity + (m_endDecel - t) * m_constraints.maxAcceleration;
units::second_t timeLeft = m_endDecel - t;
result.position =
goal.position -
(goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
timeLeft;
} else {
result = goal;
}
return Direct(result);
}
/**
* Returns the time left until a target distance in the profile is reached.
@@ -125,7 +195,71 @@ class TrapezoidProfile {
* @param target The target distance.
* @return The time left until a target distance in the profile is reached.
*/
units::second_t TimeLeftUntil(Distance_t target) const;
units::second_t TimeLeftUntil(Distance_t target) const {
Distance_t position = m_current.position * m_direction;
Velocity_t velocity = m_current.velocity * m_direction;
units::second_t endAccel = m_endAccel * m_direction;
units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
if (target < position) {
endAccel *= -1.0;
endFullSpeed *= -1.0;
velocity *= -1.0;
}
endAccel = units::math::max(endAccel, 0_s);
endFullSpeed = units::math::max(endFullSpeed, 0_s);
const Acceleration_t acceleration = m_constraints.maxAcceleration;
const Acceleration_t deceleration = -m_constraints.maxAcceleration;
Distance_t distToTarget = units::math::abs(target - position);
if (distToTarget < Distance_t{1e-6}) {
return 0_s;
}
Distance_t accelDist =
velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
Velocity_t decelVelocity;
if (endAccel > 0_s) {
decelVelocity = units::math::sqrt(
units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
} else {
decelVelocity = velocity;
}
Distance_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
Distance_t decelDist;
if (accelDist > distToTarget) {
accelDist = distToTarget;
fullSpeedDist = Distance_t{0};
decelDist = Distance_t{0};
} else if (accelDist + fullSpeedDist > distToTarget) {
fullSpeedDist = distToTarget - accelDist;
decelDist = Distance_t{0};
} else {
decelDist = distToTarget - fullSpeedDist - accelDist;
}
units::second_t accelTime =
(-velocity + units::math::sqrt(units::math::abs(
velocity * velocity + 2 * acceleration * accelDist))) /
acceleration;
units::second_t decelTime =
(-decelVelocity +
units::math::sqrt(units::math::abs(decelVelocity * decelVelocity +
2 * deceleration * decelDist))) /
deceleration;
units::second_t fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
return accelTime + fullSpeedTime + decelTime;
}
/**
* Returns the total time the profile takes to reach the goal.
@@ -176,6 +310,5 @@ class TrapezoidProfile {
units::second_t m_endFullSpeed;
units::second_t m_endDecel;
};
} // namespace frc
#include "TrapezoidProfile.inc"
} // namespace frc

156
wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
View File

@@ -1,156 +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 <algorithm>
#include "frc/trajectory/TrapezoidProfile.h"
#include "units/math.h"
namespace frc {
template <class Distance>
TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints)
: m_constraints(constraints) {}
template <class Distance>
typename TrapezoidProfile<Distance>::State
TrapezoidProfile<Distance>::Calculate(units::second_t t, State current,
State goal) {
m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1;
m_current = Direct(current);
goal = Direct(goal);
if (m_current.velocity > m_constraints.maxVelocity) {
m_current.velocity = m_constraints.maxVelocity;
}
// Deal with a possibly truncated motion profile (with nonzero initial or
// final velocity) by calculating the parameters as if the profile began and
// ended at zero velocity
units::second_t cutoffBegin =
m_current.velocity / m_constraints.maxAcceleration;
Distance_t cutoffDistBegin =
cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
units::second_t cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
Distance_t cutoffDistEnd =
cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
// Now we can calculate the parameters as if it was a full trapezoid instead
// of a truncated one
Distance_t fullTrapezoidDist =
cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
units::second_t accelerationTime =
m_constraints.maxVelocity / m_constraints.maxAcceleration;
Distance_t fullSpeedDist =
fullTrapezoidDist -
accelerationTime * accelerationTime * m_constraints.maxAcceleration;
// Handle the case where the profile never reaches full speed
if (fullSpeedDist < Distance_t{0}) {
accelerationTime =
units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
fullSpeedDist = Distance_t{0};
}
m_endAccel = accelerationTime - cutoffBegin;
m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
m_endDecel = m_endFullSpeed + accelerationTime - cutoffEnd;
State result = m_current;
if (t < m_endAccel) {
result.velocity += t * m_constraints.maxAcceleration;
result.position +=
(m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
} else if (t < m_endFullSpeed) {
result.velocity = m_constraints.maxVelocity;
result.position += (m_current.velocity +
m_endAccel * m_constraints.maxAcceleration / 2.0) *
m_endAccel +
m_constraints.maxVelocity * (t - m_endAccel);
} else if (t <= m_endDecel) {
result.velocity =
goal.velocity + (m_endDecel - t) * m_constraints.maxAcceleration;
units::second_t timeLeft = m_endDecel - t;
result.position =
goal.position -
(goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
timeLeft;
} else {
result = goal;
}
return Direct(result);
}
template <class Distance>
units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
Distance_t target) const {
Distance_t position = m_current.position * m_direction;
Velocity_t velocity = m_current.velocity * m_direction;
units::second_t endAccel = m_endAccel * m_direction;
units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
if (target < position) {
endAccel *= -1.0;
endFullSpeed *= -1.0;
velocity *= -1.0;
}
endAccel = units::math::max(endAccel, 0_s);
endFullSpeed = units::math::max(endFullSpeed, 0_s);
const Acceleration_t acceleration = m_constraints.maxAcceleration;
const Acceleration_t deceleration = -m_constraints.maxAcceleration;
Distance_t distToTarget = units::math::abs(target - position);
if (distToTarget < Distance_t{1e-6}) {
return 0_s;
}
Distance_t accelDist =
velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
Velocity_t decelVelocity;
if (endAccel > 0_s) {
decelVelocity = units::math::sqrt(
units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
} else {
decelVelocity = velocity;
}
Distance_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
Distance_t decelDist;
if (accelDist > distToTarget) {
accelDist = distToTarget;
fullSpeedDist = Distance_t{0};
decelDist = Distance_t{0};
} else if (accelDist + fullSpeedDist > distToTarget) {
fullSpeedDist = distToTarget - accelDist;
decelDist = Distance_t{0};
} else {
decelDist = distToTarget - fullSpeedDist - accelDist;
}
units::second_t accelTime =
(-velocity + units::math::sqrt(units::math::abs(
velocity * velocity + 2 * acceleration * accelDist))) /
acceleration;
units::second_t decelTime =
(-decelVelocity +
units::math::sqrt(units::math::abs(decelVelocity * decelVelocity +
2 * deceleration * decelDist))) /
deceleration;
units::second_t fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
return accelTime + fullSpeedTime + decelTime;
}
} // namespace frc

23
wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
View File

@@ -8,6 +8,7 @@
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
#include "units/math.h"
#include "units/velocity.h"
namespace frc {
@@ -22,19 +23,31 @@ class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
public:
SwerveDriveKinematicsConstraint(
const frc::SwerveDriveKinematics<NumModules>& kinematics,
units::meters_per_second_t maxSpeed);
units::meters_per_second_t maxSpeed)
: m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override;
units::meters_per_second_t velocity) const override {
auto xVelocity = velocity * pose.Rotation().Cos();
auto yVelocity = velocity * pose.Rotation().Sin();
auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
{xVelocity, yVelocity, velocity * curvature});
m_kinematics.DesaturateWheelSpeeds(&wheelSpeeds, m_maxSpeed);
auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
}
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override;
units::meters_per_second_t speed) const override {
return {};
}
private:
frc::SwerveDriveKinematics<NumModules> m_kinematics;
units::meters_per_second_t m_maxSpeed;
};
} // namespace frc
#include "SwerveDriveKinematicsConstraint.inc"
} // namespace frc

42
wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
View File

@@ -1,42 +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/trajectory/constraint/SwerveDriveKinematicsConstraint.h"
#include "units/math.h"
namespace frc {
template <size_t NumModules>
SwerveDriveKinematicsConstraint<NumModules>::SwerveDriveKinematicsConstraint(
const frc::SwerveDriveKinematics<NumModules>& kinematics,
units::meters_per_second_t maxSpeed)
: m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
template <size_t NumModules>
units::meters_per_second_t
SwerveDriveKinematicsConstraint<NumModules>::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
auto xVelocity = velocity * pose.Rotation().Cos();
auto yVelocity = velocity * pose.Rotation().Sin();
auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
{xVelocity, yVelocity, velocity * curvature});
m_kinematics.DesaturateWheelSpeeds(&wheelSpeeds, m_maxSpeed);
auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
}
template <size_t NumModules>
TrajectoryConstraint::MinMax
SwerveDriveKinematicsConstraint<NumModules>::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const {
return {};
}
} // namespace frc