mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpimath] Move math functionality into new wpimath library (#2629)
The wpimath library is a new library designed to separate the reusable math functionality from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j). Package names / include file names were NOT changed to minimize breakage. In a future year it would be good to revamp these for a more uniform user experience and to reduce the risk of accidental naming conflicts. While theoretically all of this functionality could be placed into wpiutil, several pieces of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this expense for users who only want cscore or ntcore. It also allows for easy future separation of build tasks vs number of workers on memory-constrained machines. This moves the following functionality from wpiutil into wpimath: - Eigen - ejml - Drake - DARE - wpiutil.math package (Matrix etc) - units And the following functionality from wpilibc/j into wpimath: - Geometry - Kinematics - Spline - Trajectory - LinearFilter - MedianFilter - Feed-forward controllers
This commit is contained in:
Peter Johnson
@@ -1,87 +0,0 @@
|
||||
// This file contains the implementation of both drake_assert and drake_throw.
|
||||
/* clang-format off to disable clang-format-includes */
|
||||
#include "drake/common/drake_assert.h"
|
||||
#include "drake/common/drake_throw.h"
|
||||
/* clang-format on */
|
||||
|
||||
#include <atomic>
|
||||
#include <cstdlib>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include "drake/common/drake_assertion_error.h"
|
||||
#include "drake/common/never_destroyed.h"
|
||||
|
||||
namespace drake {
|
||||
namespace internal {
|
||||
namespace {
|
||||
|
||||
// Singleton to manage assertion configuration.
|
||||
struct AssertionConfig {
|
||||
static AssertionConfig& singleton() {
|
||||
static never_destroyed<AssertionConfig> global;
|
||||
return global.access();
|
||||
}
|
||||
|
||||
std::atomic<bool> assertion_failures_are_exceptions;
|
||||
};
|
||||
|
||||
// Stream into @p out the given failure details; only @p condition may be null.
|
||||
void PrintFailureDetailTo(std::ostream& out, const char* condition,
|
||||
const char* func, const char* file, int line) {
|
||||
out << "Failure at " << file << ":" << line << " in " << func << "()";
|
||||
if (condition) {
|
||||
out << ": condition '" << condition << "' failed.";
|
||||
} else {
|
||||
out << ".";
|
||||
}
|
||||
}
|
||||
} // namespace
|
||||
|
||||
// Declared in drake_assert.h.
|
||||
void Abort(const char* condition, const char* func, const char* file,
|
||||
int line) {
|
||||
std::cerr << "abort: ";
|
||||
PrintFailureDetailTo(std::cerr, condition, func, file, line);
|
||||
std::cerr << std::endl;
|
||||
std::abort();
|
||||
}
|
||||
|
||||
// Declared in drake_throw.h.
|
||||
void Throw(const char* condition, const char* func, const char* file,
|
||||
int line) {
|
||||
std::ostringstream what;
|
||||
PrintFailureDetailTo(what, condition, func, file, line);
|
||||
throw assertion_error(what.str().c_str());
|
||||
}
|
||||
|
||||
// Declared in drake_assert.h.
|
||||
void AssertionFailed(const char* condition, const char* func, const char* file,
|
||||
int line) {
|
||||
if (AssertionConfig::singleton().assertion_failures_are_exceptions) {
|
||||
Throw(condition, func, file, line);
|
||||
} else {
|
||||
Abort(condition, func, file, line);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace internal
|
||||
} // namespace drake
|
||||
|
||||
// Configures the DRAKE_ASSERT and DRAKE_DEMAND assertion failure handling
|
||||
// behavior.
|
||||
//
|
||||
// By default, assertion failures will result in an ::abort(). If this method
|
||||
// has ever been called, failures will result in a thrown exception instead.
|
||||
//
|
||||
// Assertion configuration has process-wide scope. Changes here will affect
|
||||
// all assertions within the current process.
|
||||
//
|
||||
// This method is intended ONLY for use by pydrake bindings, and thus is not
|
||||
// declared in any header file, to discourage anyone from using it.
|
||||
extern "C" void drake_set_assertion_failure_to_throw_exception() {
|
||||
drake::internal::AssertionConfig::singleton().
|
||||
assertion_failures_are_exceptions = true;
|
||||
}
|
||||
@@ -1,463 +0,0 @@
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
|
||||
#include "drake/common/drake_assert.h"
|
||||
#include "drake/common/drake_throw.h"
|
||||
#include "drake/common/is_approx_equal_abstol.h"
|
||||
|
||||
#include <Eigen/Eigenvalues>
|
||||
#include <Eigen/QR>
|
||||
|
||||
// This code has https://github.com/RobotLocomotion/drake/pull/11118 applied to
|
||||
// fix an infinite loop in reorder_eigen().
|
||||
|
||||
namespace drake {
|
||||
namespace math {
|
||||
namespace {
|
||||
/* helper functions */
|
||||
template <typename T>
|
||||
int sgn(T val) {
|
||||
return (T(0) < val) - (val < T(0));
|
||||
}
|
||||
void check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& B) {
|
||||
// This function checks if (A,B) is a stabilizable pair.
|
||||
// (A,B) is stabilizable if and only if the uncontrollable eigenvalues of
|
||||
// A, if any, have absolute values less than one, where an eigenvalue is
|
||||
// uncontrollable if Rank[lambda * I - A, B] < n.
|
||||
int n = B.rows(), m = B.cols();
|
||||
Eigen::EigenSolver<Eigen::MatrixXd> es(A);
|
||||
for (int i = 0; i < n; i++) {
|
||||
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
|
||||
es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
|
||||
1)
|
||||
continue;
|
||||
Eigen::MatrixXcd E(n, n + m);
|
||||
E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B;
|
||||
Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr(E);
|
||||
DRAKE_THROW_UNLESS(qr.rank() == n);
|
||||
}
|
||||
}
|
||||
void check_detectable(const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& Q) {
|
||||
// This function check if (A,C) is a detectable pair, where Q = C' * C.
|
||||
// (A,C) is detectable if and only if the unobservable eigenvalues of A,
|
||||
// if any, have absolute values less than one, where an eigenvalue is
|
||||
// unobservable if Rank[lambda * I - A; C] < n.
|
||||
// Also, (A,C) is detectable if and only if (A',C') is stabilizable.
|
||||
int n = A.rows();
|
||||
Eigen::LDLT<Eigen::MatrixXd> ldlt(Q);
|
||||
Eigen::MatrixXd L = ldlt.matrixL();
|
||||
Eigen::MatrixXd D = ldlt.vectorD();
|
||||
Eigen::MatrixXd D_sqrt = Eigen::MatrixXd::Zero(n, n);
|
||||
for (int i = 0; i < n; i++) {
|
||||
D_sqrt(i, i) = sqrt(D(i));
|
||||
}
|
||||
Eigen::MatrixXd C = L * D_sqrt;
|
||||
check_stabilizable(A.transpose(), C.transpose());
|
||||
}
|
||||
|
||||
// "Givens rotation" computes an orthogonal 2x2 matrix R such that
|
||||
// it eliminates the 2nd coordinate of the vector [a,b]', i.e.,
|
||||
// R * [ a ] = [ a_hat ]
|
||||
// [ b ] [ 0 ]
|
||||
// The implementation is based on
|
||||
// https://en.wikipedia.org/wiki/Givens_rotation#Stable_calculation
|
||||
void Givens_rotation(double a, double b, Eigen::Ref<Eigen::Matrix2d> R,
|
||||
double eps = 1e-10) {
|
||||
double c, s;
|
||||
if (fabs(b) < eps) {
|
||||
c = (a < -eps ? -1 : 1);
|
||||
s = 0;
|
||||
} else if (fabs(a) < eps) {
|
||||
c = 0;
|
||||
s = -sgn(b);
|
||||
} else if (fabs(a) > fabs(b)) {
|
||||
double t = b / a;
|
||||
double u = sgn(a) * fabs(sqrt(1 + t * t));
|
||||
c = 1 / u;
|
||||
s = -c * t;
|
||||
} else {
|
||||
double t = a / b;
|
||||
double u = sgn(b) * fabs(sqrt(1 + t * t));
|
||||
s = -1 / u;
|
||||
c = -s * t;
|
||||
}
|
||||
R(0, 0) = c, R(0, 1) = -s, R(1, 0) = s, R(1, 1) = c;
|
||||
}
|
||||
|
||||
// The arguments S, T, and Z will be changed.
|
||||
void swap_block_11(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
|
||||
Eigen::Ref<Eigen::MatrixXd> Z, int p) {
|
||||
// Dooren, Case I, p124-125.
|
||||
int n2 = S.rows();
|
||||
Eigen::Matrix2d A = S.block<2, 2>(p, p);
|
||||
Eigen::Matrix2d B = T.block<2, 2>(p, p);
|
||||
Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Eigen::Matrix2d H = A(1, 1) * B - B(1, 1) * A;
|
||||
Givens_rotation(H(0, 1), H(0, 0), Z1.block<2, 2>(p, p));
|
||||
S = (S * Z1).eval();
|
||||
T = (T * Z1).eval();
|
||||
Z = (Z * Z1).eval();
|
||||
Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Givens_rotation(T(p, p), T(p + 1, p), Q.block<2, 2>(p, p));
|
||||
S = (Q * S).eval();
|
||||
T = (Q * T).eval();
|
||||
S(p + 1, p) = 0;
|
||||
T(p + 1, p) = 0;
|
||||
}
|
||||
|
||||
// The arguments S, T, and Z will be changed.
|
||||
void swap_block_21(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
|
||||
Eigen::Ref<Eigen::MatrixXd> Z, int p) {
|
||||
// Dooren, Case II, p126-127.
|
||||
int n2 = S.rows();
|
||||
Eigen::Matrix3d A = S.block<3, 3>(p, p);
|
||||
Eigen::Matrix3d B = T.block<3, 3>(p, p);
|
||||
// Compute H and eliminate H(1,0) by row operation.
|
||||
Eigen::Matrix3d H = A(2, 2) * B - B(2, 2) * A;
|
||||
Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3);
|
||||
Givens_rotation(H(0, 0), H(1, 0), R.block<2, 2>(0, 0));
|
||||
H = (R * H).eval();
|
||||
Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
// Compute Z1, Z2, Q1, Q2.
|
||||
Givens_rotation(H(1, 2), H(1, 1), Z1.block<2, 2>(p + 1, p + 1));
|
||||
H = (H * Z1.block<3, 3>(p, p)).eval();
|
||||
Givens_rotation(H(0, 1), H(0, 0), Z2.block<2, 2>(p, p));
|
||||
S = (S * Z1).eval();
|
||||
T = (T * Z1).eval();
|
||||
Z = (Z * Z1 * Z2).eval();
|
||||
Givens_rotation(T(p + 1, p + 1), T(p + 2, p + 1),
|
||||
Q1.block<2, 2>(p + 1, p + 1));
|
||||
S = (Q1 * S * Z2).eval();
|
||||
T = (Q1 * T * Z2).eval();
|
||||
Givens_rotation(T(p, p), T(p + 1, p), Q2.block<2, 2>(p, p));
|
||||
S = (Q2 * S).eval();
|
||||
T = (Q2 * T).eval();
|
||||
S(p + 1, p) = 0;
|
||||
S(p + 2, p) = 0;
|
||||
T(p + 1, p) = 0;
|
||||
T(p + 2, p) = 0;
|
||||
T(p + 2, p + 1) = 0;
|
||||
}
|
||||
|
||||
// The arguments S, T, and Z will be changed.
|
||||
void swap_block_12(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
|
||||
Eigen::Ref<Eigen::MatrixXd> Z, int p) {
|
||||
int n2 = S.rows();
|
||||
// Swap the role of S and T.
|
||||
Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Eigen::MatrixXd Q0 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Givens_rotation(S(p + 1, p + 1), S(p + 2, p + 1),
|
||||
Q0.block<2, 2>(p + 1, p + 1));
|
||||
S = (Q0 * S).eval();
|
||||
T = (Q0 * T).eval();
|
||||
Eigen::Matrix3d A = S.block<3, 3>(p, p);
|
||||
Eigen::Matrix3d B = T.block<3, 3>(p, p);
|
||||
// Compute H and eliminate H(2,1) by column operation.
|
||||
Eigen::Matrix3d H = B(0, 0) * A - A(0, 0) * B;
|
||||
Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3);
|
||||
Givens_rotation(H(2, 2), H(2, 1), R.block<2, 2>(1, 1));
|
||||
H = (H * R).eval();
|
||||
// Compute Q1, Q2, Z1, Z2.
|
||||
Givens_rotation(H(0, 1), H(1, 1), Q1.block<2, 2>(p, p));
|
||||
H = (Q1.block<3, 3>(p, p) * H).eval();
|
||||
Givens_rotation(H(1, 2), H(2, 2), Q2.block<2, 2>(p + 1, p + 1));
|
||||
S = (Q1 * S).eval();
|
||||
T = (Q1 * T).eval();
|
||||
Givens_rotation(S(p + 1, p + 1), S(p + 1, p), Z1.block<2, 2>(p, p));
|
||||
S = (Q2 * S * Z1).eval();
|
||||
T = (Q2 * T * Z1).eval();
|
||||
Givens_rotation(S(p + 2, p + 2), S(p + 2, p + 1),
|
||||
Z2.block<2, 2>(p + 1, p + 1));
|
||||
S = (S * Z2).eval();
|
||||
T = (T * Z2).eval();
|
||||
Z = (Z * Z1 * Z2).eval();
|
||||
// Swap back the role of S and T.
|
||||
Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p));
|
||||
S = (Q3 * S).eval();
|
||||
T = (Q3 * T).eval();
|
||||
S(p + 2, p) = 0;
|
||||
S(p + 2, p + 1) = 0;
|
||||
T(p + 1, p) = 0;
|
||||
T(p + 2, p) = 0;
|
||||
T(p + 2, p + 1) = 0;
|
||||
}
|
||||
|
||||
// The arguments S, T, and Z will be changed.
|
||||
void swap_block_22(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
|
||||
Eigen::Ref<Eigen::MatrixXd> Z, int p) {
|
||||
// Direct Swapping Algorithm based on
|
||||
// "Numerical Methods for General and Structured Eigenvalue Problems" by
|
||||
// Daniel Kressner, p108-111.
|
||||
// ( http://sma.epfl.ch/~anchpcommon/publications/kressner_eigenvalues.pdf ).
|
||||
// Also relevant but not applicable here:
|
||||
// "On Swapping Diagonal Blocks in Real Schur Form" by Zhaojun Bai and James
|
||||
// W. Demmelt;
|
||||
int n2 = S.rows();
|
||||
Eigen::MatrixXd A = S.block<4, 4>(p, p);
|
||||
Eigen::MatrixXd B = T.block<4, 4>(p, p);
|
||||
// Solve
|
||||
// A11 * X - Y A22 = A12
|
||||
// B11 * X - Y B22 = B12
|
||||
// Reduce to solve Cx=D, where x=[x1;...;x4;y1;...;y4].
|
||||
Eigen::Matrix<double, 8, 8> C = Eigen::Matrix<double, 8, 8>::Zero();
|
||||
Eigen::Matrix<double, 8, 1> D;
|
||||
// clang-format off
|
||||
C << A(0, 0), 0, A(0, 1), 0, -A(2, 2), -A(3, 2), 0, 0,
|
||||
0, A(0, 0), 0, A(0, 1), -A(2, 3), -A(3, 3), 0, 0,
|
||||
A(1, 0), 0, A(1, 1), 0, 0, 0, -A(2, 2), -A(3, 2),
|
||||
0, A(1, 0), 0, A(1, 1), 0, 0, -A(2, 3), -A(3, 3),
|
||||
B(0, 0), 0, B(0, 1), 0, -B(2, 2), -B(3, 2), 0, 0,
|
||||
0, B(0, 0), 0, B(0, 1), -B(2, 3), -B(3, 3), 0, 0,
|
||||
B(1, 0), 0, B(1, 1), 0, 0, 0, -B(2, 2), -B(3, 2),
|
||||
0, B(1, 0), 0, B(1, 1), 0, 0, -B(2, 3), -B(3, 3);
|
||||
// clang-format on
|
||||
D << A(0, 2), A(0, 3), A(1, 2), A(1, 3), B(0, 2), B(0, 3), B(1, 2), B(1, 3);
|
||||
Eigen::MatrixXd x = C.colPivHouseholderQr().solve(D);
|
||||
// Q * [ -Y ] = [ R_Y ] , Z' * [ -X ] = [ R_X ] .
|
||||
// [ I ] [ 0 ] [ I ] = [ 0 ]
|
||||
Eigen::Matrix<double, 4, 2> X, Y;
|
||||
X << -x(0, 0), -x(1, 0), -x(2, 0), -x(3, 0), Eigen::Matrix2d::Identity();
|
||||
Y << -x(4, 0), -x(5, 0), -x(6, 0), -x(7, 0), Eigen::Matrix2d::Identity();
|
||||
Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > qr1(X);
|
||||
Z1.block<4, 4>(p, p) = qr1.householderQ();
|
||||
Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > qr2(Y);
|
||||
Q1.block<4, 4>(p, p) = qr2.householderQ().adjoint();
|
||||
// Apply transform Q1 * (S,T) * Z1.
|
||||
S = (Q1 * S * Z1).eval();
|
||||
T = (Q1 * T * Z1).eval();
|
||||
Z = (Z * Z1).eval();
|
||||
// Eliminate the T(p+3,p+2) entry.
|
||||
Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Givens_rotation(T(p + 2, p + 2), T(p + 3, p + 2),
|
||||
Q2.block<2, 2>(p + 2, p + 2));
|
||||
S = (Q2 * S).eval();
|
||||
T = (Q2 * T).eval();
|
||||
// Eliminate the T(p+1,p) entry.
|
||||
Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2);
|
||||
Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p));
|
||||
S = (Q3 * S).eval();
|
||||
T = (Q3 * T).eval();
|
||||
S(p + 2, p) = 0;
|
||||
S(p + 2, p + 1) = 0;
|
||||
S(p + 3, p) = 0;
|
||||
S(p + 3, p + 1) = 0;
|
||||
T(p + 1, p) = 0;
|
||||
T(p + 2, p) = 0;
|
||||
T(p + 2, p + 1) = 0;
|
||||
T(p + 3, p) = 0;
|
||||
T(p + 3, p + 1) = 0;
|
||||
T(p + 3, p + 2) = 0;
|
||||
}
|
||||
|
||||
// Functionality of "swap_block" function:
|
||||
// swap the 1x1 or 2x2 blocks pointed by p and q.
|
||||
// There are four cases: swapping 1x1 and 1x1 matrices, swapping 2x2 and 1x1
|
||||
// matrices, swapping 1x1 and 2x2 matrices, and swapping 2x2 and 2x2 matrices.
|
||||
// Algorithms are described in the papers
|
||||
// "A generalized eigenvalue approach for solving Riccati equations" by P. Van
|
||||
// Dooren, 1981 ( http://epubs.siam.org/doi/pdf/10.1137/0902010 ), and
|
||||
// "Numerical Methods for General and Structured Eigenvalue Problems" by
|
||||
// Daniel Kressner, 2005.
|
||||
void swap_block(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
|
||||
Eigen::Ref<Eigen::MatrixXd> Z, int p, int q, int q_block_size,
|
||||
double eps = 1e-10) {
|
||||
int p_tmp = q, p_block_size;
|
||||
while (p_tmp-- > p) {
|
||||
p_block_size = 1;
|
||||
if (p_tmp >= 1 && fabs(S(p_tmp, p_tmp - 1)) > eps) {
|
||||
p_block_size = 2;
|
||||
p_tmp--;
|
||||
}
|
||||
switch (p_block_size * 10 + q_block_size) {
|
||||
case 11:
|
||||
swap_block_11(S, T, Z, p_tmp);
|
||||
break;
|
||||
case 21:
|
||||
swap_block_21(S, T, Z, p_tmp);
|
||||
break;
|
||||
case 12:
|
||||
swap_block_12(S, T, Z, p_tmp);
|
||||
break;
|
||||
case 22:
|
||||
swap_block_22(S, T, Z, p_tmp);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Functionality of "reorder_eigen" function:
|
||||
// Reorder the eigenvalues of (S,T) such that the top-left n by n matrix has
|
||||
// stable eigenvalues by multiplying Q's and Z's on the left and the right,
|
||||
// respectively.
|
||||
// Stable eigenvalues are inside the unit disk.
|
||||
//
|
||||
// Algorithm:
|
||||
// Go along the diagonals of (S,T) from the top left to the bottom right.
|
||||
// Once find a stable eigenvalue, push it to top left.
|
||||
// In implementation, use two pointers, p and q.
|
||||
// p points to the current block (1x1 or 2x2) and q points to the block with the
|
||||
// stable eigenvalue(s).
|
||||
// Push the block pointed by q to the position pointed by p.
|
||||
// Finish when n stable eigenvalues are placed at the top-left n by n matrix.
|
||||
// The algorithm for swapping blocks is described in the papers
|
||||
// "A generalized eigenvalue approach for solving Riccati equations" by P. Van
|
||||
// Dooren, 1981, and "Numerical Methods for General and Structured Eigenvalue
|
||||
// Problems" by Daniel Kressner, 2005.
|
||||
void reorder_eigen(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
|
||||
Eigen::Ref<Eigen::MatrixXd> Z, double eps = 1e-10) {
|
||||
// abs(a) < eps => a = 0
|
||||
int n2 = S.rows();
|
||||
int n = n2 / 2, p = 0, q = 0;
|
||||
|
||||
// Find the first unstable p block.
|
||||
while (p < n) {
|
||||
if (fabs(S(p + 1, p)) < eps) { // p block size = 1
|
||||
if (fabs(T(p, p)) > eps && fabs(S(p, p)) <= fabs(T(p, p))) { // stable
|
||||
p++;
|
||||
continue;
|
||||
}
|
||||
} else { // p block size = 2
|
||||
const double det_T =
|
||||
T(p, p) * T(p + 1, p + 1) - T(p + 1, p) * T(p, p + 1);
|
||||
if (fabs(det_T) > eps) {
|
||||
const double det_S =
|
||||
S(p, p) * S(p + 1, p + 1) - S(p + 1, p) * S(p, p + 1);
|
||||
if (fabs(det_S) <= fabs(det_T)) { // stable
|
||||
p += 2;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
q = p;
|
||||
|
||||
// Make the first n generalized eigenvalues stable.
|
||||
while (p < n && q < n2) {
|
||||
// Update q.
|
||||
int q_block_size = 0;
|
||||
while (q < n2) {
|
||||
if (q == n2 - 1 || fabs(S(q + 1, q)) < eps) { // block size = 1
|
||||
if (fabs(T(q, q)) > eps && fabs(S(q, q)) <= fabs(T(q, q))) {
|
||||
q_block_size = 1;
|
||||
break;
|
||||
}
|
||||
q++;
|
||||
} else { // block size = 2
|
||||
const double det_T =
|
||||
T(q, q) * T(q + 1, q + 1) - T(q + 1, q) * T(q, q + 1);
|
||||
if (fabs(det_T) > eps) {
|
||||
const double det_S =
|
||||
S(q, q) * S(q + 1, q + 1) - S(q + 1, q) * S(q, q + 1);
|
||||
if (fabs(det_S) <= fabs(det_T)) {
|
||||
q_block_size = 2;
|
||||
break;
|
||||
}
|
||||
}
|
||||
q += 2;
|
||||
}
|
||||
}
|
||||
if (q >= n2)
|
||||
throw std::runtime_error("fail to find enough stable eigenvalues");
|
||||
// Swap blocks pointed by p and q.
|
||||
if (p != q) {
|
||||
swap_block(S, T, Z, p, q, q_block_size);
|
||||
p += q_block_size;
|
||||
q += q_block_size;
|
||||
}
|
||||
}
|
||||
if (p < n && q >= n2)
|
||||
throw std::runtime_error("fail to find enough stable eigenvalues");
|
||||
}
|
||||
} // namespace
|
||||
|
||||
/**
|
||||
* DiscreteAlgebraicRiccatiEquation function
|
||||
* computes the unique stabilizing solution X to the discrete-time algebraic
|
||||
* Riccati equation:
|
||||
* \f[
|
||||
* A'XA - X - A'XB(B'XB+R)^{-1}B'XA + Q = 0
|
||||
* \f]
|
||||
*
|
||||
* @throws std::runtime_error if Q is not positive semi-definite.
|
||||
* @throws std::runtime_error if R is not positive definite.
|
||||
*
|
||||
* Based on the Schur Vector approach outlined in this paper:
|
||||
* "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
|
||||
* by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell, in TAC, 1980,
|
||||
* http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1102434
|
||||
*
|
||||
* Note: When, for example, n = 100, m = 80, and entries of A, B, Q_half,
|
||||
* R_half are sampled from standard normal distributions, where
|
||||
* Q = Q_half'*Q_half and similar for R, the absolute error of the solution
|
||||
* is 10^{-6}, while the absolute error of the solution computed by Matlab is
|
||||
* 10^{-8}.
|
||||
*
|
||||
* TODO(weiqiao.han): I may overwrite the RealQZ function to improve the
|
||||
* accuracy, together with more thorough tests.
|
||||
*/
|
||||
|
||||
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& B,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& Q,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& R) {
|
||||
int n = B.rows(), m = B.cols();
|
||||
|
||||
DRAKE_DEMAND(m <= n);
|
||||
DRAKE_DEMAND(A.rows() == n && A.cols() == n);
|
||||
DRAKE_DEMAND(Q.rows() == n && Q.cols() == n);
|
||||
DRAKE_DEMAND(R.rows() == m && R.cols() == m);
|
||||
DRAKE_DEMAND(is_approx_equal_abstol(Q, Q.transpose(), 1e-10));
|
||||
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(Q);
|
||||
for (int i = 0; i < n; i++) {
|
||||
DRAKE_THROW_UNLESS(es.eigenvalues()[i] >= 0);
|
||||
}
|
||||
DRAKE_DEMAND(is_approx_equal_abstol(R, R.transpose(), 1e-10));
|
||||
Eigen::LLT<Eigen::MatrixXd> R_cholesky(R);
|
||||
DRAKE_THROW_UNLESS(R_cholesky.info() == Eigen::Success);
|
||||
check_stabilizable(A, B);
|
||||
check_detectable(A, Q);
|
||||
|
||||
Eigen::MatrixXd M(2 * n, 2 * n), L(2 * n, 2 * n);
|
||||
M << A, Eigen::MatrixXd::Zero(n, n), -Q, Eigen::MatrixXd::Identity(n, n);
|
||||
L << Eigen::MatrixXd::Identity(n, n), B * R.inverse() * B.transpose(),
|
||||
Eigen::MatrixXd::Zero(n, n), A.transpose();
|
||||
|
||||
// QZ decomposition of M and L
|
||||
// QMZ = S, QLZ = T
|
||||
// where Q and Z are real orthogonal matrixes
|
||||
// T is upper-triangular matrix, and S is upper quasi-triangular matrix
|
||||
Eigen::RealQZ<Eigen::MatrixXd> qz(2 * n);
|
||||
qz.compute(M, L); // M = Q S Z, L = Q T Z (Q and Z computed by Eigen package
|
||||
// are adjoints of Q and Z above)
|
||||
Eigen::MatrixXd S = qz.matrixS(), T = qz.matrixT(),
|
||||
Z = qz.matrixZ().adjoint();
|
||||
|
||||
// Reorder the generalized eigenvalues of (S,T).
|
||||
Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(2 * n, 2 * n);
|
||||
reorder_eigen(S, T, Z2);
|
||||
Z = (Z * Z2).eval();
|
||||
|
||||
// The first n columns of Z is ( U1 ) .
|
||||
// ( U2 )
|
||||
// -1
|
||||
// X = U2 * U1 is a solution of the discrete time Riccati equation.
|
||||
Eigen::MatrixXd U1 = Z.block(0, 0, n, n), U2 = Z.block(n, 0, n, n);
|
||||
Eigen::MatrixXd X = U2 * U1.inverse();
|
||||
X = (X + X.adjoint().eval()) / 2.0;
|
||||
return X;
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
} // namespace drake
|
||||
@@ -1,62 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <jni.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <unsupported/Eigen/MatrixFunctions>
|
||||
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
#include "edu_wpi_first_wpiutil_math_DrakeJNI.h"
|
||||
#include "wpi/jni_util.h"
|
||||
|
||||
using namespace wpi::java;
|
||||
|
||||
extern "C" {
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_wpiutil_math_DrakeJNI
|
||||
* Method: discreteAlgebraicRiccatiEquation
|
||||
* Signature: ([D[D[D[DII[D)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_wpiutil_math_DrakeJNI_discreteAlgebraicRiccatiEquation
|
||||
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
|
||||
jdoubleArray R, jint states, jint inputs, jdoubleArray S)
|
||||
{
|
||||
jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
|
||||
jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
|
||||
jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
|
||||
jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);
|
||||
|
||||
Eigen::Map<
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Amat{nativeA, states, states};
|
||||
Eigen::Map<
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Bmat{nativeB, states, inputs};
|
||||
Eigen::Map<
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Qmat{nativeQ, states, states};
|
||||
Eigen::Map<
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Rmat{nativeR, inputs, inputs};
|
||||
|
||||
Eigen::MatrixXd result =
|
||||
drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat);
|
||||
|
||||
env->ReleaseDoubleArrayElements(A, nativeA, 0);
|
||||
env->ReleaseDoubleArrayElements(B, nativeB, 0);
|
||||
env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
|
||||
env->ReleaseDoubleArrayElements(R, nativeR, 0);
|
||||
|
||||
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
|
||||
}
|
||||
|
||||
} // extern "C"
|
||||
Reference in New Issue
Block a user