1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
// 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/EigenCore.h"
namespace frc {
/**
* Returns numerical Jacobian with respect to x for f(x).
*
* @tparam Rows Number of rows in result of f(x).
* @tparam Cols Number of columns in result of f(x).
* @param f Vector-valued function from which to compute Jacobian.
* @param x Vector argument.
*/
template <int Rows, int Cols, typename F>
auto NumericalJacobian(F&& f, const Vectord<Cols>& x) {
constexpr double kEpsilon = 1e-5;
Matrixd<Rows, Cols> result;
result.setZero();
// It's more expensive, but +- epsilon will be more accurate
for (int i = 0; i < Cols; ++i) {
Vectord<Cols> dX_plus = x;
dX_plus(i) += kEpsilon;
Vectord<Cols> dX_minus = x;
dX_minus(i) -= kEpsilon;
result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
}
return result;
}
/**
* Returns numerical Jacobian with respect to x for f(x).
*
* @param f Vector-valued function from which to compute Jacobian.
* @param x Vector argument.
*/
template <typename F>
Eigen::MatrixXd NumericalJacobian(F&& f, const Eigen::VectorXd& x) {
constexpr double kEpsilon = 1e-5;
Eigen::MatrixXd result;
// It's more expensive, but +- epsilon will be more accurate
for (int i = 0; i < x.rows(); ++i) {
Eigen::VectorXd dX_plus = x;
dX_plus(i) += kEpsilon;
Eigen::VectorXd dX_minus = x;
dX_minus(i) -= kEpsilon;
Eigen::VectorXd partialDerivative =
(f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
if (i == 0) {
result.resize(partialDerivative.rows(), x.rows());
result.setZero();
}
result.col(i) = partialDerivative;
}
return result;
}
/**
* Returns numerical Jacobian with respect to x for f(x, u, ...).
*
* @tparam Rows Number of rows in result of f(x, u, ...).
* @tparam States Number of rows in x.
* @tparam Inputs Number of rows in u.
* @tparam F Function object type.
* @tparam Args... Types of remaining arguments to f(x, u, ...).
* @param f Vector-valued function from which to compute Jacobian.
* @param x State vector.
* @param u Input vector.
* @param args Remaining arguments to f(x, u, ...).
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
auto NumericalJacobianX(F&& f, const Vectord<States>& x,
const Vectord<Inputs>& u, Args&&... args) {
return NumericalJacobian<Rows, States>(
[&](const Vectord<States>& x) { return f(x, u, args...); }, x);
}
/**
* Returns numerical Jacobian with respect to x for f(x, u, ...).
*
* @tparam F Function object type.
* @tparam Args... Types of remaining arguments to f(x, u, ...).
* @param f Vector-valued function from which to compute Jacobian.
* @param x State vector.
* @param u Input vector.
* @param args Remaining arguments to f(x, u, ...).
*/
template <typename F, typename... Args>
auto NumericalJacobianX(F&& f, const Eigen::VectorXd& x,
const Eigen::VectorXd& u, Args&&... args) {
return NumericalJacobian(
[&](const Eigen::VectorXd& x) { return f(x, u, args...); }, x);
}
/**
* Returns numerical Jacobian with respect to u for f(x, u, ...).
*
* @tparam Rows Number of rows in result of f(x, u, ...).
* @tparam States Number of rows in x.
* @tparam Inputs Number of rows in u.
* @tparam F Function object type.
* @tparam Args... Types of remaining arguments to f(x, u, ...).
* @param f Vector-valued function from which to compute Jacobian.
* @param x State vector.
* @param u Input vector.
* @param args Remaining arguments to f(x, u, ...).
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
auto NumericalJacobianU(F&& f, const Vectord<States>& x,
const Vectord<Inputs>& u, Args&&... args) {
return NumericalJacobian<Rows, Inputs>(
[&](const Vectord<Inputs>& u) { return f(x, u, args...); }, u);
}
/**
* Returns numerical Jacobian with respect to u for f(x, u, ...).
*
* @tparam F Function object type.
* @tparam Args... Types of remaining arguments to f(x, u, ...).
* @param f Vector-valued function from which to compute Jacobian.
* @param x State vector.
* @param u Input vector.
* @param args Remaining arguments to f(x, u, ...).
*/
template <typename F, typename... Args>
auto NumericalJacobianU(F&& f, const Eigen::VectorXd& x,
const Eigen::VectorXd& u, Args&&... args) {
return NumericalJacobian(
[&](const Eigen::VectorXd& u) { return f(x, u, args...); }, u);
}
} // namespace frc