mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-02 02:51:42 +00:00
[wpiutil] Add Drake and Drake JNI (#2613)
Drake is a collection of tools for analyzing robot dynamics and building control systems. See https://drake.mit.edu/ for details. Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
@@ -0,0 +1,87 @@
|
||||
// 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;
|
||||
}
|
||||
@@ -0,0 +1,463 @@
|
||||
#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
|
||||
62
wpiutil/src/main/native/cpp/jni/DrakeJNI.cpp
Normal file
62
wpiutil/src/main/native/cpp/jni/DrakeJNI.cpp
Normal file
@@ -0,0 +1,62 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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"
|
||||
@@ -0,0 +1,40 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_AUTODIFF_MODULE
|
||||
#define EIGEN_AUTODIFF_MODULE
|
||||
|
||||
namespace Eigen {
|
||||
|
||||
/**
|
||||
* \defgroup AutoDiff_Module Auto Diff module
|
||||
*
|
||||
* This module features forward automatic differentation via a simple
|
||||
* templated scalar type wrapper AutoDiffScalar.
|
||||
*
|
||||
* Warning : this should NOT be confused with numerical differentiation, which
|
||||
* is a different method and has its own module in Eigen : \ref NumericalDiff_Module.
|
||||
*
|
||||
* \code
|
||||
* #include <unsupported/Eigen/AutoDiff>
|
||||
* \endcode
|
||||
*/
|
||||
//@{
|
||||
|
||||
}
|
||||
|
||||
#include "src/AutoDiff/AutoDiffScalar.h"
|
||||
// #include "src/AutoDiff/AutoDiffVector.h"
|
||||
#include "src/AutoDiff/AutoDiffJacobian.h"
|
||||
|
||||
namespace Eigen {
|
||||
//@}
|
||||
}
|
||||
|
||||
#endif // EIGEN_AUTODIFF_MODULE
|
||||
@@ -0,0 +1,108 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_AUTODIFF_JACOBIAN_H
|
||||
#define EIGEN_AUTODIFF_JACOBIAN_H
|
||||
|
||||
namespace Eigen
|
||||
{
|
||||
|
||||
template<typename Functor> class AutoDiffJacobian : public Functor
|
||||
{
|
||||
public:
|
||||
AutoDiffJacobian() : Functor() {}
|
||||
AutoDiffJacobian(const Functor& f) : Functor(f) {}
|
||||
|
||||
// forward constructors
|
||||
#if EIGEN_HAS_VARIADIC_TEMPLATES
|
||||
template<typename... T>
|
||||
AutoDiffJacobian(const T& ...Values) : Functor(Values...) {}
|
||||
#else
|
||||
template<typename T0>
|
||||
AutoDiffJacobian(const T0& a0) : Functor(a0) {}
|
||||
template<typename T0, typename T1>
|
||||
AutoDiffJacobian(const T0& a0, const T1& a1) : Functor(a0, a1) {}
|
||||
template<typename T0, typename T1, typename T2>
|
||||
AutoDiffJacobian(const T0& a0, const T1& a1, const T2& a2) : Functor(a0, a1, a2) {}
|
||||
#endif
|
||||
|
||||
typedef typename Functor::InputType InputType;
|
||||
typedef typename Functor::ValueType ValueType;
|
||||
typedef typename ValueType::Scalar Scalar;
|
||||
|
||||
enum {
|
||||
InputsAtCompileTime = InputType::RowsAtCompileTime,
|
||||
ValuesAtCompileTime = ValueType::RowsAtCompileTime
|
||||
};
|
||||
|
||||
typedef Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;
|
||||
typedef typename JacobianType::Index Index;
|
||||
|
||||
typedef Matrix<Scalar, InputsAtCompileTime, 1> DerivativeType;
|
||||
typedef AutoDiffScalar<DerivativeType> ActiveScalar;
|
||||
|
||||
typedef Matrix<ActiveScalar, InputsAtCompileTime, 1> ActiveInput;
|
||||
typedef Matrix<ActiveScalar, ValuesAtCompileTime, 1> ActiveValue;
|
||||
|
||||
#if EIGEN_HAS_VARIADIC_TEMPLATES
|
||||
// Some compilers don't accept variadic parameters after a default parameter,
|
||||
// i.e., we can't just write _jac=0 but we need to overload operator():
|
||||
EIGEN_STRONG_INLINE
|
||||
void operator() (const InputType& x, ValueType* v) const
|
||||
{
|
||||
this->operator()(x, v, 0);
|
||||
}
|
||||
template<typename... ParamsType>
|
||||
void operator() (const InputType& x, ValueType* v, JacobianType* _jac,
|
||||
const ParamsType&... Params) const
|
||||
#else
|
||||
void operator() (const InputType& x, ValueType* v, JacobianType* _jac=0) const
|
||||
#endif
|
||||
{
|
||||
eigen_assert(v!=0);
|
||||
|
||||
if (!_jac)
|
||||
{
|
||||
#if EIGEN_HAS_VARIADIC_TEMPLATES
|
||||
Functor::operator()(x, v, Params...);
|
||||
#else
|
||||
Functor::operator()(x, v);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
JacobianType& jac = *_jac;
|
||||
|
||||
ActiveInput ax = x.template cast<ActiveScalar>();
|
||||
ActiveValue av(jac.rows());
|
||||
|
||||
if(InputsAtCompileTime==Dynamic)
|
||||
for (Index j=0; j<jac.rows(); j++)
|
||||
av[j].derivatives().resize(x.rows());
|
||||
|
||||
for (Index i=0; i<jac.cols(); i++)
|
||||
ax[i].derivatives() = DerivativeType::Unit(x.rows(),i);
|
||||
|
||||
#if EIGEN_HAS_VARIADIC_TEMPLATES
|
||||
Functor::operator()(ax, &av, Params...);
|
||||
#else
|
||||
Functor::operator()(ax, &av);
|
||||
#endif
|
||||
|
||||
for (Index i=0; i<jac.rows(); i++)
|
||||
{
|
||||
(*v)[i] = av[i].value();
|
||||
jac.row(i) = av[i].derivatives();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // EIGEN_AUTODIFF_JACOBIAN_H
|
||||
@@ -0,0 +1,694 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_AUTODIFF_SCALAR_H
|
||||
#define EIGEN_AUTODIFF_SCALAR_H
|
||||
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
template<typename A, typename B>
|
||||
struct make_coherent_impl {
|
||||
static void run(A&, B&) {}
|
||||
};
|
||||
|
||||
// resize a to match b is a.size()==0, and conversely.
|
||||
template<typename A, typename B>
|
||||
void make_coherent(const A& a, const B&b)
|
||||
{
|
||||
make_coherent_impl<A,B>::run(a.const_cast_derived(), b.const_cast_derived());
|
||||
}
|
||||
|
||||
template<typename _DerType, bool Enable> struct auto_diff_special_op;
|
||||
|
||||
} // end namespace internal
|
||||
|
||||
template<typename _DerType> class AutoDiffScalar;
|
||||
|
||||
template<typename NewDerType>
|
||||
inline AutoDiffScalar<NewDerType> MakeAutoDiffScalar(const typename NewDerType::Scalar& value, const NewDerType &der) {
|
||||
return AutoDiffScalar<NewDerType>(value,der);
|
||||
}
|
||||
|
||||
/** \class AutoDiffScalar
|
||||
* \brief A scalar type replacement with automatic differentation capability
|
||||
*
|
||||
* \param _DerType the vector type used to store/represent the derivatives. The base scalar type
|
||||
* as well as the number of derivatives to compute are determined from this type.
|
||||
* Typical choices include, e.g., \c Vector4f for 4 derivatives, or \c VectorXf
|
||||
* if the number of derivatives is not known at compile time, and/or, the number
|
||||
* of derivatives is large.
|
||||
* Note that _DerType can also be a reference (e.g., \c VectorXf&) to wrap a
|
||||
* existing vector into an AutoDiffScalar.
|
||||
* Finally, _DerType can also be any Eigen compatible expression.
|
||||
*
|
||||
* This class represents a scalar value while tracking its respective derivatives using Eigen's expression
|
||||
* template mechanism.
|
||||
*
|
||||
* It supports the following list of global math function:
|
||||
* - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos,
|
||||
* - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos,
|
||||
* - internal::conj, internal::real, internal::imag, numext::abs2.
|
||||
*
|
||||
* AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However,
|
||||
* in that case, the expression template mechanism only occurs at the top Matrix level,
|
||||
* while derivatives are computed right away.
|
||||
*
|
||||
*/
|
||||
|
||||
template<typename _DerType>
|
||||
class AutoDiffScalar
|
||||
: public internal::auto_diff_special_op
|
||||
<_DerType, !internal::is_same<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar,
|
||||
typename NumTraits<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar>::Real>::value>
|
||||
{
|
||||
public:
|
||||
typedef internal::auto_diff_special_op
|
||||
<_DerType, !internal::is_same<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar,
|
||||
typename NumTraits<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar>::Real>::value> Base;
|
||||
typedef typename internal::remove_all<_DerType>::type DerType;
|
||||
typedef typename internal::traits<DerType>::Scalar Scalar;
|
||||
typedef typename NumTraits<Scalar>::Real Real;
|
||||
|
||||
using Base::operator+;
|
||||
using Base::operator*;
|
||||
|
||||
/** Default constructor without any initialization. */
|
||||
AutoDiffScalar() {}
|
||||
|
||||
/** Constructs an active scalar from its \a value,
|
||||
and initializes the \a nbDer derivatives such that it corresponds to the \a derNumber -th variable */
|
||||
AutoDiffScalar(const Scalar& value, int nbDer, int derNumber)
|
||||
: m_value(value), m_derivatives(DerType::Zero(nbDer))
|
||||
{
|
||||
m_derivatives.coeffRef(derNumber) = Scalar(1);
|
||||
}
|
||||
|
||||
/** Conversion from a scalar constant to an active scalar.
|
||||
* The derivatives are set to zero. */
|
||||
/*explicit*/ AutoDiffScalar(const Real& value)
|
||||
: m_value(value)
|
||||
{
|
||||
if(m_derivatives.size()>0)
|
||||
m_derivatives.setZero();
|
||||
}
|
||||
|
||||
/** Constructs an active scalar from its \a value and derivatives \a der */
|
||||
AutoDiffScalar(const Scalar& value, const DerType& der)
|
||||
: m_value(value), m_derivatives(der)
|
||||
{}
|
||||
|
||||
template<typename OtherDerType>
|
||||
AutoDiffScalar(const AutoDiffScalar<OtherDerType>& other
|
||||
#ifndef EIGEN_PARSED_BY_DOXYGEN
|
||||
, typename internal::enable_if<
|
||||
internal::is_same<Scalar, typename internal::traits<typename internal::remove_all<OtherDerType>::type>::Scalar>::value
|
||||
&& internal::is_convertible<OtherDerType,DerType>::value , void*>::type = 0
|
||||
#endif
|
||||
)
|
||||
: m_value(other.value()), m_derivatives(other.derivatives())
|
||||
{}
|
||||
|
||||
friend std::ostream & operator << (std::ostream & s, const AutoDiffScalar& a)
|
||||
{
|
||||
return s << a.value();
|
||||
}
|
||||
|
||||
AutoDiffScalar(const AutoDiffScalar& other)
|
||||
: m_value(other.value()), m_derivatives(other.derivatives())
|
||||
{}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& other)
|
||||
{
|
||||
m_value = other.value();
|
||||
m_derivatives = other.derivatives();
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline AutoDiffScalar& operator=(const AutoDiffScalar& other)
|
||||
{
|
||||
m_value = other.value();
|
||||
m_derivatives = other.derivatives();
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline AutoDiffScalar& operator=(const Scalar& other)
|
||||
{
|
||||
m_value = other;
|
||||
if(m_derivatives.size()>0)
|
||||
m_derivatives.setZero();
|
||||
return *this;
|
||||
}
|
||||
|
||||
// inline operator const Scalar& () const { return m_value; }
|
||||
// inline operator Scalar& () { return m_value; }
|
||||
|
||||
inline const Scalar& value() const { return m_value; }
|
||||
inline Scalar& value() { return m_value; }
|
||||
|
||||
inline const DerType& derivatives() const { return m_derivatives; }
|
||||
inline DerType& derivatives() { return m_derivatives; }
|
||||
|
||||
inline bool operator< (const Scalar& other) const { return m_value < other; }
|
||||
inline bool operator<=(const Scalar& other) const { return m_value <= other; }
|
||||
inline bool operator> (const Scalar& other) const { return m_value > other; }
|
||||
inline bool operator>=(const Scalar& other) const { return m_value >= other; }
|
||||
inline bool operator==(const Scalar& other) const { return m_value == other; }
|
||||
inline bool operator!=(const Scalar& other) const { return m_value != other; }
|
||||
|
||||
friend inline bool operator< (const Scalar& a, const AutoDiffScalar& b) { return a < b.value(); }
|
||||
friend inline bool operator<=(const Scalar& a, const AutoDiffScalar& b) { return a <= b.value(); }
|
||||
friend inline bool operator> (const Scalar& a, const AutoDiffScalar& b) { return a > b.value(); }
|
||||
friend inline bool operator>=(const Scalar& a, const AutoDiffScalar& b) { return a >= b.value(); }
|
||||
friend inline bool operator==(const Scalar& a, const AutoDiffScalar& b) { return a == b.value(); }
|
||||
friend inline bool operator!=(const Scalar& a, const AutoDiffScalar& b) { return a != b.value(); }
|
||||
|
||||
template<typename OtherDerType> inline bool operator< (const AutoDiffScalar<OtherDerType>& b) const { return m_value < b.value(); }
|
||||
template<typename OtherDerType> inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const { return m_value <= b.value(); }
|
||||
template<typename OtherDerType> inline bool operator> (const AutoDiffScalar<OtherDerType>& b) const { return m_value > b.value(); }
|
||||
template<typename OtherDerType> inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const { return m_value >= b.value(); }
|
||||
template<typename OtherDerType> inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const { return m_value == b.value(); }
|
||||
template<typename OtherDerType> inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const { return m_value != b.value(); }
|
||||
|
||||
inline const AutoDiffScalar<DerType&> operator+(const Scalar& other) const
|
||||
{
|
||||
return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
|
||||
}
|
||||
|
||||
friend inline const AutoDiffScalar<DerType&> operator+(const Scalar& a, const AutoDiffScalar& b)
|
||||
{
|
||||
return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
|
||||
}
|
||||
|
||||
// inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
|
||||
// {
|
||||
// return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
|
||||
// }
|
||||
|
||||
// friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar& b)
|
||||
// {
|
||||
// return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
|
||||
// }
|
||||
|
||||
inline AutoDiffScalar& operator+=(const Scalar& other)
|
||||
{
|
||||
value() += other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >
|
||||
operator+(const AutoDiffScalar<OtherDerType>& other) const
|
||||
{
|
||||
internal::make_coherent(m_derivatives, other.derivatives());
|
||||
return AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >(
|
||||
m_value + other.value(),
|
||||
m_derivatives + other.derivatives());
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline AutoDiffScalar&
|
||||
operator+=(const AutoDiffScalar<OtherDerType>& other)
|
||||
{
|
||||
(*this) = (*this) + other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const AutoDiffScalar<DerType&> operator-(const Scalar& b) const
|
||||
{
|
||||
return AutoDiffScalar<DerType&>(m_value - b, m_derivatives);
|
||||
}
|
||||
|
||||
friend inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
|
||||
operator-(const Scalar& a, const AutoDiffScalar& b)
|
||||
{
|
||||
return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
|
||||
(a - b.value(), -b.derivatives());
|
||||
}
|
||||
|
||||
inline AutoDiffScalar& operator-=(const Scalar& other)
|
||||
{
|
||||
value() -= other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >
|
||||
operator-(const AutoDiffScalar<OtherDerType>& other) const
|
||||
{
|
||||
internal::make_coherent(m_derivatives, other.derivatives());
|
||||
return AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >(
|
||||
m_value - other.value(),
|
||||
m_derivatives - other.derivatives());
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline AutoDiffScalar&
|
||||
operator-=(const AutoDiffScalar<OtherDerType>& other)
|
||||
{
|
||||
*this = *this - other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
|
||||
operator-() const
|
||||
{
|
||||
return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >(
|
||||
-m_value,
|
||||
-m_derivatives);
|
||||
}
|
||||
|
||||
inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
|
||||
operator*(const Scalar& other) const
|
||||
{
|
||||
return MakeAutoDiffScalar(m_value * other, m_derivatives * other);
|
||||
}
|
||||
|
||||
friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
|
||||
operator*(const Scalar& other, const AutoDiffScalar& a)
|
||||
{
|
||||
return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other);
|
||||
}
|
||||
|
||||
// inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
|
||||
// operator*(const Real& other) const
|
||||
// {
|
||||
// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
|
||||
// m_value * other,
|
||||
// (m_derivatives * other));
|
||||
// }
|
||||
//
|
||||
// friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
|
||||
// operator*(const Real& other, const AutoDiffScalar& a)
|
||||
// {
|
||||
// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
|
||||
// a.value() * other,
|
||||
// a.derivatives() * other);
|
||||
// }
|
||||
|
||||
inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
|
||||
operator/(const Scalar& other) const
|
||||
{
|
||||
return MakeAutoDiffScalar(m_value / other, (m_derivatives * (Scalar(1)/other)));
|
||||
}
|
||||
|
||||
friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
|
||||
operator/(const Scalar& other, const AutoDiffScalar& a)
|
||||
{
|
||||
return MakeAutoDiffScalar(other / a.value(), a.derivatives() * (Scalar(-other) / (a.value()*a.value())));
|
||||
}
|
||||
|
||||
// inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
|
||||
// operator/(const Real& other) const
|
||||
// {
|
||||
// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
|
||||
// m_value / other,
|
||||
// (m_derivatives * (Real(1)/other)));
|
||||
// }
|
||||
//
|
||||
// friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
|
||||
// operator/(const Real& other, const AutoDiffScalar& a)
|
||||
// {
|
||||
// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
|
||||
// other / a.value(),
|
||||
// a.derivatives() * (-Real(1)/other));
|
||||
// }
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(
|
||||
CwiseBinaryOp<internal::scalar_difference_op<Scalar> EIGEN_COMMA
|
||||
const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) EIGEN_COMMA
|
||||
const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) >,Scalar,product) >
|
||||
operator/(const AutoDiffScalar<OtherDerType>& other) const
|
||||
{
|
||||
internal::make_coherent(m_derivatives, other.derivatives());
|
||||
return MakeAutoDiffScalar(
|
||||
m_value / other.value(),
|
||||
((m_derivatives * other.value()) - (other.derivatives() * m_value))
|
||||
* (Scalar(1)/(other.value()*other.value())));
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
|
||||
const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product),
|
||||
const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) > >
|
||||
operator*(const AutoDiffScalar<OtherDerType>& other) const
|
||||
{
|
||||
internal::make_coherent(m_derivatives, other.derivatives());
|
||||
return MakeAutoDiffScalar(
|
||||
m_value * other.value(),
|
||||
(m_derivatives * other.value()) + (other.derivatives() * m_value));
|
||||
}
|
||||
|
||||
inline AutoDiffScalar& operator*=(const Scalar& other)
|
||||
{
|
||||
*this = *this * other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other)
|
||||
{
|
||||
*this = *this * other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline AutoDiffScalar& operator/=(const Scalar& other)
|
||||
{
|
||||
*this = *this / other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& other)
|
||||
{
|
||||
*this = *this / other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
protected:
|
||||
Scalar m_value;
|
||||
DerType m_derivatives;
|
||||
|
||||
};
|
||||
|
||||
namespace internal {
|
||||
|
||||
template<typename _DerType>
|
||||
struct auto_diff_special_op<_DerType, true>
|
||||
// : auto_diff_scalar_op<_DerType, typename NumTraits<Scalar>::Real,
|
||||
// is_same<Scalar,typename NumTraits<Scalar>::Real>::value>
|
||||
{
|
||||
typedef typename remove_all<_DerType>::type DerType;
|
||||
typedef typename traits<DerType>::Scalar Scalar;
|
||||
typedef typename NumTraits<Scalar>::Real Real;
|
||||
|
||||
// typedef auto_diff_scalar_op<_DerType, typename NumTraits<Scalar>::Real,
|
||||
// is_same<Scalar,typename NumTraits<Scalar>::Real>::value> Base;
|
||||
|
||||
// using Base::operator+;
|
||||
// using Base::operator+=;
|
||||
// using Base::operator-;
|
||||
// using Base::operator-=;
|
||||
// using Base::operator*;
|
||||
// using Base::operator*=;
|
||||
|
||||
const AutoDiffScalar<_DerType>& derived() const { return *static_cast<const AutoDiffScalar<_DerType>*>(this); }
|
||||
AutoDiffScalar<_DerType>& derived() { return *static_cast<AutoDiffScalar<_DerType>*>(this); }
|
||||
|
||||
|
||||
inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
|
||||
{
|
||||
return AutoDiffScalar<DerType&>(derived().value() + other, derived().derivatives());
|
||||
}
|
||||
|
||||
friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar<_DerType>& b)
|
||||
{
|
||||
return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
|
||||
}
|
||||
|
||||
inline AutoDiffScalar<_DerType>& operator+=(const Real& other)
|
||||
{
|
||||
derived().value() += other;
|
||||
return derived();
|
||||
}
|
||||
|
||||
|
||||
inline const AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >
|
||||
operator*(const Real& other) const
|
||||
{
|
||||
return AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >(
|
||||
derived().value() * other,
|
||||
derived().derivatives() * other);
|
||||
}
|
||||
|
||||
friend inline const AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >
|
||||
operator*(const Real& other, const AutoDiffScalar<_DerType>& a)
|
||||
{
|
||||
return AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >(
|
||||
a.value() * other,
|
||||
a.derivatives() * other);
|
||||
}
|
||||
|
||||
inline AutoDiffScalar<_DerType>& operator*=(const Scalar& other)
|
||||
{
|
||||
*this = *this * other;
|
||||
return derived();
|
||||
}
|
||||
};
|
||||
|
||||
template<typename _DerType>
|
||||
struct auto_diff_special_op<_DerType, false>
|
||||
{
|
||||
void operator*() const;
|
||||
void operator-() const;
|
||||
void operator+() const;
|
||||
};
|
||||
|
||||
template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols, typename B>
|
||||
struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>, B> {
|
||||
typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
|
||||
static void run(A& a, B& b) {
|
||||
if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
|
||||
{
|
||||
a.resize(b.size());
|
||||
a.setZero();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template<typename A, typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
|
||||
struct make_coherent_impl<A, Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
|
||||
typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
|
||||
static void run(A& a, B& b) {
|
||||
if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
|
||||
{
|
||||
b.resize(a.size());
|
||||
b.setZero();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols,
|
||||
typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
|
||||
struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>,
|
||||
Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
|
||||
typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
|
||||
typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
|
||||
static void run(A& a, B& b) {
|
||||
if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
|
||||
{
|
||||
a.resize(b.size());
|
||||
a.setZero();
|
||||
}
|
||||
else if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
|
||||
{
|
||||
b.resize(a.size());
|
||||
b.setZero();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // end namespace internal
|
||||
|
||||
template<typename DerType, typename BinOp>
|
||||
struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,typename DerType::Scalar,BinOp>
|
||||
{
|
||||
typedef AutoDiffScalar<DerType> ReturnType;
|
||||
};
|
||||
|
||||
template<typename DerType, typename BinOp>
|
||||
struct ScalarBinaryOpTraits<typename DerType::Scalar,AutoDiffScalar<DerType>, BinOp>
|
||||
{
|
||||
typedef AutoDiffScalar<DerType> ReturnType;
|
||||
};
|
||||
|
||||
|
||||
// The following is an attempt to let Eigen's known about expression template, but that's more tricky!
|
||||
|
||||
// template<typename DerType, typename BinOp>
|
||||
// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,AutoDiffScalar<DerType>, BinOp>
|
||||
// {
|
||||
// enum { Defined = 1 };
|
||||
// typedef AutoDiffScalar<typename DerType::PlainObject> ReturnType;
|
||||
// };
|
||||
//
|
||||
// template<typename DerType1,typename DerType2, typename BinOp>
|
||||
// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType1>,AutoDiffScalar<DerType2>, BinOp>
|
||||
// {
|
||||
// enum { Defined = 1 };//internal::is_same<typename DerType1::Scalar,typename DerType2::Scalar>::value };
|
||||
// typedef AutoDiffScalar<typename DerType1::PlainObject> ReturnType;
|
||||
// };
|
||||
|
||||
#define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \
|
||||
template<typename DerType> \
|
||||
inline const Eigen::AutoDiffScalar< \
|
||||
EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename Eigen::internal::remove_all<DerType>::type, typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar, product) > \
|
||||
FUNC(const Eigen::AutoDiffScalar<DerType>& x) { \
|
||||
using namespace Eigen; \
|
||||
typedef typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar Scalar; \
|
||||
EIGEN_UNUSED_VARIABLE(sizeof(Scalar)); \
|
||||
CODE; \
|
||||
}
|
||||
|
||||
template<typename DerType>
|
||||
inline const AutoDiffScalar<DerType>& conj(const AutoDiffScalar<DerType>& x) { return x; }
|
||||
template<typename DerType>
|
||||
inline const AutoDiffScalar<DerType>& real(const AutoDiffScalar<DerType>& x) { return x; }
|
||||
template<typename DerType>
|
||||
inline typename DerType::Scalar imag(const AutoDiffScalar<DerType>&) { return 0.; }
|
||||
template<typename DerType, typename T>
|
||||
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const AutoDiffScalar<DerType>& x, const T& y) {
|
||||
typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
|
||||
return (x <= y ? ADS(x) : ADS(y));
|
||||
}
|
||||
template<typename DerType, typename T>
|
||||
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const AutoDiffScalar<DerType>& x, const T& y) {
|
||||
typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
|
||||
return (x >= y ? ADS(x) : ADS(y));
|
||||
}
|
||||
template<typename DerType, typename T>
|
||||
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const T& x, const AutoDiffScalar<DerType>& y) {
|
||||
typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
|
||||
return (x < y ? ADS(x) : ADS(y));
|
||||
}
|
||||
template<typename DerType, typename T>
|
||||
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const T& x, const AutoDiffScalar<DerType>& y) {
|
||||
typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
|
||||
return (x > y ? ADS(x) : ADS(y));
|
||||
}
|
||||
template<typename DerType>
|
||||
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
|
||||
return (x.value() < y.value() ? x : y);
|
||||
}
|
||||
template<typename DerType>
|
||||
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
|
||||
return (x.value() >= y.value() ? x : y);
|
||||
}
|
||||
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs,
|
||||
using std::abs;
|
||||
return Eigen::MakeAutoDiffScalar(abs(x.value()), x.derivatives() * (x.value()<0 ? -1 : 1) );)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs2,
|
||||
using numext::abs2;
|
||||
return Eigen::MakeAutoDiffScalar(abs2(x.value()), x.derivatives() * (Scalar(2)*x.value()));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sqrt,
|
||||
using std::sqrt;
|
||||
Scalar sqrtx = sqrt(x.value());
|
||||
return Eigen::MakeAutoDiffScalar(sqrtx,x.derivatives() * (Scalar(0.5) / sqrtx));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cos,
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
return Eigen::MakeAutoDiffScalar(cos(x.value()), x.derivatives() * (-sin(x.value())));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sin,
|
||||
using std::sin;
|
||||
using std::cos;
|
||||
return Eigen::MakeAutoDiffScalar(sin(x.value()),x.derivatives() * cos(x.value()));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(exp,
|
||||
using std::exp;
|
||||
Scalar expx = exp(x.value());
|
||||
return Eigen::MakeAutoDiffScalar(expx,x.derivatives() * expx);)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(log,
|
||||
using std::log;
|
||||
return Eigen::MakeAutoDiffScalar(log(x.value()),x.derivatives() * (Scalar(1)/x.value()));)
|
||||
|
||||
template<typename DerType>
|
||||
inline const Eigen::AutoDiffScalar<
|
||||
EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<DerType>::type,typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar,product) >
|
||||
pow(const Eigen::AutoDiffScalar<DerType> &x, const typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar &y)
|
||||
{
|
||||
using namespace Eigen;
|
||||
using std::pow;
|
||||
return Eigen::MakeAutoDiffScalar(pow(x.value(),y), x.derivatives() * (y * pow(x.value(),y-1)));
|
||||
}
|
||||
|
||||
|
||||
template<typename DerTypeA,typename DerTypeB>
|
||||
inline const AutoDiffScalar<Matrix<typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar,Dynamic,1> >
|
||||
atan2(const AutoDiffScalar<DerTypeA>& a, const AutoDiffScalar<DerTypeB>& b)
|
||||
{
|
||||
using std::atan2;
|
||||
typedef typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar Scalar;
|
||||
typedef AutoDiffScalar<Matrix<Scalar,Dynamic,1> > PlainADS;
|
||||
PlainADS ret;
|
||||
ret.value() = atan2(a.value(), b.value());
|
||||
|
||||
Scalar squared_hypot = a.value() * a.value() + b.value() * b.value();
|
||||
|
||||
// if (squared_hypot==0) the derivation is undefined and the following results in a NaN:
|
||||
ret.derivatives() = (a.derivatives() * b.value() - a.value() * b.derivatives()) / squared_hypot;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tan,
|
||||
using std::tan;
|
||||
using std::cos;
|
||||
return Eigen::MakeAutoDiffScalar(tan(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cos(x.value()))));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(asin,
|
||||
using std::sqrt;
|
||||
using std::asin;
|
||||
return Eigen::MakeAutoDiffScalar(asin(x.value()),x.derivatives() * (Scalar(1)/sqrt(1-numext::abs2(x.value()))));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(acos,
|
||||
using std::sqrt;
|
||||
using std::acos;
|
||||
return Eigen::MakeAutoDiffScalar(acos(x.value()),x.derivatives() * (Scalar(-1)/sqrt(1-numext::abs2(x.value()))));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tanh,
|
||||
using std::cosh;
|
||||
using std::tanh;
|
||||
return Eigen::MakeAutoDiffScalar(tanh(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cosh(x.value()))));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sinh,
|
||||
using std::sinh;
|
||||
using std::cosh;
|
||||
return Eigen::MakeAutoDiffScalar(sinh(x.value()),x.derivatives() * cosh(x.value()));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cosh,
|
||||
using std::sinh;
|
||||
using std::cosh;
|
||||
return Eigen::MakeAutoDiffScalar(cosh(x.value()),x.derivatives() * sinh(x.value()));)
|
||||
|
||||
#undef EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY
|
||||
|
||||
template<typename DerType> struct NumTraits<AutoDiffScalar<DerType> >
|
||||
: NumTraits< typename NumTraits<typename internal::remove_all<DerType>::type::Scalar>::Real >
|
||||
{
|
||||
typedef typename internal::remove_all<DerType>::type DerTypeCleaned;
|
||||
typedef AutoDiffScalar<Matrix<typename NumTraits<typename DerTypeCleaned::Scalar>::Real,DerTypeCleaned::RowsAtCompileTime,DerTypeCleaned::ColsAtCompileTime,
|
||||
0, DerTypeCleaned::MaxRowsAtCompileTime, DerTypeCleaned::MaxColsAtCompileTime> > Real;
|
||||
typedef AutoDiffScalar<DerType> NonInteger;
|
||||
typedef AutoDiffScalar<DerType> Nested;
|
||||
typedef typename NumTraits<typename DerTypeCleaned::Scalar>::Literal Literal;
|
||||
enum{
|
||||
RequireInitialization = 1
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
namespace std {
|
||||
template <typename T>
|
||||
class numeric_limits<Eigen::AutoDiffScalar<T> >
|
||||
: public numeric_limits<typename T::Scalar> {};
|
||||
|
||||
} // namespace std
|
||||
|
||||
#endif // EIGEN_AUTODIFF_SCALAR_H
|
||||
@@ -0,0 +1,220 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_AUTODIFF_VECTOR_H
|
||||
#define EIGEN_AUTODIFF_VECTOR_H
|
||||
|
||||
namespace Eigen {
|
||||
|
||||
/* \class AutoDiffScalar
|
||||
* \brief A scalar type replacement with automatic differentation capability
|
||||
*
|
||||
* \param DerType the vector type used to store/represent the derivatives (e.g. Vector3f)
|
||||
*
|
||||
* This class represents a scalar value while tracking its respective derivatives.
|
||||
*
|
||||
* It supports the following list of global math function:
|
||||
* - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos,
|
||||
* - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos,
|
||||
* - internal::conj, internal::real, internal::imag, numext::abs2.
|
||||
*
|
||||
* AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However,
|
||||
* in that case, the expression template mechanism only occurs at the top Matrix level,
|
||||
* while derivatives are computed right away.
|
||||
*
|
||||
*/
|
||||
template<typename ValueType, typename JacobianType>
|
||||
class AutoDiffVector
|
||||
{
|
||||
public:
|
||||
//typedef typename internal::traits<ValueType>::Scalar Scalar;
|
||||
typedef typename internal::traits<ValueType>::Scalar BaseScalar;
|
||||
typedef AutoDiffScalar<Matrix<BaseScalar,JacobianType::RowsAtCompileTime,1> > ActiveScalar;
|
||||
typedef ActiveScalar Scalar;
|
||||
typedef AutoDiffScalar<typename JacobianType::ColXpr> CoeffType;
|
||||
typedef typename JacobianType::Index Index;
|
||||
|
||||
inline AutoDiffVector() {}
|
||||
|
||||
inline AutoDiffVector(const ValueType& values)
|
||||
: m_values(values)
|
||||
{
|
||||
m_jacobian.setZero();
|
||||
}
|
||||
|
||||
|
||||
CoeffType operator[] (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
|
||||
const CoeffType operator[] (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
|
||||
|
||||
CoeffType operator() (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
|
||||
const CoeffType operator() (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
|
||||
|
||||
CoeffType coeffRef(Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
|
||||
const CoeffType coeffRef(Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
|
||||
|
||||
Index size() const { return m_values.size(); }
|
||||
|
||||
// FIXME here we could return an expression of the sum
|
||||
Scalar sum() const { /*std::cerr << "sum \n\n";*/ /*std::cerr << m_jacobian.rowwise().sum() << "\n\n";*/ return Scalar(m_values.sum(), m_jacobian.rowwise().sum()); }
|
||||
|
||||
|
||||
inline AutoDiffVector(const ValueType& values, const JacobianType& jac)
|
||||
: m_values(values), m_jacobian(jac)
|
||||
{}
|
||||
|
||||
template<typename OtherValueType, typename OtherJacobianType>
|
||||
inline AutoDiffVector(const AutoDiffVector<OtherValueType, OtherJacobianType>& other)
|
||||
: m_values(other.values()), m_jacobian(other.jacobian())
|
||||
{}
|
||||
|
||||
inline AutoDiffVector(const AutoDiffVector& other)
|
||||
: m_values(other.values()), m_jacobian(other.jacobian())
|
||||
{}
|
||||
|
||||
template<typename OtherValueType, typename OtherJacobianType>
|
||||
inline AutoDiffVector& operator=(const AutoDiffVector<OtherValueType, OtherJacobianType>& other)
|
||||
{
|
||||
m_values = other.values();
|
||||
m_jacobian = other.jacobian();
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline AutoDiffVector& operator=(const AutoDiffVector& other)
|
||||
{
|
||||
m_values = other.values();
|
||||
m_jacobian = other.jacobian();
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const ValueType& values() const { return m_values; }
|
||||
inline ValueType& values() { return m_values; }
|
||||
|
||||
inline const JacobianType& jacobian() const { return m_jacobian; }
|
||||
inline JacobianType& jacobian() { return m_jacobian; }
|
||||
|
||||
template<typename OtherValueType,typename OtherJacobianType>
|
||||
inline const AutoDiffVector<
|
||||
typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
|
||||
typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >
|
||||
operator+(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
|
||||
{
|
||||
return AutoDiffVector<
|
||||
typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
|
||||
typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >(
|
||||
m_values + other.values(),
|
||||
m_jacobian + other.jacobian());
|
||||
}
|
||||
|
||||
template<typename OtherValueType, typename OtherJacobianType>
|
||||
inline AutoDiffVector&
|
||||
operator+=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
|
||||
{
|
||||
m_values += other.values();
|
||||
m_jacobian += other.jacobian();
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename OtherValueType,typename OtherJacobianType>
|
||||
inline const AutoDiffVector<
|
||||
typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
|
||||
typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >
|
||||
operator-(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
|
||||
{
|
||||
return AutoDiffVector<
|
||||
typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
|
||||
typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >(
|
||||
m_values - other.values(),
|
||||
m_jacobian - other.jacobian());
|
||||
}
|
||||
|
||||
template<typename OtherValueType, typename OtherJacobianType>
|
||||
inline AutoDiffVector&
|
||||
operator-=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
|
||||
{
|
||||
m_values -= other.values();
|
||||
m_jacobian -= other.jacobian();
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const AutoDiffVector<
|
||||
typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
|
||||
typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >
|
||||
operator-() const
|
||||
{
|
||||
return AutoDiffVector<
|
||||
typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
|
||||
typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >(
|
||||
-m_values,
|
||||
-m_jacobian);
|
||||
}
|
||||
|
||||
inline const AutoDiffVector<
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type>
|
||||
operator*(const BaseScalar& other) const
|
||||
{
|
||||
return AutoDiffVector<
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
|
||||
m_values * other,
|
||||
m_jacobian * other);
|
||||
}
|
||||
|
||||
friend inline const AutoDiffVector<
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >
|
||||
operator*(const Scalar& other, const AutoDiffVector& v)
|
||||
{
|
||||
return AutoDiffVector<
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
|
||||
v.values() * other,
|
||||
v.jacobian() * other);
|
||||
}
|
||||
|
||||
// template<typename OtherValueType,typename OtherJacobianType>
|
||||
// inline const AutoDiffVector<
|
||||
// CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
|
||||
// CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
|
||||
// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
|
||||
// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >
|
||||
// operator*(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
|
||||
// {
|
||||
// return AutoDiffVector<
|
||||
// CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
|
||||
// CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
|
||||
// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
|
||||
// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >(
|
||||
// m_values.cwise() * other.values(),
|
||||
// (m_jacobian * other.values()) + (m_values * other.jacobian()));
|
||||
// }
|
||||
|
||||
inline AutoDiffVector& operator*=(const Scalar& other)
|
||||
{
|
||||
m_values *= other;
|
||||
m_jacobian *= other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename OtherValueType,typename OtherJacobianType>
|
||||
inline AutoDiffVector& operator*=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
|
||||
{
|
||||
*this = *this * other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
protected:
|
||||
ValueType m_values;
|
||||
JacobianType m_jacobian;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // EIGEN_AUTODIFF_VECTOR_H
|
||||
152
wpiutil/src/main/native/include/drake/common/drake_assert.h
Normal file
152
wpiutil/src/main/native/include/drake/common/drake_assert.h
Normal file
@@ -0,0 +1,152 @@
|
||||
#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
|
||||
// symbolic_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<Condition, bool>::value;
|
||||
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<decltype(condition)>::type> Trait; \
|
||||
static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
|
||||
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<decltype(expression), void>::value, \
|
||||
"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) static_assert( \
|
||||
::drake::assert::ConditionTraits< \
|
||||
typename std::remove_cv<decltype(condition)>::type>::is_valid, \
|
||||
"Condition should be bool-convertible.");
|
||||
# define DRAKE_ASSERT_VOID(expression) static_assert( \
|
||||
std::is_convertible<decltype(expression), void>::value, \
|
||||
"Expression should be void.")
|
||||
#endif
|
||||
|
||||
#endif // DRAKE_DOXYGEN_CXX
|
||||
@@ -0,0 +1,18 @@
|
||||
#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
|
||||
112
wpiutil/src/main/native/include/drake/common/drake_copyable.h
Normal file
112
wpiutil/src/main/native/include/drake/common/drake_copyable.h
Normal file
@@ -0,0 +1,112 @@
|
||||
#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 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. Invoke this 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>
|
||||
*/
|
||||
#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 default-copy doesn't work. */ \
|
||||
static void DRAKE_COPYABLE_DEMAND_COPY_CAN_COMPILE() { \
|
||||
(void) static_cast<Classname& (Classname::*)( \
|
||||
const Classname&)>(&Classname::operator=); \
|
||||
}
|
||||
|
||||
/** DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN declares (but does not define) the
|
||||
special member functions for copy-construction, copy-assignment,
|
||||
move-construction, and move-assignment. Drake's Doxygen is customized to
|
||||
render the declarations with appropriate comments.
|
||||
|
||||
This is useful when paired with DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T
|
||||
to work around https://gcc.gnu.org/bugzilla/show_bug.cgi?id=57728 whereby the
|
||||
declaration and definition must be split. Once Drake no longer supports GCC
|
||||
versions prior to 6.3, this macro could be removed.
|
||||
|
||||
Invoke this this macro in the public section of the class declaration, e.g.:
|
||||
<pre>
|
||||
template <typename T>
|
||||
class Foo {
|
||||
public:
|
||||
DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN(Foo)
|
||||
|
||||
// ...
|
||||
};
|
||||
DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T(Foo)
|
||||
</pre>
|
||||
*/
|
||||
#define DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN(Classname) \
|
||||
Classname(const Classname&); \
|
||||
Classname& operator=(const Classname&); \
|
||||
Classname(Classname&&); \
|
||||
Classname& operator=(Classname&&); \
|
||||
/* Fails at compile-time if default-copy doesn't work. */ \
|
||||
static void DRAKE_COPYABLE_DEMAND_COPY_CAN_COMPILE() { \
|
||||
(void) static_cast<Classname& (Classname::*)( \
|
||||
const Classname&)>(&Classname::operator=); \
|
||||
}
|
||||
|
||||
/** Helper for DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN. Provides defaulted
|
||||
definitions for the four special member functions of a templated class.
|
||||
*/
|
||||
#define DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T(Classname) \
|
||||
template <typename T> \
|
||||
Classname<T>::Classname(const Classname<T>&) = default; \
|
||||
template <typename T> \
|
||||
Classname<T>& Classname<T>::operator=(const Classname<T>&) = default; \
|
||||
template <typename T> \
|
||||
Classname<T>::Classname(Classname<T>&&) = default; \
|
||||
template <typename T> \
|
||||
Classname<T>& Classname<T>::operator=(Classname<T>&&) = default;
|
||||
31
wpiutil/src/main/native/include/drake/common/drake_throw.h
Normal file
31
wpiutil/src/main/native/include/drake/common/drake_throw.h
Normal file
@@ -0,0 +1,31 @@
|
||||
#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.
|
||||
#define DRAKE_THROW_UNLESS(condition) \
|
||||
do { \
|
||||
typedef ::drake::assert::ConditionTraits< \
|
||||
typename std::remove_cv<decltype(condition)>::type> Trait; \
|
||||
static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
|
||||
if (!Trait::Evaluate(condition)) { \
|
||||
::drake::internal::Throw(#condition, __func__, __FILE__, __LINE__); \
|
||||
} \
|
||||
} while (0)
|
||||
@@ -0,0 +1,62 @@
|
||||
#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
|
||||
@@ -0,0 +1,84 @@
|
||||
#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
|
||||
//
|
||||
// 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
|
||||
@@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
namespace drake {
|
||||
namespace math {
|
||||
|
||||
/// 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
|
||||
///
|
||||
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);
|
||||
} // namespace math
|
||||
} // namespace drake
|
||||
|
||||
Reference in New Issue
Block a user