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

[wpimath] Add typedefs for common types

Browse Source 
This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>
This commit is contained in:
Peter Johnson
2022-04-29 22:29:20 -07:00
parent 97c493241f
commit e767605e94
76 changed files with 1136 additions and 1449 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
wpimath/src/main/native/include/frc/system/NumericalJacobian.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include "Eigen/Core"
#include "frc/EigenCore.h"
namespace frc {
@@ -17,16 +17,16 @@ namespace frc {
* @param x Vector argument.
*/
template <int Rows, int Cols, typename F>
auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& x) {
auto NumericalJacobian(F&& f, const Vectord<Cols>& x) {
constexpr double kEpsilon = 1e-5;
Eigen::Matrix<double, Rows, Cols> result;
Matrixd<Rows, Cols> result;
result.setZero();
// It's more expensive, but +- epsilon will be more accurate
for (int i = 0; i < Cols; ++i) {
Eigen::Vector<double, Cols> dX_plus = x;
Vectord<Cols> dX_plus = x;
dX_plus(i) += kEpsilon;
Eigen::Vector<double, Cols> dX_minus = x;
Vectord<Cols> dX_minus = x;
dX_minus(i) -= kEpsilon;
result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
}
@@ -48,12 +48,10 @@ auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& x) {
* @param args Remaining arguments to f(x, u, ...).
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u,
Args&&... args) {
auto NumericalJacobianX(F&& f, const Vectord<States>& x,
const Vectord<Inputs>& u, Args&&... args) {
return NumericalJacobian<Rows, States>(
[&](const Eigen::Vector<double, States>& x) { return f(x, u, args...); },
x);
[&](const Vectord<States>& x) { return f(x, u, args...); }, x);
}
/**
@@ -70,12 +68,10 @@ auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
* @param args Remaining arguments to f(x, u, ...).
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
auto NumericalJacobianU(F&& f, const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u,
Args&&... args) {
auto NumericalJacobianU(F&& f, const Vectord<States>& x,
const Vectord<Inputs>& u, Args&&... args) {
return NumericalJacobian<Rows, Inputs>(
[&](const Eigen::Vector<double, Inputs>& u) { return f(x, u, args...); },
u);
[&](const Vectord<Inputs>& u) { return f(x, u, args...); }, u);
}
} // namespace frc