mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-26 01:51:41 +00:00
[wpimath] Rewrite DARE solver (#5328)
I timed the DARE unit tests, and the new solver is 0 to 100% faster in all cases (that is, it's at least as fast as Drake's and up to 2x faster in some cases). The new solver is also much simpler, takes less time to compile, and drops the libwpimath.so size from 325 MB to 301 MB. I think most of the compilation time is coming from the eigenvalue decompositions used to enforce argument preconditions.
This commit is contained in:
Tyler Veness
265
wpimath/src/main/native/cpp/DARE.cpp
Normal file
265
wpimath/src/main/native/cpp/DARE.cpp
Normal file
@@ -0,0 +1,265 @@
|
||||
// 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.
|
||||
|
||||
#include "frc/DARE.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include "Eigen/Cholesky"
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Eigenvalues"
|
||||
#include "Eigen/QR"
|
||||
#include "frc/fmt/Eigen.h"
|
||||
|
||||
// Works cited:
|
||||
//
|
||||
// [1] E. K.-W. Chu, H.-Y. Fan, W.-W. Lin & C.-S. Wang "Structure-Preserving
|
||||
// Algorithms for Periodic Discrete-Time Algebraic Riccati Equations",
|
||||
// International Journal of Control, 77:8, 767-788, 2004.
|
||||
// DOI: 10.1080/00207170410001714988
|
||||
|
||||
namespace frc {
|
||||
|
||||
namespace {
|
||||
|
||||
/**
|
||||
* Returns true 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([λI - A, B]) < n where n is the number of states.
|
||||
*
|
||||
* @param A System matrix.
|
||||
* @param B Input matrix.
|
||||
*/
|
||||
bool IsStabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& B) {
|
||||
Eigen::EigenSolver<Eigen::MatrixXd> es{A};
|
||||
|
||||
for (int i = 0; i < A.rows(); ++i) {
|
||||
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
|
||||
es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
|
||||
1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
|
||||
E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
|
||||
A,
|
||||
B;
|
||||
|
||||
Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
|
||||
if (qr.rank() < A.rows()) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if (A, C) is a detectable pair.
|
||||
*
|
||||
* (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([λI - A; C]) < n where n is the number of states.
|
||||
*
|
||||
* @param A System matrix.
|
||||
* @param C Output matrix.
|
||||
*/
|
||||
bool IsDetectable(const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& C) {
|
||||
return IsStabilizable(A.transpose(), C.transpose());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if all the matrix's eigenvalues are greater than or equal to
|
||||
* zero.
|
||||
*
|
||||
* @param A The matrix.
|
||||
*/
|
||||
bool IsPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& A) {
|
||||
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es{A};
|
||||
for (int i = 0; i < A.rows(); ++i) {
|
||||
if (es.eigenvalues()[i] < 0) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if all the matrix's eigenvalues are greater than zero.
|
||||
*
|
||||
* @param A The matrix.
|
||||
*/
|
||||
bool IsPositiveDefinite(const Eigen::Ref<const Eigen::MatrixXd>& A) {
|
||||
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es{A};
|
||||
for (int i = 0; i < A.rows(); ++i) {
|
||||
if (es.eigenvalues()[i] <= 0) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
Eigen::MatrixXd DARE(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) {
|
||||
// These are unused if assertions aren't compiled in
|
||||
[[maybe_unused]] int states = A.rows();
|
||||
[[maybe_unused]] int inputs = B.cols();
|
||||
|
||||
// Check argument dimensions
|
||||
assert(A.rows() == states && A.cols() == states);
|
||||
assert(B.rows() == states && B.cols() == inputs);
|
||||
assert(Q.rows() == states && Q.cols() == states);
|
||||
assert(R.rows() == inputs && R.cols() == inputs);
|
||||
|
||||
// Require the number of inputs be less than or equal to the number of states
|
||||
if (inputs > states) {
|
||||
std::string msg = fmt::format(
|
||||
"Number of inputs ({}) is greater than number of states ({})!", inputs,
|
||||
states);
|
||||
throw std::invalid_argument(msg);
|
||||
}
|
||||
|
||||
// Require Q be symmetric
|
||||
if ((Q - Q.transpose()).norm() > 1e-10) {
|
||||
std::string msg =
|
||||
fmt::format("Q isn't symmetric!\n\nQ =\n{}\n", Eigen::MatrixXd{Q});
|
||||
throw std::invalid_argument(msg);
|
||||
}
|
||||
|
||||
// Require Q be positive semidefinite
|
||||
if (!IsPositiveSemidefinite(Q)) {
|
||||
std::string msg = fmt::format("Q isn't positive semidefinite!\n\nQ =\n{}\n",
|
||||
Eigen::MatrixXd{Q});
|
||||
throw std::invalid_argument(msg);
|
||||
}
|
||||
|
||||
// Require R be symmetric
|
||||
if ((R - R.transpose()).norm() > 1e-10) {
|
||||
std::string msg =
|
||||
fmt::format("R isn't symmetric!\n\nR =\n{}\n", Eigen::MatrixXd{R});
|
||||
throw std::invalid_argument(msg);
|
||||
}
|
||||
|
||||
// Require R be positive definite
|
||||
if (!IsPositiveDefinite(R)) {
|
||||
std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n",
|
||||
Eigen::MatrixXd{R});
|
||||
throw std::invalid_argument(msg);
|
||||
}
|
||||
|
||||
// Require (A, B) pair be stabilizable
|
||||
if (!IsStabilizable(A, B)) {
|
||||
std::string msg =
|
||||
fmt::format("The (A, B) pair isn't stabilizable!\n\nA =\n{}\nB =\n{}\n",
|
||||
Eigen::MatrixXd{A}, Eigen::MatrixXd{B});
|
||||
throw std::invalid_argument(msg);
|
||||
}
|
||||
|
||||
// Require (A, C) pair be detectable where Q = CᵀC
|
||||
{
|
||||
Eigen::LDLT<Eigen::MatrixXd> ldlt{Q};
|
||||
Eigen::MatrixXd C = Eigen::MatrixXd{ldlt.matrixL()} *
|
||||
ldlt.vectorD().cwiseSqrt().asDiagonal();
|
||||
|
||||
if (!IsDetectable(A, C)) {
|
||||
std::string msg = fmt::format(
|
||||
"The (A, C) pair where Q = CᵀC isn't detectable!\n\nA =\n{}\nQ "
|
||||
"=\n{}\n",
|
||||
Eigen::MatrixXd{A}, Eigen::MatrixXd{Q});
|
||||
throw std::invalid_argument(msg);
|
||||
}
|
||||
}
|
||||
|
||||
// Implements the SSCA algorithm on page 12 of [1].
|
||||
|
||||
// A₀ = A
|
||||
Eigen::MatrixXd A_k = A;
|
||||
Eigen::MatrixXd A_k1 = A;
|
||||
|
||||
// G₀ = BR⁻¹Bᵀ
|
||||
//
|
||||
// See equation (4) of [1].
|
||||
Eigen::MatrixXd G_k = B * R.llt().solve(B.transpose());
|
||||
|
||||
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(A.rows(), A.cols());
|
||||
|
||||
// H₀ = Q
|
||||
//
|
||||
// See equation (4) of [1].
|
||||
Eigen::MatrixXd H_k = Q;
|
||||
Eigen::MatrixXd H_k1 = Q;
|
||||
|
||||
do {
|
||||
A_k = A_k1;
|
||||
H_k = H_k1;
|
||||
|
||||
// W = I + HₖGₖ
|
||||
Eigen::MatrixXd W = I + H_k * G_k;
|
||||
|
||||
// W is symmetric positive definite, so the LLT decomposition would work
|
||||
// here and is faster than the householder QR decomposition [2]. However,
|
||||
// it's not accurate enough. Experimentation showed that so many iterations
|
||||
// of iterative refinement [3] were required to fix the accuracy that the
|
||||
// total system solve time was much higher than householder QR.
|
||||
//
|
||||
// [2] https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html
|
||||
// [3] https://en.wikipedia.org/wiki/Iterative_refinement
|
||||
auto W_solver = W.householderQr();
|
||||
|
||||
// Solve WV₁ = Aₖᵀ for V₁
|
||||
Eigen::MatrixXd V_1 = W_solver.solve(A_k.transpose());
|
||||
|
||||
// Solve WV₂ = Hₖ for V₂
|
||||
Eigen::MatrixXd V_2 = W_solver.solve(H_k);
|
||||
|
||||
// Aₖ₊₁ = V₁ᵀAₖ
|
||||
A_k1 = V_1.transpose() * A_k;
|
||||
|
||||
// Gₖ₊₁ = Gₖ + AₖGₖV₁
|
||||
G_k += A_k * G_k * V_1;
|
||||
|
||||
// Hₖ₊₁ = Hₖ + AₖᵀV₂Aₖ
|
||||
H_k1 = H_k + A_k.transpose() * V_2 * A_k;
|
||||
|
||||
// while |Hₖ₊₁ − Hₖ| > ε |Hₖ₊₁|
|
||||
} while ((H_k1 - H_k).norm() > 1e-10 * H_k1.norm());
|
||||
|
||||
return H_k1;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd DARE(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,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& N) {
|
||||
// Check argument dimensions
|
||||
assert(N.rows() == B.rows() && N.cols() == B.cols());
|
||||
|
||||
// This is a change of variables to make the DARE that includes Q, R, and N
|
||||
// cost matrices fit the form of the DARE that includes only Q and R cost
|
||||
// matrices.
|
||||
//
|
||||
// This is equivalent to solving the original DARE:
|
||||
//
|
||||
// A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
|
||||
//
|
||||
// where A₂ and Q₂ are a change of variables:
|
||||
//
|
||||
// A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
|
||||
return DARE(A - B * R.llt().solve(N.transpose()), B,
|
||||
Q - N * R.llt().solve(N.transpose()), R);
|
||||
}
|
||||
|
||||
} // namespace frc
|
||||
@@ -12,47 +12,51 @@
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Eigenvalues"
|
||||
#include "Eigen/QR"
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
#include "edu_wpi_first_math_WPIMathJNI.h"
|
||||
#include "frc/DARE.h"
|
||||
#include "frc/trajectory/TrajectoryUtil.h"
|
||||
#include "unsupported/Eigen/MatrixFunctions"
|
||||
|
||||
using namespace wpi::java;
|
||||
|
||||
namespace {
|
||||
|
||||
/**
|
||||
* Returns true 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(λI - A, B) < n where n is the number of states.
|
||||
* uncontrollable if rank([λI - A, B]) < n where n is the number of states.
|
||||
*
|
||||
* @param A System matrix.
|
||||
* @param B Input matrix.
|
||||
*/
|
||||
bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& B) {
|
||||
int states = B.rows();
|
||||
int inputs = B.cols();
|
||||
bool IsStabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& B) {
|
||||
Eigen::EigenSolver<Eigen::MatrixXd> es{A};
|
||||
for (int i = 0; i < states; ++i) {
|
||||
|
||||
for (int i = 0; i < A.rows(); ++i) {
|
||||
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
|
||||
es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
|
||||
1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
Eigen::MatrixXcd E{states, states + inputs};
|
||||
E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(states, states) - A,
|
||||
Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
|
||||
E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
|
||||
A,
|
||||
B;
|
||||
|
||||
Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
|
||||
if (qr.rank() < states) {
|
||||
if (qr.rank() < A.rows()) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
std::vector<double> GetElementsFromTrajectory(
|
||||
const frc::Trajectory& trajectory) {
|
||||
std::vector<double> elements;
|
||||
@@ -97,11 +101,11 @@ extern "C" {
|
||||
|
||||
/*
|
||||
* Class: edu_wpi_first_math_WPIMathJNI
|
||||
* Method: discreteAlgebraicRiccatiEquation
|
||||
* Method: dare
|
||||
* Signature: ([D[D[D[DII[D)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_edu_wpi_first_math_WPIMathJNI_discreteAlgebraicRiccatiEquation
|
||||
Java_edu_wpi_first_math_WPIMathJNI_dare
|
||||
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
|
||||
jdoubleArray R, jint states, jint inputs, jdoubleArray S)
|
||||
{
|
||||
@@ -124,8 +128,7 @@ Java_edu_wpi_first_math_WPIMathJNI_discreteAlgebraicRiccatiEquation
|
||||
Rmat{nativeR, inputs, inputs};
|
||||
|
||||
try {
|
||||
Eigen::MatrixXd result =
|
||||
drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat);
|
||||
Eigen::MatrixXd result = frc::DARE(Amat, Bmat, Qmat, Rmat);
|
||||
|
||||
env->ReleaseDoubleArrayElements(A, nativeA, 0);
|
||||
env->ReleaseDoubleArrayElements(B, nativeB, 0);
|
||||
@@ -205,7 +208,7 @@ Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
B{nativeB, states, inputs};
|
||||
|
||||
bool isStabilizable = check_stabilizable(A, B);
|
||||
bool isStabilizable = IsStabilizable(A, B);
|
||||
|
||||
env->ReleaseDoubleArrayElements(aSrc, nativeA, 0);
|
||||
env->ReleaseDoubleArrayElements(bSrc, nativeB, 0);
|
||||
|
||||
91
wpimath/src/main/native/include/frc/DARE.h
Normal file
91
wpimath/src/main/native/include/frc/DARE.h
Normal file
@@ -0,0 +1,91 @@
|
||||
// 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/SymbolExports.h>
|
||||
|
||||
#include "Eigen/Core"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
*
|
||||
* Computes the unique stabilizing solution X to the discrete-time algebraic
|
||||
* Riccati equation:
|
||||
*
|
||||
* AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
*
|
||||
* @param A The system matrix.
|
||||
* @param B The input matrix.
|
||||
* @param Q The state cost matrix.
|
||||
* @param R The input cost matrix.
|
||||
* @throws std::invalid_argument if number of inputs is greater than number of
|
||||
* states.
|
||||
* @throws std::invalid_argument if Q isn't symmetric positive semidefinite.
|
||||
* @throws std::invalid_argument if R isn't symmetric positive definite.
|
||||
* @throws std::invalid_argument if the (A, B) pair isn't stabilizable.
|
||||
* @throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't
|
||||
* detectable.
|
||||
*/
|
||||
WPILIB_DLLEXPORT
|
||||
Eigen::MatrixXd DARE(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);
|
||||
|
||||
/**
|
||||
Computes the unique stabilizing solution X to the discrete-time algebraic
|
||||
Riccati equation:
|
||||
|
||||
AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
|
||||
This overload of the DARE is useful for finding the control law uₖ that
|
||||
minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
|
||||
|
||||
@verbatim
|
||||
∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
k=0
|
||||
@endverbatim
|
||||
|
||||
This is a more general form of the following. The linear-quadratic regulator
|
||||
is the feedback control law uₖ that minimizes the following cost function
|
||||
subject to xₖ₊₁ = Axₖ + Buₖ:
|
||||
|
||||
@verbatim
|
||||
∞
|
||||
J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
k=0
|
||||
@endverbatim
|
||||
|
||||
This can be refactored as:
|
||||
|
||||
@verbatim
|
||||
∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
k=0
|
||||
@endverbatim
|
||||
|
||||
@param A The system matrix.
|
||||
@param B The input matrix.
|
||||
@param Q The state cost matrix.
|
||||
@param R The input cost matrix.
|
||||
@param N The state-input cross cost matrix.
|
||||
@throws std::invalid_argument if number of inputs is greater than number of
|
||||
states.
|
||||
@throws std::invalid_argument if Q − NR⁻¹Nᵀ isn't symmetric positive
|
||||
semidefinite.
|
||||
@throws std::invalid_argument if R isn't symmetric positive definite.
|
||||
@throws std::invalid_argument if the (A, B) pair isn't stabilizable.
|
||||
@throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't detectable.
|
||||
*/
|
||||
WPILIB_DLLEXPORT
|
||||
Eigen::MatrixXd DARE(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,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& N);
|
||||
|
||||
} // namespace frc
|
||||
@@ -8,8 +8,7 @@
|
||||
#include <string>
|
||||
|
||||
#include "Eigen/Cholesky"
|
||||
#include "Eigen/Eigenvalues"
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
#include "frc/DARE.h"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/controller/LinearQuadraticRegulator.h"
|
||||
#include "frc/fmt/Eigen.h"
|
||||
@@ -52,8 +51,7 @@ LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
|
||||
throw std::invalid_argument(msg);
|
||||
}
|
||||
|
||||
Matrixd<States, States> S =
|
||||
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
|
||||
Matrixd<States, States> S = DARE(discA, discB, Q, R);
|
||||
|
||||
// K = (BᵀSB + R)⁻¹BᵀSA
|
||||
m_K = (discB.transpose() * S * discB + R)
|
||||
@@ -72,8 +70,7 @@ LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
|
||||
Matrixd<States, Inputs> discB;
|
||||
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
|
||||
|
||||
Matrixd<States, States> S =
|
||||
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
|
||||
Matrixd<States, States> S = DARE(discA, discB, Q, R, N);
|
||||
|
||||
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
|
||||
m_K = (discB.transpose() * S * discB + R)
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
#pragma once
|
||||
|
||||
#include "Eigen/Cholesky"
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
#include "frc/DARE.h"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/estimator/ExtendedKalmanFilter.h"
|
||||
#include "frc/system/Discretization.h"
|
||||
@@ -41,8 +41,7 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
|
||||
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
|
||||
|
||||
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
|
||||
m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
|
||||
discA.transpose(), C.transpose(), discQ, discR);
|
||||
m_initP = DARE(discA.transpose(), C.transpose(), discQ, discR);
|
||||
} else {
|
||||
m_initP = StateMatrix::Zero();
|
||||
}
|
||||
@@ -77,8 +76,7 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
|
||||
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
|
||||
|
||||
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
|
||||
m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
|
||||
discA.transpose(), C.transpose(), discQ, discR);
|
||||
m_initP = DARE(discA.transpose(), C.transpose(), discQ, discR);
|
||||
} else {
|
||||
m_initP = StateMatrix::Zero();
|
||||
}
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
#include <string>
|
||||
|
||||
#include "Eigen/Cholesky"
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
#include "frc/DARE.h"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/estimator/KalmanFilter.h"
|
||||
#include "frc/system/Discretization.h"
|
||||
@@ -47,8 +47,8 @@ KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
|
||||
throw std::invalid_argument(msg);
|
||||
}
|
||||
|
||||
Matrixd<States, States> P = drake::math::DiscreteAlgebraicRiccatiEquation(
|
||||
discA.transpose(), C.transpose(), discQ, discR);
|
||||
Matrixd<States, States> P =
|
||||
DARE(discA.transpose(), C.transpose(), discQ, discR);
|
||||
|
||||
// S = CPCᵀ + R
|
||||
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
|
||||
|
||||
@@ -1,162 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <type_traits>
|
||||
|
||||
/// @file
|
||||
/// Provides Drake's assertion implementation. This is intended to be used
|
||||
/// both within Drake and by other software. Drake's asserts can be armed
|
||||
/// and disarmed independently from the system-wide asserts.
|
||||
|
||||
#ifdef DRAKE_DOXYGEN_CXX
|
||||
/// @p DRAKE_ASSERT(condition) is similar to the built-in @p assert(condition)
|
||||
/// from the C++ system header @p <cassert>. Unless Drake's assertions are
|
||||
/// disarmed by the pre-processor definitions listed below, @p DRAKE_ASSERT
|
||||
/// will evaluate @p condition and iff the value is false will trigger an
|
||||
/// assertion failure with a message showing at least the condition text,
|
||||
/// function name, file, and line.
|
||||
///
|
||||
/// By default, assertion failures will :abort() the program. However, when
|
||||
/// using the pydrake python bindings, assertion failures will instead throw a
|
||||
/// C++ exception that causes a python SystemExit exception.
|
||||
///
|
||||
/// Assertions are enabled or disabled using the following pre-processor macros:
|
||||
///
|
||||
/// - If @p DRAKE_ENABLE_ASSERTS is defined, then @p DRAKE_ASSERT is armed.
|
||||
/// - If @p DRAKE_DISABLE_ASSERTS is defined, then @p DRAKE_ASSERT is disarmed.
|
||||
/// - If both macros are defined, then it is a compile-time error.
|
||||
/// - If neither are defined, then NDEBUG governs assertions as usual.
|
||||
///
|
||||
/// This header will define exactly one of either @p DRAKE_ASSERT_IS_ARMED or
|
||||
/// @p DRAKE_ASSERT_IS_DISARMED to indicate whether @p DRAKE_ASSERT is armed.
|
||||
///
|
||||
/// This header will define both `constexpr bool drake::kDrakeAssertIsArmed`
|
||||
/// and `constexpr bool drake::kDrakeAssertIsDisarmed` globals.
|
||||
///
|
||||
/// One difference versus the standard @p assert(condition) is that the
|
||||
/// @p condition within @p DRAKE_ASSERT is always syntax-checked, even if
|
||||
/// Drake's assertions are disarmed.
|
||||
///
|
||||
/// Treat @p DRAKE_ASSERT like a statement -- it must always be used
|
||||
/// in block scope, and must always be followed by a semicolon.
|
||||
#define DRAKE_ASSERT(condition)
|
||||
/// Like @p DRAKE_ASSERT, except that the expression must be void-valued; this
|
||||
/// allows for guarding expensive assertion-checking subroutines using the same
|
||||
/// macros as stand-alone assertions.
|
||||
#define DRAKE_ASSERT_VOID(expression)
|
||||
/// Evaluates @p condition and iff the value is false will trigger an assertion
|
||||
/// failure with a message showing at least the condition text, function name,
|
||||
/// file, and line.
|
||||
#define DRAKE_DEMAND(condition)
|
||||
/// Silences a "no return value" compiler warning by calling a function that
|
||||
/// always raises an exception or aborts (i.e., a function marked noreturn).
|
||||
/// Only use this macro at a point where (1) a point in the code is truly
|
||||
/// unreachable, (2) the fact that it's unreachable is knowable from only
|
||||
/// reading the function itself (and not, e.g., some larger design invariant),
|
||||
/// and (3) there is a compiler warning if this macro were removed. The most
|
||||
/// common valid use is with a switch-case-return block where all cases are
|
||||
/// accounted for but the enclosing function is supposed to return a value. Do
|
||||
/// *not* use this macro as a "logic error" assertion; it should *only* be used
|
||||
/// to silence false positive warnings. When in doubt, throw an exception
|
||||
/// manually instead of using this macro.
|
||||
#define DRAKE_UNREACHABLE()
|
||||
#else // DRAKE_DOXYGEN_CXX
|
||||
|
||||
// Users should NOT set these; only this header should set them.
|
||||
#ifdef DRAKE_ASSERT_IS_ARMED
|
||||
# error Unexpected DRAKE_ASSERT_IS_ARMED defined.
|
||||
#endif
|
||||
#ifdef DRAKE_ASSERT_IS_DISARMED
|
||||
# error Unexpected DRAKE_ASSERT_IS_DISARMED defined.
|
||||
#endif
|
||||
|
||||
// Decide whether Drake assertions are enabled.
|
||||
#if defined(DRAKE_ENABLE_ASSERTS) && defined(DRAKE_DISABLE_ASSERTS)
|
||||
# error Conflicting assertion toggles.
|
||||
#elif defined(DRAKE_ENABLE_ASSERTS)
|
||||
# define DRAKE_ASSERT_IS_ARMED
|
||||
#elif defined(DRAKE_DISABLE_ASSERTS) || defined(NDEBUG)
|
||||
# define DRAKE_ASSERT_IS_DISARMED
|
||||
#else
|
||||
# define DRAKE_ASSERT_IS_ARMED
|
||||
#endif
|
||||
|
||||
namespace drake {
|
||||
namespace internal {
|
||||
// Abort the program with an error message.
|
||||
[[noreturn]] void Abort(const char* condition, const char* func,
|
||||
const char* file, int line);
|
||||
// Report an assertion failure; will either Abort(...) or throw.
|
||||
[[noreturn]] void AssertionFailed(const char* condition, const char* func,
|
||||
const char* file, int line);
|
||||
} // namespace internal
|
||||
namespace assert {
|
||||
// Allows for specialization of how to bool-convert Conditions used in
|
||||
// assertions, in case they are not intrinsically convertible. See
|
||||
// common/symbolic/expression/formula.h for an example use. This is a public
|
||||
// interface to extend; it is intended to be specialized by unusual Scalar
|
||||
// types that require special handling.
|
||||
template <typename Condition>
|
||||
struct ConditionTraits {
|
||||
static constexpr bool is_valid = std::is_convertible_v<Condition, bool>;
|
||||
static bool Evaluate(const Condition& value) {
|
||||
return value;
|
||||
}
|
||||
};
|
||||
} // namespace assert
|
||||
} // namespace drake
|
||||
|
||||
#define DRAKE_UNREACHABLE() \
|
||||
::drake::internal::Abort( \
|
||||
"Unreachable code was reached?!", __func__, __FILE__, __LINE__)
|
||||
|
||||
#define DRAKE_DEMAND(condition) \
|
||||
do { \
|
||||
typedef ::drake::assert::ConditionTraits< \
|
||||
typename std::remove_cv_t<decltype(condition)>> Trait; \
|
||||
static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
|
||||
static_assert( \
|
||||
!std::is_pointer_v<decltype(condition)>, \
|
||||
"When using DRAKE_DEMAND on a raw pointer, always write out " \
|
||||
"DRAKE_DEMAND(foo != nullptr), do not write DRAKE_DEMAND(foo) " \
|
||||
"and rely on implicit pointer-to-bool conversion."); \
|
||||
if (!Trait::Evaluate(condition)) { \
|
||||
::drake::internal::AssertionFailed( \
|
||||
#condition, __func__, __FILE__, __LINE__); \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
#ifdef DRAKE_ASSERT_IS_ARMED
|
||||
// Assertions are enabled.
|
||||
namespace drake {
|
||||
constexpr bool kDrakeAssertIsArmed = true;
|
||||
constexpr bool kDrakeAssertIsDisarmed = false;
|
||||
} // namespace drake
|
||||
# define DRAKE_ASSERT(condition) DRAKE_DEMAND(condition)
|
||||
# define DRAKE_ASSERT_VOID(expression) do { \
|
||||
static_assert( \
|
||||
std::is_convertible_v<decltype(expression), void>, \
|
||||
"Expression should be void."); \
|
||||
expression; \
|
||||
} while (0)
|
||||
#else
|
||||
// Assertions are disabled, so just typecheck the expression.
|
||||
namespace drake {
|
||||
constexpr bool kDrakeAssertIsArmed = false;
|
||||
constexpr bool kDrakeAssertIsDisarmed = true;
|
||||
} // namespace drake
|
||||
# define DRAKE_ASSERT(condition) do { \
|
||||
typedef ::drake::assert::ConditionTraits< \
|
||||
typename std::remove_cv_t<decltype(condition)>> Trait; \
|
||||
static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
|
||||
static_assert( \
|
||||
!std::is_pointer_v<decltype(condition)>, \
|
||||
"When using DRAKE_ASSERT on a raw pointer, always write out " \
|
||||
"DRAKE_ASSERT(foo != nullptr), do not write DRAKE_ASSERT(foo) " \
|
||||
"and rely on implicit pointer-to-bool conversion."); \
|
||||
} while (0)
|
||||
# define DRAKE_ASSERT_VOID(expression) static_assert( \
|
||||
std::is_convertible_v<decltype(expression), void>, \
|
||||
"Expression should be void.")
|
||||
#endif
|
||||
|
||||
#endif // DRAKE_DOXYGEN_CXX
|
||||
@@ -1,18 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
namespace drake {
|
||||
namespace internal {
|
||||
|
||||
// This is what DRAKE_ASSERT and DRAKE_DEMAND throw when our assertions are
|
||||
// configured to throw.
|
||||
class assertion_error : public std::runtime_error {
|
||||
public:
|
||||
explicit assertion_error(const std::string& what_arg)
|
||||
: std::runtime_error(what_arg) {}
|
||||
};
|
||||
|
||||
} // namespace internal
|
||||
} // namespace drake
|
||||
@@ -1,90 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
// ============================================================================
|
||||
// N.B. The spelling of the macro names between doc/Doxyfile_CXX.in and this
|
||||
// file must be kept in sync!
|
||||
// ============================================================================
|
||||
|
||||
/** @file
|
||||
Provides careful macros to selectively enable or disable the special member
|
||||
functions for copy-construction, copy-assignment, move-construction, and
|
||||
move-assignment.
|
||||
|
||||
http://en.cppreference.com/w/cpp/language/member_functions#Special_member_functions
|
||||
|
||||
When enabled via these macros, the `= default` implementation is provided.
|
||||
Code that needs custom copy or move functions should not use these macros.
|
||||
*/
|
||||
|
||||
/** DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for
|
||||
copy-construction, copy-assignment, move-construction, and move-assignment.
|
||||
Drake's Doxygen is customized to render the deletions in detail, with
|
||||
appropriate comments. Invoke this macro in the public section of the class
|
||||
declaration, e.g.:
|
||||
<pre>
|
||||
class Foo {
|
||||
public:
|
||||
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Foo)
|
||||
|
||||
// ...
|
||||
};
|
||||
</pre>
|
||||
*/
|
||||
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname) \
|
||||
Classname(const Classname&) = delete; \
|
||||
void operator=(const Classname&) = delete; \
|
||||
Classname(Classname&&) = delete; \
|
||||
void operator=(Classname&&) = delete;
|
||||
|
||||
/** DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member
|
||||
functions for copy-construction, copy-assignment, move-construction, and
|
||||
move-assignment. This macro should be used only when copy-construction and
|
||||
copy-assignment defaults are well-formed. Note that the defaulted move
|
||||
functions could conceivably still be ill-formed, in which case they will
|
||||
effectively not be declared or used -- but because the copy constructor exists
|
||||
the type will still be MoveConstructible. Drake's Doxygen is customized to
|
||||
render the functions in detail, with appropriate comments. Typically, you
|
||||
should invoke this macro in the public section of the class declaration, e.g.:
|
||||
<pre>
|
||||
class Foo {
|
||||
public:
|
||||
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Foo)
|
||||
|
||||
// ...
|
||||
};
|
||||
</pre>
|
||||
|
||||
However, if Foo has a virtual destructor (i.e., is subclassable), then
|
||||
typically you should use either DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN in the
|
||||
public section or else DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN in the
|
||||
protected section, to prevent
|
||||
<a href="https://en.wikipedia.org/wiki/Object_slicing">object slicing</a>.
|
||||
|
||||
The macro contains a built-in self-check that copy-assignment is well-formed.
|
||||
This self-check proves that the Classname is CopyConstructible, CopyAssignable,
|
||||
MoveConstructible, and MoveAssignable (in all but the most arcane cases where a
|
||||
member of the Classname is somehow CopyAssignable but not CopyConstructible).
|
||||
Therefore, classes that use this macro typically will not need to have any unit
|
||||
tests that check for the presence nor correctness of these functions.
|
||||
|
||||
However, the self-check does not provide any checks of the runtime efficiency
|
||||
of the functions. If it is important for performance that the move functions
|
||||
actually move (instead of making a copy), then you should consider capturing
|
||||
that in a unit test.
|
||||
*/
|
||||
#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname) \
|
||||
Classname(const Classname&) = default; \
|
||||
Classname& operator=(const Classname&) = default; \
|
||||
Classname(Classname&&) = default; \
|
||||
Classname& operator=(Classname&&) = default; \
|
||||
/* Fails at compile-time if copy-assign doesn't compile. */ \
|
||||
/* Note that we do not test the copy-ctor here, because */ \
|
||||
/* it will not exist when Classname is abstract. */ \
|
||||
static void DrakeDefaultCopyAndMoveAndAssign_DoAssign( \
|
||||
Classname* a, const Classname& b) { *a = b; } \
|
||||
static_assert( \
|
||||
&DrakeDefaultCopyAndMoveAndAssign_DoAssign == \
|
||||
&DrakeDefaultCopyAndMoveAndAssign_DoAssign, \
|
||||
"This assertion is never false; its only purpose is to " \
|
||||
"generate 'use of deleted function: operator=' errors " \
|
||||
"when Classname is a template.");
|
||||
@@ -1,47 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <type_traits>
|
||||
|
||||
#include "drake/common/drake_assert.h"
|
||||
|
||||
/// @file
|
||||
/// Provides a convenient wrapper to throw an exception when a condition is
|
||||
/// unmet. This is similar to an assertion, but uses exceptions instead of
|
||||
/// ::abort(), and cannot be disabled.
|
||||
|
||||
namespace drake {
|
||||
namespace internal {
|
||||
// Throw an error message.
|
||||
[[noreturn]] void Throw(const char* condition, const char* func,
|
||||
const char* file, int line);
|
||||
} // namespace internal
|
||||
} // namespace drake
|
||||
|
||||
/// Evaluates @p condition and iff the value is false will throw an exception
|
||||
/// with a message showing at least the condition text, function name, file,
|
||||
/// and line.
|
||||
///
|
||||
/// The condition must not be a pointer, where we'd implicitly rely on its
|
||||
/// nullness. Instead, always write out "!= nullptr" to be precise.
|
||||
///
|
||||
/// Correct: `DRAKE_THROW_UNLESS(foo != nullptr);`
|
||||
/// Incorrect: `DRAKE_THROW_UNLESS(foo);`
|
||||
///
|
||||
/// Because this macro is intended to provide a useful exception message to
|
||||
/// users, we should err on the side of extra detail about the failure. The
|
||||
/// meaning of "foo" isolated within error message text does not make it
|
||||
/// clear that a null pointer is the proximate cause of the problem.
|
||||
#define DRAKE_THROW_UNLESS(condition) \
|
||||
do { \
|
||||
typedef ::drake::assert::ConditionTraits< \
|
||||
typename std::remove_cv_t<decltype(condition)>> Trait; \
|
||||
static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
|
||||
static_assert( \
|
||||
!std::is_pointer_v<decltype(condition)>, \
|
||||
"When using DRAKE_THROW_UNLESS on a raw pointer, always write out " \
|
||||
"DRAKE_THROW_UNLESS(foo != nullptr), do not write DRAKE_THROW_UNLESS" \
|
||||
"(foo) and rely on implicit pointer-to-bool conversion."); \
|
||||
if (!Trait::Evaluate(condition)) { \
|
||||
::drake::internal::Throw(#condition, __func__, __FILE__, __LINE__); \
|
||||
} \
|
||||
} while (0)
|
||||
@@ -1,62 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
namespace drake {
|
||||
|
||||
/// Returns true if and only if the two matrices are equal to within a certain
|
||||
/// absolute elementwise @p tolerance. Special values (infinities, NaN, etc.)
|
||||
/// do not compare as equal elements.
|
||||
template <typename DerivedA, typename DerivedB>
|
||||
bool is_approx_equal_abstol(const Eigen::MatrixBase<DerivedA>& m1,
|
||||
const Eigen::MatrixBase<DerivedB>& m2,
|
||||
double tolerance) {
|
||||
return (
|
||||
(m1.rows() == m2.rows()) &&
|
||||
(m1.cols() == m2.cols()) &&
|
||||
((m1 - m2).template lpNorm<Eigen::Infinity>() <= tolerance));
|
||||
}
|
||||
|
||||
/// Returns true if and only if a simple greedy search reveals a permutation
|
||||
/// of the columns of m2 to make the matrix equal to m1 to within a certain
|
||||
/// absolute elementwise @p tolerance. E.g., there exists a P such that
|
||||
/// <pre>
|
||||
/// forall i,j, |m1 - m2*P|_{i,j} <= tolerance
|
||||
/// where P is a permutation matrix:
|
||||
/// P(i,j)={0,1}, sum_i P(i,j)=1, sum_j P(i,j)=1.
|
||||
/// </pre>
|
||||
/// Note: Returns false for matrices of different sizes.
|
||||
/// Note: The current implementation is O(n^2) in the number of columns.
|
||||
/// Note: In marginal cases (with similar but not identical columns) this
|
||||
/// algorithm can fail to find a permutation P even if it exists because it
|
||||
/// accepts the first column match (m1(i),m2(j)) and removes m2(j) from the
|
||||
/// pool. It is possible that other columns of m2 would also match m1(i) but
|
||||
/// that m2(j) is the only match possible for a later column of m1.
|
||||
template <typename DerivedA, typename DerivedB>
|
||||
bool IsApproxEqualAbsTolWithPermutedColumns(
|
||||
const Eigen::MatrixBase<DerivedA>& m1,
|
||||
const Eigen::MatrixBase<DerivedB>& m2, double tolerance) {
|
||||
if ((m1.cols() != m2.cols()) || (m1.rows() != m2.rows())) return false;
|
||||
|
||||
std::vector<bool> available(m2.cols());
|
||||
for (int i = 0; i < m2.cols(); i++) available[i] = true;
|
||||
|
||||
for (int i = 0; i < m1.cols(); i++) {
|
||||
bool found_match = false;
|
||||
for (int j = 0; j < m2.cols(); j++) {
|
||||
if (available[j] &&
|
||||
is_approx_equal_abstol(m1.col(i), m2.col(j), tolerance)) {
|
||||
found_match = true;
|
||||
available[j] = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found_match) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
} // namespace drake
|
||||
@@ -1,103 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <new>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
|
||||
#include "drake/common/drake_copyable.h"
|
||||
|
||||
namespace drake {
|
||||
|
||||
/// Wraps an underlying type T such that its storage is a direct member field
|
||||
/// of this object (i.e., without any indirection into the heap), but *unlike*
|
||||
/// most member fields T's destructor is never invoked.
|
||||
///
|
||||
/// This is especially useful for function-local static variables that are not
|
||||
/// trivially destructable. We shouldn't call their destructor at program exit
|
||||
/// because of the "indeterminate order of ... destruction" as mentioned in
|
||||
/// cppguide's
|
||||
/// <a href="https://drake.mit.edu/styleguide/cppguide.html#Static_and_Global_Variables">Static
|
||||
/// and Global Variables</a> section, but other solutions to this problem place
|
||||
/// the objects on the heap through an indirection.
|
||||
///
|
||||
/// Compared with other approaches, this mechanism more clearly describes the
|
||||
/// intent to readers, avoids "possible leak" warnings from memory-checking
|
||||
/// tools, and is probably slightly faster.
|
||||
///
|
||||
/// Example uses:
|
||||
///
|
||||
/// The singleton pattern:
|
||||
/// @code
|
||||
/// class Singleton {
|
||||
/// public:
|
||||
/// DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Singleton)
|
||||
/// static Singleton& getInstance() {
|
||||
/// static never_destroyed<Singleton> instance;
|
||||
/// return instance.access();
|
||||
/// }
|
||||
/// private:
|
||||
/// friend never_destroyed<Singleton>;
|
||||
/// Singleton() = default;
|
||||
/// };
|
||||
/// @endcode
|
||||
///
|
||||
/// A lookup table, created on demand the first time its needed, and then
|
||||
/// reused thereafter:
|
||||
/// @code
|
||||
/// enum class Foo { kBar, kBaz };
|
||||
/// Foo ParseFoo(const std::string& foo_string) {
|
||||
/// using Dict = std::unordered_map<std::string, Foo>;
|
||||
/// static const drake::never_destroyed<Dict> string_to_enum{
|
||||
/// std::initializer_list<Dict::value_type>{
|
||||
/// {"bar", Foo::kBar},
|
||||
/// {"baz", Foo::kBaz},
|
||||
/// }
|
||||
/// };
|
||||
/// return string_to_enum.access().at(foo_string);
|
||||
/// }
|
||||
/// @endcode
|
||||
///
|
||||
/// In cases where computing the static data is more complicated than an
|
||||
/// initializer_list, you can use a temporary lambda to populate the value:
|
||||
/// @code
|
||||
/// const std::vector<double>& GetConstantMagicNumbers() {
|
||||
/// static const drake::never_destroyed<std::vector<double>> result{[]() {
|
||||
/// std::vector<double> prototype;
|
||||
/// std::mt19937 random_generator;
|
||||
/// for (int i = 0; i < 10; ++i) {
|
||||
/// double new_value = random_generator();
|
||||
/// prototype.push_back(new_value);
|
||||
/// }
|
||||
/// return prototype;
|
||||
/// }()};
|
||||
/// return result.access();
|
||||
/// }
|
||||
/// @endcode
|
||||
///
|
||||
/// Note in particular the `()` after the lambda. That causes it to be invoked.
|
||||
//
|
||||
// The above examples are repeated in the unit test; keep them in sync.
|
||||
template <typename T>
|
||||
class never_destroyed {
|
||||
public:
|
||||
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(never_destroyed)
|
||||
|
||||
/// Passes the constructor arguments along to T using perfect forwarding.
|
||||
template <typename... Args>
|
||||
explicit never_destroyed(Args&&... args) {
|
||||
// Uses "placement new" to construct a `T` in `storage_`.
|
||||
new (&storage_) T(std::forward<Args>(args)...);
|
||||
}
|
||||
|
||||
/// Does nothing. Guaranteed!
|
||||
~never_destroyed() = default;
|
||||
|
||||
/// Returns the underlying T reference.
|
||||
T& access() { return *reinterpret_cast<T*>(&storage_); }
|
||||
const T& access() const { return *reinterpret_cast<const T*>(&storage_); }
|
||||
|
||||
private:
|
||||
typename std::aligned_storage<sizeof(T), alignof(T)>::type storage_;
|
||||
};
|
||||
|
||||
} // namespace drake
|
||||
@@ -1,85 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <wpi/SymbolExports.h>
|
||||
|
||||
namespace drake {
|
||||
namespace math {
|
||||
|
||||
/**
|
||||
Computes the unique stabilizing solution X to the discrete-time algebraic
|
||||
Riccati equation:
|
||||
|
||||
AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
|
||||
@throws std::exception if Q is not positive semi-definite.
|
||||
@throws std::exception 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
|
||||
*/
|
||||
WPILIB_DLLEXPORT
|
||||
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);
|
||||
|
||||
/**
|
||||
Computes the unique stabilizing solution X to the discrete-time algebraic
|
||||
Riccati equation:
|
||||
|
||||
AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
|
||||
This is equivalent to solving the original DARE:
|
||||
|
||||
A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
|
||||
|
||||
where A₂ and Q₂ are a change of variables:
|
||||
|
||||
A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
|
||||
|
||||
This overload of the DARE is useful for finding the control law uₖ that
|
||||
minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
|
||||
|
||||
@verbatim
|
||||
∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
k=0
|
||||
@endverbatim
|
||||
|
||||
This is a more general form of the following. The linear-quadratic regulator
|
||||
is the feedback control law uₖ that minimizes the following cost function
|
||||
subject to xₖ₊₁ = Axₖ + Buₖ:
|
||||
|
||||
@verbatim
|
||||
∞
|
||||
J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
k=0
|
||||
@endverbatim
|
||||
|
||||
This can be refactored as:
|
||||
|
||||
@verbatim
|
||||
∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
k=0
|
||||
@endverbatim
|
||||
|
||||
@throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
|
||||
@throws std::runtime_error if R is not positive definite.
|
||||
*/
|
||||
WPILIB_DLLEXPORT
|
||||
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,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& N);
|
||||
} // namespace math
|
||||
} // namespace drake
|
||||
|
||||
@@ -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,475 +0,0 @@
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
|
||||
#include <Eigen/Eigenvalues>
|
||||
#include <Eigen/QR>
|
||||
|
||||
#include "drake/common/drake_assert.h"
|
||||
#include "drake/common/drake_throw.h"
|
||||
#include "drake/common/is_approx_equal_abstol.h"
|
||||
|
||||
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:
|
||||
*
|
||||
* AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
*
|
||||
* @throws std::exception if Q is not positive semi-definite.
|
||||
* @throws std::exception 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⁻⁶, while the absolute error of the solution computed by Matlab is
|
||||
* 10⁻⁸.
|
||||
*
|
||||
* 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;
|
||||
}
|
||||
|
||||
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,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& N) {
|
||||
DRAKE_DEMAND(N.rows() == B.rows() && N.cols() == B.cols());
|
||||
|
||||
// This is a change of variables to make the DARE that includes Q, R, and N
|
||||
// cost matrices fit the form of the DARE that includes only Q and R cost
|
||||
// matrices.
|
||||
Eigen::MatrixXd A2 = A - B * R.llt().solve(N.transpose());
|
||||
Eigen::MatrixXd Q2 = Q - N * R.llt().solve(N.transpose());
|
||||
return DiscreteAlgebraicRiccatiEquation(A2, B, Q2, R);
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
} // namespace drake
|
||||
Reference in New Issue
Block a user