diff --git a/wpimath/drake-dare-qrn-tests.patch b/wpimath/drake-dare-qrn-tests.patch new file mode 100644 index 0000000000..16b6d20dd8 --- /dev/null +++ b/wpimath/drake-dare-qrn-tests.patch @@ -0,0 +1,82 @@ +diff --git a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp +index 2deb039a0..2ee42a19c 100644 +--- a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp ++++ b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp +@@ -33,6 +33,29 @@ void SolveDAREandVerify(const Eigen::Ref& A, + MatrixCompareType::absolute)); + } + ++void SolveDAREandVerify(const Eigen::Ref& A, ++ const Eigen::Ref& B, ++ const Eigen::Ref& Q, ++ const Eigen::Ref& R, ++ const Eigen::Ref& N) { ++ MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R, N); ++ // Check that X is positive semi-definite. ++ EXPECT_TRUE( ++ CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute)); ++ int n = X.rows(); ++ Eigen::SelfAdjointEigenSolver es(X); ++ for (int i = 0; i < n; i++) { ++ EXPECT_GE(es.eigenvalues()[i], 0); ++ } ++ // Check that X is the solution to the discrete time ARE. ++ MatrixXd Y = A.transpose() * X * A - X - ++ (A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse() * ++ (B.transpose() * X * A + N.transpose()) + ++ Q; ++ EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10, ++ MatrixCompareType::absolute)); ++} ++ + GTEST_TEST(DARE, SolveDAREandVerify) { + // Test 1: non-invertible A + // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic +@@ -44,6 +67,12 @@ GTEST_TEST(DARE, SolveDAREandVerify) { + Q1 << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; + R1 << 0.25; + SolveDAREandVerify(A1, B1, Q1, R1); ++ ++ MatrixXd Aref1(n1, n1); ++ Aref1 << 0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0; ++ SolveDAREandVerify(A1, B1, (A1 - Aref1).transpose() * Q1 * (A1 - Aref1), ++ B1.transpose() * Q1 * B1 + R1, (A1 - Aref1).transpose() * Q1 * B1); ++ + // Test 2: invertible A + int n2 = 2, m2 = 1; + MatrixXd A2(n2, n2), B2(n2, m2), Q2(n2, n2), R2(m2, m2); +@@ -52,6 +81,12 @@ GTEST_TEST(DARE, SolveDAREandVerify) { + Q2 << 1, 0, 0, 0; + R2 << 0.3; + SolveDAREandVerify(A2, B2, Q2, R2); ++ ++ MatrixXd Aref2(n2, n2); ++ Aref2 << 0.5, 1, 0, 1; ++ SolveDAREandVerify(A2, B2, (A2 - Aref2).transpose() * Q2 * (A2 - Aref2), ++ B2.transpose() * Q2 * B2 + R2, (A2 - Aref2).transpose() * Q2 * B2); ++ + // Test 3: the first generalized eigenvalue of (S,T) is stable + int n3 = 2, m3 = 1; + MatrixXd A3(n3, n3), B3(n3, m3), Q3(n3, n3), R3(m3, m3); +@@ -60,12 +95,21 @@ GTEST_TEST(DARE, SolveDAREandVerify) { + Q3 << 1, 0, 0, 1; + R3 << 1; + SolveDAREandVerify(A3, B3, Q3, R3); ++ ++ MatrixXd Aref3(n3, n3); ++ Aref3 << 0, 0.5, 0, 0; ++ SolveDAREandVerify(A3, B3, (A3 - Aref3).transpose() * Q3 * (A3 - Aref3), ++ B3.transpose() * Q3 * B3 + R3, (A3 - Aref3).transpose() * Q3 * B3); ++ + // Test 4: A = B = Q = R = I_2 (2-by-2 identity matrix) + const Eigen::MatrixXd A4{Eigen::Matrix2d::Identity()}; + const Eigen::MatrixXd B4{Eigen::Matrix2d::Identity()}; + const Eigen::MatrixXd Q4{Eigen::Matrix2d::Identity()}; + const Eigen::MatrixXd R4{Eigen::Matrix2d::Identity()}; + SolveDAREandVerify(A4, B4, Q4, R4); ++ ++ const Eigen::MatrixXd N4{Eigen::Matrix2d::Identity()}; ++ SolveDAREandVerify(A4, B4, Q4, R4, N4); + } + } // namespace + } // namespace math diff --git a/wpimath/drake-replace-dense-with-core.patch b/wpimath/drake-replace-dense-with-core.patch index 0294a0e2a3..d225597bd3 100644 --- a/wpimath/drake-replace-dense-with-core.patch +++ b/wpimath/drake-replace-dense-with-core.patch @@ -38,3 +38,27 @@ index b64bfe75e..fc5efb313 100644 namespace drake { namespace math { +diff --git b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp +index 74aa4b23d..2deb039a0 100644 +--- b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp ++++ a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp +@@ -1,5 +1,6 @@ + #include "drake/math/discrete_algebraic_riccati_equation.h" + ++#include + #include + + #include "drake/common/test_utilities/eigen_matrix_compare.h" +diff --git b/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h +index e3bd85349..d6bcbb8ec 100644 +--- b/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h ++++ a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h +@@ -4,7 +4,7 @@ + #include + #include + +-#include ++#include + #include + + // #include "drake/common/text_logging.h" diff --git a/wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp similarity index 57% rename from wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp rename to wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp index 53d8d3b87a..2ee42a19cd 100644 --- a/wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp +++ b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp @@ -1,12 +1,10 @@ #include "drake/math/discrete_algebraic_riccati_equation.h" -#include #include - #include #include "drake/common/test_utilities/eigen_matrix_compare.h" -#include "drake/math/autodiff.h" +// #include "drake/math/autodiff.h" using Eigen::MatrixXd; @@ -35,6 +33,29 @@ void SolveDAREandVerify(const Eigen::Ref& A, MatrixCompareType::absolute)); } +void SolveDAREandVerify(const Eigen::Ref& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R, + const Eigen::Ref& N) { + MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R, N); + // Check that X is positive semi-definite. + EXPECT_TRUE( + CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute)); + int n = X.rows(); + Eigen::SelfAdjointEigenSolver es(X); + for (int i = 0; i < n; i++) { + EXPECT_GE(es.eigenvalues()[i], 0); + } + // Check that X is the solution to the discrete time ARE. + MatrixXd Y = A.transpose() * X * A - X - + (A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse() * + (B.transpose() * X * A + N.transpose()) + + Q; + EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10, + MatrixCompareType::absolute)); +} + GTEST_TEST(DARE, SolveDAREandVerify) { // Test 1: non-invertible A // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic @@ -46,6 +67,12 @@ GTEST_TEST(DARE, SolveDAREandVerify) { Q1 << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; R1 << 0.25; SolveDAREandVerify(A1, B1, Q1, R1); + + MatrixXd Aref1(n1, n1); + Aref1 << 0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0; + SolveDAREandVerify(A1, B1, (A1 - Aref1).transpose() * Q1 * (A1 - Aref1), + B1.transpose() * Q1 * B1 + R1, (A1 - Aref1).transpose() * Q1 * B1); + // Test 2: invertible A int n2 = 2, m2 = 1; MatrixXd A2(n2, n2), B2(n2, m2), Q2(n2, n2), R2(m2, m2); @@ -54,6 +81,12 @@ GTEST_TEST(DARE, SolveDAREandVerify) { Q2 << 1, 0, 0, 0; R2 << 0.3; SolveDAREandVerify(A2, B2, Q2, R2); + + MatrixXd Aref2(n2, n2); + Aref2 << 0.5, 1, 0, 1; + SolveDAREandVerify(A2, B2, (A2 - Aref2).transpose() * Q2 * (A2 - Aref2), + B2.transpose() * Q2 * B2 + R2, (A2 - Aref2).transpose() * Q2 * B2); + // Test 3: the first generalized eigenvalue of (S,T) is stable int n3 = 2, m3 = 1; MatrixXd A3(n3, n3), B3(n3, m3), Q3(n3, n3), R3(m3, m3); @@ -62,12 +95,21 @@ GTEST_TEST(DARE, SolveDAREandVerify) { Q3 << 1, 0, 0, 1; R3 << 1; SolveDAREandVerify(A3, B3, Q3, R3); + + MatrixXd Aref3(n3, n3); + Aref3 << 0, 0.5, 0, 0; + SolveDAREandVerify(A3, B3, (A3 - Aref3).transpose() * Q3 * (A3 - Aref3), + B3.transpose() * Q3 * B3 + R3, (A3 - Aref3).transpose() * Q3 * B3); + // Test 4: A = B = Q = R = I_2 (2-by-2 identity matrix) const Eigen::MatrixXd A4{Eigen::Matrix2d::Identity()}; const Eigen::MatrixXd B4{Eigen::Matrix2d::Identity()}; const Eigen::MatrixXd Q4{Eigen::Matrix2d::Identity()}; const Eigen::MatrixXd R4{Eigen::Matrix2d::Identity()}; SolveDAREandVerify(A4, B4, Q4, R4); + + const Eigen::MatrixXd N4{Eigen::Matrix2d::Identity()}; + SolveDAREandVerify(A4, B4, Q4, R4, N4); } } // namespace } // namespace math diff --git a/wpimath/src/test/native/include/drake/common/autodiff.h b/wpimath/src/test/native/include/drake/common/autodiff.h deleted file mode 100644 index 66fd88a64e..0000000000 --- a/wpimath/src/test/native/include/drake/common/autodiff.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once -/// @file This header provides a single inclusion point for autodiff-related -/// header files in the `drake/common` directory. Users should include this -/// file. Including other individual headers such as `drake/common/autodiffxd.h` -/// will generate a compile-time warning. - -// In each header included below, it asserts that this macro -// `DRAKE_COMMON_AUTODIFF_HEADER` is defined. If the macro is not defined, it -// generates diagnostic warning messages. -#define DRAKE_COMMON_AUTODIFF_HEADER - -#include -#include - -static_assert(EIGEN_VERSION_AT_LEAST(3, 3, 3), - "Drake requires Eigen >= v3.3.3."); - -// Do not alpha-sort the following block of hard-coded #includes, which is -// protected by `clang-format on/off`. -// -// Rationale: We want to maximize the use of this header, `autodiff.h`, even -// inside of the autodiff-related files to avoid any mistakes which might not be -// detected. By centralizing the list here, we make sure that everyone will see -// the correct order which respects the inter-dependencies of the autodiff -// headers. This shields us from triggering undefined behaviors due to -// order-of-specialization-includes-changed mistakes. -// -// clang-format off -#include "drake/common/eigen_autodiff_limits.h" -#include "drake/common/eigen_autodiff_types.h" -#include "drake/common/autodiffxd.h" -#include "drake/common/autodiff_overloads.h" -// clang-format on -#undef DRAKE_COMMON_AUTODIFF_HEADER diff --git a/wpimath/src/test/native/include/drake/common/autodiff_overloads.h b/wpimath/src/test/native/include/drake/common/autodiff_overloads.h deleted file mode 100644 index 7eaeb3f221..0000000000 --- a/wpimath/src/test/native/include/drake/common/autodiff_overloads.h +++ /dev/null @@ -1,203 +0,0 @@ -/// @file -/// Overloads for STL mathematical operations on AutoDiffScalar. -/// -/// Used via argument-dependent lookup (ADL). These functions appear -/// in the Eigen namespace so that ADL can automatically choose between -/// the STL version and the overloaded version to match the type of the -/// arguments. The proper use would be e.g. -/// -/// \code{.cc} -/// void mymethod() { -/// using std::isinf; -/// isinf(myval); -/// } -/// \endcode{} -/// -/// @note The if_then_else and cond functions for AutoDiffScalar are in -/// namespace drake because cond is defined in namespace drake in -/// "drake/common/cond.h" file. - -#pragma once - -#ifndef DRAKE_COMMON_AUTODIFF_HEADER -// TODO(soonho-tri): Change to #error. -#warning Do not directly include this file. Include "drake/common/autodiff.h". -#endif - -#include -#include - -#include "drake/common/cond.h" -#include "drake/common/drake_assert.h" -#include "drake/common/dummy_value.h" - -namespace Eigen { - -/// Overloads nexttoward to mimic std::nexttoward from . -template -double nexttoward(const Eigen::AutoDiffScalar& from, long double to) { - using std::nexttoward; - return nexttoward(from.value(), to); -} - -/// Overloads round to mimic std::round from . -template -double round(const Eigen::AutoDiffScalar& x) { - using std::round; - return round(x.value()); -} - -/// Overloads isinf to mimic std::isinf from . -template -bool isinf(const Eigen::AutoDiffScalar& x) { - using std::isinf; - return isinf(x.value()); -} - -/// Overloads isfinite to mimic std::isfinite from . -template -bool isfinite(const Eigen::AutoDiffScalar& x) { - using std::isfinite; - return isfinite(x.value()); -} - -/// Overloads isnan to mimic std::isnan from . -template -bool isnan(const Eigen::AutoDiffScalar& x) { - using std::isnan; - return isnan(x.value()); -} - -/// Overloads floor to mimic std::floor from . -template -double floor(const Eigen::AutoDiffScalar& x) { - using std::floor; - return floor(x.value()); -} - -/// Overloads ceil to mimic std::ceil from . -template -double ceil(const Eigen::AutoDiffScalar& x) { - using std::ceil; - return ceil(x.value()); -} - -/// Overloads copysign from . -template -Eigen::AutoDiffScalar copysign(const Eigen::AutoDiffScalar& x, - const T& y) { - using std::isnan; - if (isnan(x)) return (y >= 0) ? NAN : -NAN; - if ((x < 0 && y >= 0) || (x >= 0 && y < 0)) - return -x; - else - return x; -} - -/// Overloads copysign from . -template -double copysign(double x, const Eigen::AutoDiffScalar& y) { - using std::isnan; - if (isnan(x)) return (y >= 0) ? NAN : -NAN; - if ((x < 0 && y >= 0) || (x >= 0 && y < 0)) - return -x; - else - return x; -} - -/// Overloads pow for an AutoDiffScalar base and exponent, implementing the -/// chain rule. -template -Eigen::AutoDiffScalar< - typename internal::remove_all::type::PlainObject> -pow(const Eigen::AutoDiffScalar& base, - const Eigen::AutoDiffScalar& exponent) { - // The two AutoDiffScalars being exponentiated must have the same matrix - // type. This includes, but is not limited to, the same scalar type and - // the same dimension. - static_assert( - std::is_same< - typename internal::remove_all::type::PlainObject, - typename internal::remove_all::type::PlainObject>::value, - "The derivative types must match."); - - internal::make_coherent(base.derivatives(), exponent.derivatives()); - - const auto& x = base.value(); - const auto& xgrad = base.derivatives(); - const auto& y = exponent.value(); - const auto& ygrad = exponent.derivatives(); - - using std::pow; - using std::log; - const auto x_to_the_y = pow(x, y); - if (ygrad.isZero(std::numeric_limits::epsilon()) || - ygrad.size() == 0) { - // The derivative only depends on ∂(x^y)/∂x -- this prevents undefined - // behavior in the corner case where ∂(x^y)/∂y is infinite when x = 0, - // despite ∂y/∂v being 0. - return Eigen::MakeAutoDiffScalar(x_to_the_y, y * pow(x, y - 1) * xgrad); - } - return Eigen::MakeAutoDiffScalar( - // The value is x ^ y. - x_to_the_y, - // The multivariable chain rule states: - // df/dv_i = (∂f/∂x * dx/dv_i) + (∂f/∂y * dy/dv_i) - // ∂f/∂x is y*x^(y-1) - y * pow(x, y - 1) * xgrad + - // ∂f/∂y is (x^y)*ln(x) - x_to_the_y * log(x) * ygrad); -} - -} // namespace Eigen - -namespace drake { - -/// Returns the autodiff scalar's value() as a double. Never throws. -/// Overloads ExtractDoubleOrThrow from common/extract_double.h. -template -double ExtractDoubleOrThrow(const Eigen::AutoDiffScalar& scalar) { - return static_cast(scalar.value()); -} - -/// Specializes common/dummy_value.h. -template -struct dummy_value> { - static constexpr Eigen::AutoDiffScalar get() { - constexpr double kNaN = std::numeric_limits::quiet_NaN(); - DerType derivatives; - derivatives.fill(kNaN); - return Eigen::AutoDiffScalar(kNaN, derivatives); - } -}; - -/// Provides if-then-else expression for Eigen::AutoDiffScalar type. To support -/// Eigen's generic expressions, we use casting to the plain object after -/// applying Eigen::internal::remove_all. It is based on the Eigen's -/// implementation of min/max function for AutoDiffScalar type -/// (https://bitbucket.org/eigen/eigen/src/10a1de58614569c9250df88bdfc6402024687bc6/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h?at=default&fileviewer=file-view-default#AutoDiffScalar.h-546). -template -inline Eigen::AutoDiffScalar< - typename Eigen::internal::remove_all::type::PlainObject> -if_then_else(bool f_cond, const Eigen::AutoDiffScalar& x, - const Eigen::AutoDiffScalar& y) { - typedef Eigen::AutoDiffScalar< - typename Eigen::internal::remove_all::type::PlainObject> - ADS1; - typedef Eigen::AutoDiffScalar< - typename Eigen::internal::remove_all::type::PlainObject> - ADS2; - static_assert(std::is_same::value, - "The derivative types must match."); - return f_cond ? ADS1(x) : ADS2(y); -} - -/// Provides special case of cond expression for Eigen::AutoDiffScalar type. -template -Eigen::AutoDiffScalar< - typename Eigen::internal::remove_all::type::PlainObject> -cond(bool f_cond, const Eigen::AutoDiffScalar& e_then, Rest... rest) { - return if_then_else(f_cond, e_then, cond(rest...)); -} - -} // namespace drake diff --git a/wpimath/src/test/native/include/drake/common/autodiffxd.h b/wpimath/src/test/native/include/drake/common/autodiffxd.h deleted file mode 100644 index a99b2f0502..0000000000 --- a/wpimath/src/test/native/include/drake/common/autodiffxd.h +++ /dev/null @@ -1,423 +0,0 @@ -#pragma once - -// This file is a modification of Eigen-3.3.3's AutoDiffScalar.h file which is -// available at -// https://bitbucket.org/eigen/eigen/raw/67e894c6cd8f5f1f604b27d37ed47fdf012674ff/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h -// -// Copyright (C) 2009 Gael Guennebaud -// Copyright (C) 2017 Drake Authors -// -// 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 DRAKE_COMMON_AUTODIFF_HEADER -// TODO(soonho-tri): Change to #error. -#warning Do not directly include this file. Include "drake/common/autodiff.h". -#endif - -#include -#include - -#include - -namespace Eigen { - -#if !defined(DRAKE_DOXYGEN_CXX) -// Explicit template specializations of Eigen::AutoDiffScalar for VectorXd. -// -// AutoDiffScalar tries to call internal::make_coherent to promote empty -// derivatives. However, it fails to do the promotion when an operand is an -// expression tree (i.e. CwiseBinaryOp). Our solution is to provide special -// overloading for VectorXd and change the return types of its operators. With -// this change, the operators evaluate terms immediately and return an -// AutoDiffScalar instead of expression trees (such as CwiseBinaryOp). -// Eigen's implementation of internal::make_coherent makes use of const_cast in -// order to promote zero sized derivatives. This however interferes badly with -// our caching system and produces unexpected behaviors. See #10971 for details. -// Therefore our implementation stops using internal::make_coherent and treats -// scalars with zero sized derivatives as constants, as it should. -// -// We also provide overloading of math functions for AutoDiffScalar -// which return AutoDiffScalar instead of an expression tree. -// -// See https://github.com/RobotLocomotion/drake/issues/6944 for more -// information. See also drake/common/autodiff_overloads.h. -// -// TODO(soonho-tri): Next time when we upgrade Eigen, please check if we still -// need these specializations. -template <> -class AutoDiffScalar - : public internal::auto_diff_special_op { - public: - typedef internal::auto_diff_special_op Base; - typedef typename internal::remove_all::type DerType; - typedef typename internal::traits::Scalar Scalar; - typedef typename NumTraits::Real Real; - - using Base::operator+; - using Base::operator*; - - AutoDiffScalar() {} - - AutoDiffScalar(const Scalar& value, int nbDer, int derNumber) - : m_value(value), m_derivatives(DerType::Zero(nbDer)) { - m_derivatives.coeffRef(derNumber) = Scalar(1); - } - - // NOLINTNEXTLINE(runtime/explicit): Code from Eigen. - AutoDiffScalar(const Real& value) : m_value(value) { - if (m_derivatives.size() > 0) m_derivatives.setZero(); - } - - AutoDiffScalar(const Scalar& value, const DerType& der) - : m_value(value), m_derivatives(der) {} - - template - AutoDiffScalar( - const AutoDiffScalar& other -#ifndef EIGEN_PARSED_BY_DOXYGEN - , - typename internal::enable_if< - internal::is_same< - Scalar, typename internal::traits::type>::Scalar>::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 - inline AutoDiffScalar& operator=(const AutoDiffScalar& 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 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 - inline bool operator<(const AutoDiffScalar& b) const { - return m_value < b.value(); - } - template - inline bool operator<=(const AutoDiffScalar& b) const { - return m_value <= b.value(); - } - template - inline bool operator>(const AutoDiffScalar& b) const { - return m_value > b.value(); - } - template - inline bool operator>=(const AutoDiffScalar& b) const { - return m_value >= b.value(); - } - template - inline bool operator==(const AutoDiffScalar& b) const { - return m_value == b.value(); - } - template - inline bool operator!=(const AutoDiffScalar& b) const { - return m_value != b.value(); - } - - inline const AutoDiffScalar operator+(const Scalar& other) const { - return AutoDiffScalar(m_value + other, m_derivatives); - } - - friend inline const AutoDiffScalar operator+( - const Scalar& a, const AutoDiffScalar& b) { - return AutoDiffScalar(a + b.value(), b.derivatives()); - } - - inline AutoDiffScalar& operator+=(const Scalar& other) { - value() += other; - return *this; - } - - template - inline const AutoDiffScalar operator+( - const AutoDiffScalar& other) const { - const bool has_this_der = m_derivatives.size() > 0; - const bool has_both_der = has_this_der && (other.derivatives().size() > 0); - return MakeAutoDiffScalar( - m_value + other.value(), - has_both_der - ? VectorXd(m_derivatives + other.derivatives()) - : has_this_der ? m_derivatives : VectorXd(other.derivatives())); - } - - template - inline AutoDiffScalar& operator+=(const AutoDiffScalar& other) { - (*this) = (*this) + other; - return *this; - } - - inline const AutoDiffScalar operator-(const Scalar& b) const { - return AutoDiffScalar(m_value - b, m_derivatives); - } - - friend inline const AutoDiffScalar operator-( - const Scalar& a, const AutoDiffScalar& b) { - return AutoDiffScalar(a - b.value(), -b.derivatives()); - } - - inline AutoDiffScalar& operator-=(const Scalar& other) { - value() -= other; - return *this; - } - - template - inline const AutoDiffScalar operator-( - const AutoDiffScalar& other) const { - const bool has_this_der = m_derivatives.size() > 0; - const bool has_both_der = has_this_der && (other.derivatives().size() > 0); - return MakeAutoDiffScalar( - m_value - other.value(), - has_both_der - ? VectorXd(m_derivatives - other.derivatives()) - : has_this_der ? m_derivatives : VectorXd(-other.derivatives())); - } - - template - inline AutoDiffScalar& operator-=(const AutoDiffScalar& other) { - *this = *this - other; - return *this; - } - - inline const AutoDiffScalar operator-() const { - return AutoDiffScalar(-m_value, -m_derivatives); - } - - inline const AutoDiffScalar operator*(const Scalar& other) const { - return MakeAutoDiffScalar(m_value * other, m_derivatives * other); - } - - friend inline const AutoDiffScalar operator*( - const Scalar& other, const AutoDiffScalar& a) { - return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other); - } - - inline const AutoDiffScalar operator/(const Scalar& other) const { - return MakeAutoDiffScalar(m_value / other, - (m_derivatives * (Scalar(1) / other))); - } - - friend inline const AutoDiffScalar operator/( - const Scalar& other, const AutoDiffScalar& a) { - return MakeAutoDiffScalar( - other / a.value(), - a.derivatives() * (Scalar(-other) / (a.value() * a.value()))); - } - - template - inline const AutoDiffScalar operator/( - const AutoDiffScalar& other) const { - const auto& this_der = m_derivatives; - const auto& other_der = other.derivatives(); - const bool has_this_der = m_derivatives.size() > 0; - const bool has_both_der = has_this_der && (other.derivatives().size() > 0); - const double scale = 1. / (other.value() * other.value()); - return MakeAutoDiffScalar( - m_value / other.value(), - has_both_der ? - VectorXd(this_der * other.value() - other_der * m_value) * scale : - has_this_der ? - VectorXd(this_der * other.value()) * scale : - // has_other_der || has_neither - VectorXd(other_der * -m_value) * scale); - } - - template - inline const AutoDiffScalar operator*( - const AutoDiffScalar& other) const { - const bool has_this_der = m_derivatives.size() > 0; - const bool has_both_der = has_this_der && (other.derivatives().size() > 0); - return MakeAutoDiffScalar( - m_value * other.value(), - has_both_der ? VectorXd(m_derivatives * other.value() + - other.derivatives() * m_value) - : has_this_der ? VectorXd(m_derivatives * other.value()) - : VectorXd(other.derivatives() * m_value)); - } - - inline AutoDiffScalar& operator*=(const Scalar& other) { - *this = *this * other; - return *this; - } - - template - inline AutoDiffScalar& operator*=(const AutoDiffScalar& other) { - *this = *this * other; - return *this; - } - - inline AutoDiffScalar& operator/=(const Scalar& other) { - *this = *this / other; - return *this; - } - - template - inline AutoDiffScalar& operator/=(const AutoDiffScalar& other) { - *this = *this / other; - return *this; - } - - protected: - Scalar m_value; - DerType m_derivatives; -}; - -#define DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(FUNC, CODE) \ - inline const AutoDiffScalar FUNC( \ - const AutoDiffScalar& x) { \ - EIGEN_UNUSED typedef double Scalar; \ - CODE; \ - } - -DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( - abs, using std::abs; return Eigen::MakeAutoDiffScalar( - abs(x.value()), x.derivatives() * (x.value() < 0 ? -1 : 1));) - -DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( - abs2, using numext::abs2; return Eigen::MakeAutoDiffScalar( - abs2(x.value()), x.derivatives() * (Scalar(2) * x.value()));) - -DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( - sqrt, using std::sqrt; Scalar sqrtx = sqrt(x.value()); - return Eigen::MakeAutoDiffScalar(sqrtx, - x.derivatives() * (Scalar(0.5) / sqrtx));) - -DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( - cos, using std::cos; using std::sin; - return Eigen::MakeAutoDiffScalar(cos(x.value()), - x.derivatives() * (-sin(x.value())));) - -DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( - sin, using std::sin; using std::cos; - return Eigen::MakeAutoDiffScalar(sin(x.value()), - x.derivatives() * cos(x.value()));) - -DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( - exp, using std::exp; Scalar expx = exp(x.value()); - return Eigen::MakeAutoDiffScalar(expx, x.derivatives() * expx);) - -DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( - log, using std::log; return Eigen::MakeAutoDiffScalar( - log(x.value()), x.derivatives() * (Scalar(1) / x.value()));) - -DRAKE_EIGEN_AUTODIFFXD_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()))));) - -DRAKE_EIGEN_AUTODIFFXD_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()))));) - -DRAKE_EIGEN_AUTODIFFXD_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()))));) - -DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( - atan, using std::atan; return Eigen::MakeAutoDiffScalar( - atan(x.value()), - x.derivatives() * (Scalar(1) / (1 + x.value() * x.value())));) - -DRAKE_EIGEN_AUTODIFFXD_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()))));) - -DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( - sinh, using std::sinh; using std::cosh; - return Eigen::MakeAutoDiffScalar(sinh(x.value()), - x.derivatives() * cosh(x.value()));) - -DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY( - cosh, using std::sinh; using std::cosh; - return Eigen::MakeAutoDiffScalar(cosh(x.value()), - x.derivatives() * sinh(x.value()));) - -#undef DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY - -// We have this specialization here because the Eigen-3.3.3's atan2 -// implementation for AutoDiffScalar does not make a return with properly sized -// derivatives. -inline const AutoDiffScalar atan2(const AutoDiffScalar& a, - const AutoDiffScalar& b) { - const bool has_a_der = a.derivatives().size() > 0; - const bool has_both_der = has_a_der && (b.derivatives().size() > 0); - const double squared_hypot = a.value() * a.value() + b.value() * b.value(); - return MakeAutoDiffScalar( - std::atan2(a.value(), b.value()), - VectorXd((has_both_der - ? VectorXd(a.derivatives() * b.value() - - a.value() * b.derivatives()) - : has_a_der ? VectorXd(a.derivatives() * b.value()) - : VectorXd(-a.value() * b.derivatives())) / - squared_hypot)); -} - -inline const AutoDiffScalar pow(const AutoDiffScalar& a, - double b) { - using std::pow; - return MakeAutoDiffScalar(pow(a.value(), b), - a.derivatives() * (b * pow(a.value(), b - 1))); -} - -#endif - -} // namespace Eigen diff --git a/wpimath/src/test/native/include/drake/common/cond.h b/wpimath/src/test/native/include/drake/common/cond.h deleted file mode 100644 index 16dd21ed5f..0000000000 --- a/wpimath/src/test/native/include/drake/common/cond.h +++ /dev/null @@ -1,44 +0,0 @@ -#pragma once - -#include -#include - -#include "drake/common/double_overloads.h" - -namespace drake { -/** @name cond - Constructs conditional expression (similar to Lisp's cond). - - @verbatim - cond(cond_1, expr_1, - cond_2, expr_2, - ..., ..., - cond_n, expr_n, - expr_{n+1}) - @endverbatim - - The value returned by the above cond expression is @c expr_1 if @c cond_1 is - true; else if @c cond_2 is true then @c expr_2; ... ; else if @c cond_n is - true then @c expr_n. If none of the conditions are true, it returns @c - expr_{n+1}. - - @note This functions assumes that @p ScalarType provides @c operator< and the - type of @c f_cond is the type of the return type of operator<(ScalarType, - ScalarType). For example, @c symbolic::Expression can be used as a @p - ScalarType because it provides symbolic::Formula - operator<(symbolic::Expression, symbolic::Expression). - - - @{ - */ -template -ScalarType cond(const ScalarType& e) { - return e; -} -template -ScalarType cond(const decltype(ScalarType() < ScalarType()) & f_cond, - const ScalarType& e_then, Rest... rest) { - return if_then_else(f_cond, e_then, cond(rest...)); -} -///@} -} // namespace drake diff --git a/wpimath/src/test/native/include/drake/common/constants.h b/wpimath/src/test/native/include/drake/common/constants.h deleted file mode 100644 index 0ccddca66a..0000000000 --- a/wpimath/src/test/native/include/drake/common/constants.h +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -namespace drake { - -constexpr int kQuaternionSize = 4; - -constexpr int kSpaceDimension = 3; - -constexpr int kRpySize = 3; - -/// https://en.wikipedia.org/wiki/Screw_theory#Twist -constexpr int kTwistSize = 6; - -/// http://www.euclideanspace.com/maths/geometry/affine/matrix4x4/ -constexpr int kHomogeneousTransformSize = 16; - -const int kRotmatSize = kSpaceDimension * kSpaceDimension; - -} // namespace drake diff --git a/wpimath/src/test/native/include/drake/common/double_overloads.h b/wpimath/src/test/native/include/drake/common/double_overloads.h deleted file mode 100644 index 125f11362e..0000000000 --- a/wpimath/src/test/native/include/drake/common/double_overloads.h +++ /dev/null @@ -1,21 +0,0 @@ -/// @file -/// Provides necessary operations on double to have it as a ScalarType in drake. - -#pragma once - -namespace drake { -/// Provides if-then-else expression for double. The value returned by the -/// if-then-else expression is @p v_then if @p f_cond is @c true. Otherwise, it -/// returns @p v_else. - -/// The semantics is similar but not exactly the same as C++'s conditional -/// expression constructed by its ternary operator, @c ?:. In -/// if_then_else(f_cond, v_then, v_else), both of @p v_then and @p -/// v_else are evaluated regardless of the evaluation of @p f_cond. In contrast, -/// only one of @p v_then or @p v_else is evaluated in C++'s conditional -/// expression f_cond ? v_then : v_else. -inline double if_then_else(bool f_cond, double v_then, double v_else) { - return f_cond ? v_then : v_else; -} - -} // namespace drake diff --git a/wpimath/src/test/native/include/drake/common/drake_deprecated.h b/wpimath/src/test/native/include/drake/common/drake_deprecated.h deleted file mode 100644 index 5ce6328d99..0000000000 --- a/wpimath/src/test/native/include/drake/common/drake_deprecated.h +++ /dev/null @@ -1,65 +0,0 @@ -#pragma once - -/** @file -Provides a portable macro for use in generating compile-time warnings for -use of code that is permitted but discouraged. */ - -#ifdef DRAKE_DOXYGEN_CXX -/** Use `DRAKE_DEPRECATED("removal_date", "message")` to discourage use of -certain APIs. It can be used on classes, typedefs, variables, non-static data -members, functions, arguments, enumerations, and template specializations. When -code refers to the deprecated item, a compile time warning will be issued -displaying the given message, preceded by "DRAKE DEPRECATED: ". The Doxygen API -reference will show that the API is deprecated, along with the message. - -This is typically used for constructs that have been replaced by something -better and it is good practice to suggest the appropriate replacement in the -deprecation message. Deprecation warnings are conventionally used to convey to -users that a feature they are depending on will be removed in a future release. - -Every deprecation notice must include a date (as YYYY-MM-DD string) where the -deprecated item is planned for removal. Future commits may change the date -(e.g., delaying the removal) but should generally always reflect the best -current expectation for removal. - -Absent any other particular need, we prefer to use a deprecation period of -three months by default, often rounded up to the next first of the month. So -for code announced as deprecated on 2018-01-22 the removal_date would nominally -be set to 2018-05-01. - -Try to keep the date string immediately after the DRAKE_DEPRECATED macro name, -even if the message itself must be wrapped to a new line: -@code - DRAKE_DEPRECATED("2038-01-19", - "foo is being replaced with a safer, const-method named bar().") - int foo(); -@endcode - -Sample uses: @code - // Attribute comes *before* declaration of a deprecated function or variable; - // no semicolon is allowed. - DRAKE_DEPRECATED("2038-01-19", "f() is slow; use g() instead.") - int f(int arg); - - // Attribute comes *after* struct, class, enum keyword. - class DRAKE_DEPRECATED("2038-01-19", "Use MyNewClass instead.") - MyClass { - }; - - // Type alias goes before the '='. - using NewType - DRAKE_DEPRECATED("2038-01-19", "Use NewType instead.") - = OldType; -@endcode -*/ -#define DRAKE_DEPRECATED(removal_date, message) - -#else // DRAKE_DOXYGEN_CXX - -#define DRAKE_DEPRECATED(removal_date, message) \ - [[deprecated( \ - "\nDRAKE DEPRECATED: " message \ - "\nThe deprecated code will be removed from Drake" \ - " on or after " removal_date ".")]] - -#endif // DRAKE_DOXYGEN_CXX diff --git a/wpimath/src/test/native/include/drake/common/drake_nodiscard.h b/wpimath/src/test/native/include/drake/common/drake_nodiscard.h deleted file mode 100644 index 29f078dd22..0000000000 --- a/wpimath/src/test/native/include/drake/common/drake_nodiscard.h +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once - -// TODO(jwnimmer-tri) Once we are in --std=c++17 mode as our minimum version, -// we can remove this file and just say [[nodiscard]] directly everywhere. - -#if defined(DRAKE_DOXYGEN_CXX) || __has_cpp_attribute(nodiscard) -/** Synonym for [[nodiscard]], iff the current compiler supports it; -see https://en.cppreference.com/w/cpp/language/attributes/nodiscard. */ -// NOLINTNEXTLINE(whitespace/braces) -#define DRAKE_NODISCARD [[nodiscard]] -#else -#define DRAKE_NODISCARD -#endif diff --git a/wpimath/src/test/native/include/drake/common/dummy_value.h b/wpimath/src/test/native/include/drake/common/dummy_value.h deleted file mode 100644 index b9c616a316..0000000000 --- a/wpimath/src/test/native/include/drake/common/dummy_value.h +++ /dev/null @@ -1,35 +0,0 @@ -#pragma once - -#include - -namespace drake { - -/// Provides a "dummy" value for a ScalarType -- a value that is unlikely to be -/// mistaken for a purposefully-computed value, useful for initializing a value -/// before the true result is available. -/// -/// Defaults to using std::numeric_limits::quiet_NaN when available; it is a -/// compile-time error to call the unspecialized dummy_value::get() when -/// quiet_NaN is unavailable. -/// -/// See autodiff_overloads.h to use this with Eigen's AutoDiffScalar. -template -struct dummy_value { - static constexpr T get() { - static_assert(std::numeric_limits::has_quiet_NaN, - "Custom scalar types should specialize this struct"); - return std::numeric_limits::quiet_NaN(); - } -}; - -template <> -struct dummy_value { - static constexpr int get() { - // D is for "Dummy". We assume as least 32 bits (per cppguide) -- if `int` - // is larger than 32 bits, this will leave some fraction of the bytes zero - // instead of 0xDD, but that's okay. - return 0xDDDDDDDD; - } -}; - -} // namespace drake diff --git a/wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h b/wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h deleted file mode 100644 index 49175cee30..0000000000 --- a/wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once - -#ifndef DRAKE_COMMON_AUTODIFF_HEADER -// TODO(soonho-tri): Change to #error. -#warning Do not directly include this file. Include "drake/common/autodiff.h". -#endif - -#include - -// Eigen provides `numeric_limits>` starting with v3.3.4. -#if !EIGEN_VERSION_AT_LEAST(3, 3, 4) // Eigen Version < v3.3.4 - -namespace std { -template -class numeric_limits> - : public numeric_limits {}; - -} // namespace std - -#endif // Eigen Version < v3.3.4 diff --git a/wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h b/wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h deleted file mode 100644 index 10ffec6987..0000000000 --- a/wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h +++ /dev/null @@ -1,38 +0,0 @@ -#pragma once - -/// @file -/// This file contains abbreviated definitions for certain uses of -/// AutoDiffScalar that are commonly used in Drake. -/// @see also eigen_types.h - -#ifndef DRAKE_COMMON_AUTODIFF_HEADER -// TODO(soonho-tri): Change to #error. -#warning Do not directly include this file. Include "drake/common/autodiff.h". -#endif - -#include - -#include - -#include "drake/common/eigen_types.h" - -namespace drake { - -/// An autodiff variable with a dynamic number of partials. -using AutoDiffXd = Eigen::AutoDiffScalar; - -// TODO(hongkai-dai): Recursive template to get arbitrary gradient order. - -/// An autodiff variable with `num_vars` partials. -template -using AutoDiffd = Eigen::AutoDiffScalar >; - -/// A vector of `rows` autodiff variables, each with `num_vars` partials. -template -using AutoDiffVecd = Eigen::Matrix, rows, 1>; - -/// A dynamic-sized vector of autodiff variables, each with a dynamic-sized -/// vector of partials. -typedef AutoDiffVecd AutoDiffVecXd; - -} // namespace drake diff --git a/wpimath/src/test/native/include/drake/common/eigen_types.h b/wpimath/src/test/native/include/drake/common/eigen_types.h deleted file mode 100644 index abe3e0bb27..0000000000 --- a/wpimath/src/test/native/include/drake/common/eigen_types.h +++ /dev/null @@ -1,461 +0,0 @@ -#pragma once - -/// @file -/// This file contains abbreviated definitions for certain specializations of -/// Eigen::Matrix that are commonly used in Drake. -/// These convenient definitions are templated on the scalar type of the Eigen -/// object. While Drake uses `` for scalar types across the entire code base -/// we decided in this file to use `` to be more consistent with the -/// usage of `` in Eigen's code base. -/// @see also eigen_autodiff_types.h - -#include - -#include - -#include "drake/common/constants.h" -#include "drake/common/drake_assert.h" -#include "drake/common/drake_copyable.h" -#include "drake/common/drake_deprecated.h" - -namespace drake { - -/// The empty column vector (zero rows, one column), templated on scalar type. -template -using Vector0 = Eigen::Matrix; - -/// A column vector of size 1 (that is, a scalar), templated on scalar type. -template -using Vector1 = Eigen::Matrix; - -/// A column vector of size 1 of doubles. -using Vector1d = Eigen::Matrix; - -/// A column vector of size 2, templated on scalar type. -template -using Vector2 = Eigen::Matrix; - -/// A column vector of size 3, templated on scalar type. -template -using Vector3 = Eigen::Matrix; - -/// A column vector of size 4, templated on scalar type. -template -using Vector4 = Eigen::Matrix; - -/// A column vector of size 6. -template -using Vector6 = Eigen::Matrix; - -/// A column vector templated on the number of rows. -template -using Vector = Eigen::Matrix; - -/// A column vector of any size, templated on scalar type. -template -using VectorX = Eigen::Matrix; - -/// A vector of dynamic size templated on scalar type, up to a maximum of 6 -/// elements. -template -using VectorUpTo6 = Eigen::Matrix; - -/// A row vector of size 2, templated on scalar type. -template -using RowVector2 = Eigen::Matrix; - -/// A row vector of size 3, templated on scalar type. -template -using RowVector3 = Eigen::Matrix; - -/// A row vector of size 4, templated on scalar type. -template -using RowVector4 = Eigen::Matrix; - -/// A row vector of size 6. -template -using RowVector6 = Eigen::Matrix; - -/// A row vector templated on the number of columns. -template -using RowVector = Eigen::Matrix; - -/// A row vector of any size, templated on scalar type. -template -using RowVectorX = Eigen::Matrix; - - -/// A matrix of 2 rows and 2 columns, templated on scalar type. -template -using Matrix2 = Eigen::Matrix; - -/// A matrix of 3 rows and 3 columns, templated on scalar type. -template -using Matrix3 = Eigen::Matrix; - -/// A matrix of 4 rows and 4 columns, templated on scalar type. -template -using Matrix4 = Eigen::Matrix; - -/// A matrix of 6 rows and 6 columns, templated on scalar type. -template -using Matrix6 = Eigen::Matrix; - -/// A matrix of 2 rows, dynamic columns, templated on scalar type. -template -using Matrix2X = Eigen::Matrix; - -/// A matrix of 3 rows, dynamic columns, templated on scalar type. -template -using Matrix3X = Eigen::Matrix; - -/// A matrix of 4 rows, dynamic columns, templated on scalar type. -template -using Matrix4X = Eigen::Matrix; - -/// A matrix of 6 rows, dynamic columns, templated on scalar type. -template -using Matrix6X = Eigen::Matrix; - -/// A matrix of dynamic size, templated on scalar type. -template -using MatrixX = Eigen::Matrix; - -/// A matrix of dynamic size templated on scalar type, up to a maximum of 6 rows -/// and 6 columns. Rectangular matrices, with different number of rows and -/// columns, are allowed. -template -using MatrixUpTo6 = -Eigen::Matrix; - -/// A quaternion templated on scalar type. -template -using Quaternion = Eigen::Quaternion; - -/// An AngleAxis templated on scalar type. -template -using AngleAxis = Eigen::AngleAxis; - -/// An Isometry templated on scalar type. -template -using Isometry3 = Eigen::Transform; - -/// A translation in 3D templated on scalar type. -template -using Translation3 = Eigen::Translation; - -/// A column vector consisting of one twist. -template -using TwistVector = Eigen::Matrix; - -/// A matrix with one twist per column, and dynamically many columns. -template -using TwistMatrix = Eigen::Matrix; - -/// A six-by-six matrix. -template -using SquareTwistMatrix = Eigen::Matrix; - -/// A column vector consisting of one wrench (spatial force) = `[r X f; f]`, -/// where f is a force (translational force) applied at a point `P` and `r` is -/// the position vector from a point `O` (called the "moment center") to point -/// `P`. -template -using WrenchVector = Eigen::Matrix; - -/// A column vector consisting of a concatenated rotational and translational -/// force. The wrench is a special case of a SpatialForce. For a general -/// SpatialForce the rotational force can be a pure torque or the accumulation -/// of moments and need not necessarily be a function of the force term. -template -using SpatialForce -DRAKE_DEPRECATED("2019-10-15", "Please use Vector6<> instead.") - = Eigen::Matrix; - -/// EigenSizeMinPreferDynamic::value gives the min between compile-time -/// sizes @p a and @p b. 0 has absolute priority, followed by 1, followed by -/// Dynamic, followed by other finite values. -/// -/// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_DYNAMIC -/// macro in "Eigen/Core/util/Macros.h". -template -struct EigenSizeMinPreferDynamic { - // clang-format off - static constexpr int value = (a == 0 || b == 0) ? 0 : - (a == 1 || b == 1) ? 1 : - (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic : - a <= b ? a : b; - // clang-format on -}; - -/// EigenSizeMinPreferFixed is a variant of EigenSizeMinPreferDynamic. The -/// difference is that finite values now have priority over Dynamic, so that -/// EigenSizeMinPreferFixed<3, Dynamic>::value gives 3. -/// -/// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_FIXED macro -/// in "Eigen/Core/util/Macros.h". -template -struct EigenSizeMinPreferFixed { - // clang-format off - static constexpr int value = (a == 0 || b == 0) ? 0 : - (a == 1 || b == 1) ? 1 : - (a == Eigen::Dynamic && b == Eigen::Dynamic) ? Eigen::Dynamic : - (a == Eigen::Dynamic) ? b : - (b == Eigen::Dynamic) ? a : - a <= b ? a : b; - // clang-format on -}; - -/// MultiplyEigenSizes gives a * b if both of a and b are fixed -/// sizes. Otherwise it gives Eigen::Dynamic. -template -struct MultiplyEigenSizes { - static constexpr int value = - (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic : a * b; -}; - -/* - * Determines if a type is derived from EigenBase<> (e.g. ArrayBase<>, - * MatrixBase<>). - */ -template -struct is_eigen_type : std::is_base_of, Derived> {}; - -/* - * Determines if an EigenBase<> has a specific scalar type. - */ -template -struct is_eigen_scalar_same - : std::integral_constant< - bool, is_eigen_type::value && - std::is_same::value> {}; - -/* - * Determines if an EigenBase<> type is a compile-time (column) vector. - * This will not check for run-time size. - */ -template -struct is_eigen_vector - : std::integral_constant::value && - Derived::ColsAtCompileTime == 1> {}; - -/* - * Determines if an EigenBase<> type is a compile-time (column) vector of a - * scalar type. This will not check for run-time size. - */ -template -struct is_eigen_vector_of - : std::integral_constant< - bool, is_eigen_scalar_same::value && - is_eigen_vector::value> {}; - -// TODO(eric.cousineau): A 1x1 matrix will be disqualified in this case, and -// this logic will qualify it as a vector. Address the downstream logic if this -// becomes an issue. -/* - * Determines if a EigenBase<> type is a compile-time non-column-vector matrix - * of a scalar type. This will not check for run-time size. - * @note For an EigenBase<> of the correct Scalar type, this logic is - * exclusive to is_eigen_vector_of<> such that distinct specializations are not - * ambiguous. - */ -template -struct is_eigen_nonvector_of - : std::integral_constant< - bool, is_eigen_scalar_same::value && - !is_eigen_vector::value> {}; - -// TODO(eric.cousineau): Add alias is_eigen_matrix_of = is_eigen_scalar_same if -// appropriate. - -/// This wrapper class provides a way to write non-template functions taking raw -/// pointers to Eigen objects as parameters while limiting the number of copies, -/// similar to `Eigen::Ref`. Internally, it keeps an instance of `Eigen::Ref` -/// and provides access to it via `operator*` and `operator->`. -/// -/// The motivation of this class is to follow GSG's -/// "output arguments should be pointers" rule while taking advantage of -/// using `Eigen::Ref`. Here is an example. -/// -/// @code -/// // This function is taking an Eigen::Ref of a matrix and modifies it in -/// // the body. This violates GSG's rule on output parameters. -/// void foo(Eigen::Ref M) { -/// M(0, 0) = 0; -/// } -/// // At Call-site, we have: -/// foo(M); -/// foo(M.block(0, 0, 2, 2)); -/// -/// // We can rewrite the above function into the following using EigenPtr. -/// void foo(EigenPtr M) { -/// (*M)(0, 0) = 0; -/// } -/// // Note that, call sites should be changed to: -/// foo(&M); -/// -/// // We need tmp to avoid taking the address of a temporary object such as the -/// // return value of .block(). -/// auto tmp = M.block(0, 0, 2, 2); -/// foo(&tmp); -/// @endcode -/// -/// Notice that methods taking an EigenPtr can mutate the entries of a matrix as -/// in method `foo()` in the example code above, but cannot change its size. -/// This is because `operator*` and `operator->` return an `Eigen::Ref` -/// object and only plain matrices/arrays can be resized and not expressions. -/// This **is** the desired behavior, since resizing the block of a matrix or -/// even a more general expression should not be allowed. If you do want to be -/// able to resize a mutable matrix argument, then you must pass it as a -/// `Matrix*`, like so: -/// @code -/// void bar(Eigen::MatrixXd* M) { -/// DRAKE_THROW_UNLESS(M != nullptr); -/// // In this case this method only works with 4x3 matrices. -/// if (M->rows() != 4 && M->cols() != 3) { -/// M->resize(4, 3); -/// } -/// (*M)(0, 0) = 0; -/// } -/// @endcode -/// -/// @note This class provides a way to avoid the `const_cast` hack introduced in -/// Eigen's -/// documentation. -template -class EigenPtr { - public: - typedef Eigen::Ref RefType; - - EigenPtr() : EigenPtr(nullptr) {} - - /// Overload for `nullptr`. - // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable. - EigenPtr(std::nullptr_t) {} - - /// Constructs with a reference to the given matrix type. - // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable. - EigenPtr(const EigenPtr& other) { assign(other); } - - /// Constructs with a reference to another matrix type. - /// May be `nullptr`. - template - // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable. - EigenPtr(PlainObjectTypeIn* m) { - if (m) { - m_.set_value(m); - } - } - - /// Constructs from another EigenPtr. - template - // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable. - EigenPtr(const EigenPtr& other) { - // Cannot directly construct `m_` from `other.m_`. - assign(other); - } - - EigenPtr& operator=(const EigenPtr& other) { - // We must explicitly override this version of operator=. - // The template below will not take precedence over this one. - return assign(other); - } - - template - EigenPtr& operator=(const EigenPtr& other) { - return assign(other); - } - - /// @throws std::runtime_error if this is a null dereference. - RefType& operator*() const { return get_reference(); } - - /// @throws std::runtime_error if this is a null dereference. - RefType* operator->() const { return &get_reference(); } - - /// Returns whether or not this contains a valid reference. - operator bool() const { return is_valid(); } - - bool operator==(std::nullptr_t) const { return !is_valid(); } - - bool operator!=(std::nullptr_t) const { return is_valid(); } - - private: - // Simple reassignable container without requirement of heap allocation. - // This is used because `drake::optional<>` does not work with `Eigen::Ref<>` - // because `Ref` deletes the necessary `operator=` overload for - // `std::is_copy_assignable`. - class ReassignableRef { - public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ReassignableRef) - ReassignableRef() {} - ~ReassignableRef() { - reset(); - } - - // Reset value to null. - void reset() { - if (has_value_) { - raw_value().~RefType(); - has_value_ = false; - } - } - - // Set value. - template - void set_value(PlainObjectTypeIn* value_in) { - if (has_value_) { - raw_value().~RefType(); - } - new (&raw_value()) RefType(*value_in); - has_value_ = true; - } - - // Access to value. - RefType& value() { - DRAKE_ASSERT(has_value()); - return raw_value(); - } - - // Indicates if it has a value. - bool has_value() const { return has_value_; } - - private: - // Unsafe access to value. - RefType& raw_value() { return reinterpret_cast(storage_); } - - bool has_value_{}; - typename std::aligned_storage::type - storage_; - }; - - // Use mutable, reassignable ref to permit pointer-like semantics (with - // ownership) on the stack. - mutable ReassignableRef m_; - - // Consolidate assignment here, so that both the copy constructor and the - // construction from another type may be used. - template - EigenPtr& assign(const EigenPtr& other) { - if (other) { - m_.set_value(&(*other)); - } else { - m_.reset(); - } - return *this; - } - - // Consolidate getting a reference here. - RefType& get_reference() const { - if (!m_.has_value()) - throw std::runtime_error("EigenPtr: nullptr dereference"); - return m_.value(); - } - - bool is_valid() const { - return m_.has_value(); - } -}; - -} // namespace drake diff --git a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h b/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h index 8847d87daa..d6bcbb8ec9 100644 --- a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h +++ b/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h @@ -7,7 +7,7 @@ #include #include -#include "drake/common/drake_nodiscard.h" +// #include "drake/common/text_logging.h" namespace drake { @@ -28,7 +28,7 @@ enum class MatrixCompareType { absolute, relative }; * @return true if the two matrices are equal based on the specified tolerance. */ template -DRAKE_NODISCARD ::testing::AssertionResult CompareMatrices( +[[nodiscard]] ::testing::AssertionResult CompareMatrices( const Eigen::MatrixBase& m1, const Eigen::MatrixBase& m2, double tolerance = 0.0, MatrixCompareType compare_type = MatrixCompareType::absolute) { diff --git a/wpimath/src/test/native/include/drake/common/unused.h b/wpimath/src/test/native/include/drake/common/unused.h deleted file mode 100644 index 5a28b01627..0000000000 --- a/wpimath/src/test/native/include/drake/common/unused.h +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once - -namespace drake { - -/// Documents the argument(s) as unused, placating GCC's -Wunused-parameter -/// warning. This can be called within function bodies to mark that certain -/// parameters are unused. -/// -/// When possible, removing the unused parameter is better than placating the -/// warning. However, in some cases the parameter is part of a virtual API or -/// template concept that is used elsewhere, so we can't remove it. In those -/// cases, this function might be an appropriate work-around. -/// -/// Here's rough advice on how to fix Wunused-parameter warnings: -/// -/// (1) If the parameter can be removed entirely, prefer that as the first -/// choice. (This may not be possible if, e.g., a method must match some -/// virtual API or template concept.) -/// -/// (2) Unless the parameter name has acute value, prefer to omit the name of -/// the parameter, leaving only the type, e.g. -/// @code -/// void Print(const State& state) override { /* No state to print. */ } -/// @endcode -/// changes to -/// @code -/// void Print(const State&) override { /* No state to print. */} -/// @endcode -/// This no longer triggers the warning and further makes it clear that a -/// parameter required by the API is definitively unused in the function. -/// -/// This is an especially good solution in the context of method -/// definitions (vs declarations); the parameter name used in a definition -/// is entirely irrelevant to Doxygen and most readers. -/// -/// (3) When leaving the parameter name intact has acute value, it is -/// acceptable to keep the name and mark it `unused`. For example, when -/// the name appears as part of a virtual method's base class declaration, -/// the name is used by Doxygen to document the method, e.g., -/// @code -/// /** Sets the default State of a System. This default implementation is to -/// set all zeros. Subclasses may override to use non-zero defaults. The -/// custom defaults may be based on the given @p context, when relevant. */ -/// virtual void SetDefault(const Context& context, State* state) const { -/// unused(context); -/// state->SetZero(); -/// } -/// @endcode -/// -template -void unused(const Args& ...) {} - -} // namespace drake diff --git a/wpimath/src/test/native/include/drake/math/autodiff.h b/wpimath/src/test/native/include/drake/math/autodiff.h deleted file mode 100644 index 52edb11f87..0000000000 --- a/wpimath/src/test/native/include/drake/math/autodiff.h +++ /dev/null @@ -1,332 +0,0 @@ -/// @file -/// Utilities for arithmetic on AutoDiffScalar. - -// TODO(russt): rename methods to be GSG compliant. - -#pragma once - -#include -#include - -#include - -#include "drake/common/autodiff.h" -#include "drake/common/unused.h" - -namespace drake { -namespace math { - -template -struct AutoDiffToValueMatrix { - typedef typename Eigen::Matrix type; -}; - -template -typename AutoDiffToValueMatrix::type autoDiffToValueMatrix( - const Eigen::MatrixBase& auto_diff_matrix) { - typename AutoDiffToValueMatrix::type ret(auto_diff_matrix.rows(), - auto_diff_matrix.cols()); - for (int i = 0; i < auto_diff_matrix.rows(); i++) { - for (int j = 0; j < auto_diff_matrix.cols(); ++j) { - ret(i, j) = auto_diff_matrix(i, j).value(); - } - } - return ret; -} - -/** `B = DiscardGradient(A)` enables casting from a matrix of AutoDiffScalars - * to AutoDiffScalar::Scalar type, explicitly throwing away any gradient - * information. For a matrix of type, e.g. `MatrixX A`, the - * comparable operation - * `B = A.cast()` - * should (and does) fail to compile. Use `DiscardGradient(A)` if you want to - * force the cast (and explicitly declare that information is lost). - * - * This method is overloaded to permit the user to call it for double types and - * AutoDiffScalar types (to avoid the calling function having to handle the - * two cases differently). - * - * @see DiscardZeroGradient - */ -template -typename std::enable_if< - !std::is_same::value, - Eigen::Matrix>::type -DiscardGradient(const Eigen::MatrixBase& auto_diff_matrix) { - return autoDiffToValueMatrix(auto_diff_matrix); -} - -/// @see DiscardGradient(). -template -typename std::enable_if< - std::is_same::value, - const Eigen::MatrixBase&>::type -DiscardGradient(const Eigen::MatrixBase& matrix) { - return matrix; -} - -/// @see DiscardGradient(). -template -typename std::enable_if< - !std::is_same<_Scalar, double>::value, - Eigen::Transform>::type -DiscardGradient(const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& - auto_diff_transform) { - return Eigen::Transform( - autoDiffToValueMatrix(auto_diff_transform.matrix())); -} - -/// @see DiscardGradient(). -template -typename std::enable_if::value, - const Eigen::Transform<_Scalar, _Dim, _Mode, - _Options>&>::type -DiscardGradient( - const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& transform) { - return transform; -} - - -/** \brief Initialize a single autodiff matrix given the corresponding value - *matrix. - * - * Set the values of \p auto_diff_matrix to be equal to \p val, and for each - *element i of \p auto_diff_matrix, - * resize the derivatives vector to \p num_derivatives, and set derivative - *number \p deriv_num_start + i to one (all other elements of the derivative - *vector set to zero). - * - * \param[in] mat 'regular' matrix of values - * \param[out] ret AutoDiff matrix - * \param[in] num_derivatives the size of the derivatives vector @default the - *size of mat - * \param[in] deriv_num_start starting index into derivative vector (i.e. - *element deriv_num_start in derivative vector corresponds to mat(0, 0)). - *@default 0 - */ -template -void initializeAutoDiff(const Eigen::MatrixBase& val, - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - Eigen::MatrixBase& auto_diff_matrix, - Eigen::DenseIndex num_derivatives = Eigen::Dynamic, - Eigen::DenseIndex deriv_num_start = 0) { - using ADScalar = typename DerivedAutoDiff::Scalar; - static_assert(static_cast(Derived::RowsAtCompileTime) == - static_cast(DerivedAutoDiff::RowsAtCompileTime), - "auto diff matrix has wrong number of rows at compile time"); - static_assert(static_cast(Derived::ColsAtCompileTime) == - static_cast(DerivedAutoDiff::ColsAtCompileTime), - "auto diff matrix has wrong number of columns at compile time"); - - if (num_derivatives == Eigen::Dynamic) num_derivatives = val.size(); - - auto_diff_matrix.resize(val.rows(), val.cols()); - Eigen::DenseIndex deriv_num = deriv_num_start; - for (Eigen::DenseIndex i = 0; i < val.size(); i++) { - auto_diff_matrix(i) = ADScalar(val(i), num_derivatives, deriv_num++); - } -} - -/** \brief The appropriate AutoDiffScalar gradient type given the value type and - * the number of derivatives at compile time - */ -template -using AutoDiffMatrixType = Eigen::Matrix< - Eigen::AutoDiffScalar>, - Derived::RowsAtCompileTime, Derived::ColsAtCompileTime, 0, - Derived::MaxRowsAtCompileTime, Derived::MaxColsAtCompileTime>; - -/** \brief Initialize a single autodiff matrix given the corresponding value - *matrix. - * - * Create autodiff matrix that matches \p mat in size with derivatives of - *compile time size \p Nq and runtime size \p num_derivatives. - * Set its values to be equal to \p val, and for each element i of \p - *auto_diff_matrix, set derivative number \p deriv_num_start + i to one (all - *other derivatives set to zero). - * - * \param[in] mat 'regular' matrix of values - * \param[in] num_derivatives the size of the derivatives vector @default the - *size of mat - * \param[in] deriv_num_start starting index into derivative vector (i.e. - *element deriv_num_start in derivative vector corresponds to mat(0, 0)). - *@default 0 - * \return AutoDiff matrix - */ -template -AutoDiffMatrixType initializeAutoDiff( - const Eigen::MatrixBase& mat, - Eigen::DenseIndex num_derivatives = -1, - Eigen::DenseIndex deriv_num_start = 0) { - if (num_derivatives == -1) num_derivatives = mat.size(); - - AutoDiffMatrixType ret(mat.rows(), mat.cols()); - initializeAutoDiff(mat, ret, num_derivatives, deriv_num_start); - return ret; -} - -namespace internal { -template -struct ResizeDerivativesToMatchScalarImpl { - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - static void run(Eigen::MatrixBase&, const Scalar&) {} -}; - -template -struct ResizeDerivativesToMatchScalarImpl> { - using Scalar = Eigen::AutoDiffScalar; - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - static void run(Eigen::MatrixBase& mat, const Scalar& scalar) { - for (int i = 0; i < mat.size(); i++) { - auto& derivs = mat(i).derivatives(); - if (derivs.size() == 0) { - derivs.resize(scalar.derivatives().size()); - derivs.setZero(); - } - } - } -}; -} // namespace internal - -/** Resize derivatives vector of each element of a matrix to to match the size - * of the derivatives vector of a given scalar. - * \brief If the mat and scalar inputs are AutoDiffScalars, resize the - * derivatives vector of each element of the matrix mat to match - * the number of derivatives of the scalar. This is useful in functions that - * return matrices that do not depend on an AutoDiffScalar - * argument (e.g. a function with a constant output), while it is desired that - * information about the number of derivatives is preserved. - * \param mat matrix, for which the derivative vectors of the elements will be - * resized - * \param scalar scalar to match the derivative size vector against. - */ -template -// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). -void resizeDerivativesToMatchScalar(Eigen::MatrixBase& mat, - const typename Derived::Scalar& scalar) { - internal::ResizeDerivativesToMatchScalarImpl< - Derived, typename Derived::Scalar>::run(mat, scalar); -} - -namespace internal { -/** \brief Helper for totalSizeAtCompileTime function (recursive) - */ -template -struct TotalSizeAtCompileTime { - static constexpr int eval() { - return Head::SizeAtCompileTime == Eigen::Dynamic || - TotalSizeAtCompileTime::eval() == Eigen::Dynamic - ? Eigen::Dynamic - : Head::SizeAtCompileTime + - TotalSizeAtCompileTime::eval(); - } -}; - -/** \brief Helper for totalSizeAtCompileTime function (base case) - */ -template -struct TotalSizeAtCompileTime { - static constexpr int eval() { return Head::SizeAtCompileTime; } -}; - -/** \brief Determine the total size at compile time of a number of arguments - * based on their SizeAtCompileTime static members - */ -template -constexpr int totalSizeAtCompileTime() { - return TotalSizeAtCompileTime::eval(); -} - -/** \brief Determine the total size at runtime of a number of arguments using - * their size() methods (base case). - */ -constexpr Eigen::DenseIndex totalSizeAtRunTime() { return 0; } - -/** \brief Determine the total size at runtime of a number of arguments using - * their size() methods (recursive) - */ -template -Eigen::DenseIndex totalSizeAtRunTime(const Eigen::MatrixBase& head, - const Tail&... tail) { - return head.size() + totalSizeAtRunTime(tail...); -} - -/** \brief Helper for initializeAutoDiffTuple function (recursive) - */ -template -struct InitializeAutoDiffTupleHelper { - template - static void run(const std::tuple& values, - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - std::tuple& auto_diffs, - Eigen::DenseIndex num_derivatives, - Eigen::DenseIndex deriv_num_start) { - constexpr size_t tuple_index = sizeof...(AutoDiffTypes)-Index; - const auto& value = std::get(values); - auto& auto_diff = std::get(auto_diffs); - auto_diff.resize(value.rows(), value.cols()); - initializeAutoDiff(value, auto_diff, num_derivatives, deriv_num_start); - InitializeAutoDiffTupleHelper::run( - values, auto_diffs, num_derivatives, deriv_num_start + value.size()); - } -}; - -/** \brief Helper for initializeAutoDiffTuple function (base case) - */ -template <> -struct InitializeAutoDiffTupleHelper<0> { - template - static void run(const std::tuple& values, - const std::tuple& auto_diffs, - Eigen::DenseIndex num_derivatives, - Eigen::DenseIndex deriv_num_start) { - unused(values, auto_diffs, num_derivatives, deriv_num_start); - } -}; -} // namespace internal - -/** \brief Given a series of Eigen matrices, create a tuple of corresponding - *AutoDiff matrices with values equal to the input matrices and properly - *initialized derivative vectors. - * - * The size of the derivative vector of each element of the matrices in the - *output tuple will be the same, and will equal the sum of the number of - *elements of the matrices in \p args. - * If all of the matrices in \p args have fixed size, then the derivative - *vectors will also have fixed size (being the sum of the sizes at compile time - *of all of the input arguments), - * otherwise the derivative vectors will have dynamic size. - * The 0th element of the derivative vectors will correspond to the derivative - *with respect to the 0th element of the first argument. - * Subsequent derivative vector elements correspond first to subsequent elements - *of the first input argument (traversed first by row, then by column), and so - *on for subsequent arguments. - * - * \param args a series of Eigen matrices - * \return a tuple of properly initialized AutoDiff matrices corresponding to \p - *args - * - */ -template -std::tuple()>...> -initializeAutoDiffTuple(const Eigen::MatrixBase&... args) { - Eigen::DenseIndex dynamic_num_derivs = internal::totalSizeAtRunTime(args...); - std::tuple()>...> - ret(AutoDiffMatrixType()>( - args.rows(), args.cols())...); - auto values = std::forward_as_tuple(args...); - internal::InitializeAutoDiffTupleHelper::run( - values, ret, dynamic_num_derivs, 0); - return ret; -} - -} // namespace math -} // namespace drake diff --git a/wpimath/update_drake.py b/wpimath/update_drake.py index 199def0e1b..296e85f278 100755 --- a/wpimath/update_drake.py +++ b/wpimath/update_drake.py @@ -7,7 +7,7 @@ import subprocess from upstream_utils import clone_repo, comment_out_invalid_includes, copy_to, walk_if -def drake_cpp_inclusions(dp, f): +def drake_main_cpp_inclusions(dp, f): """Returns true if the given source file in the drake git repo should be copied into allwpilib @@ -20,7 +20,7 @@ def drake_cpp_inclusions(dp, f): ] -def drake_h_inclusions(dp, f): +def drake_main_h_inclusions(dp, f): """Returns true if the given header file in the drake git repo should be copied into allwpilib @@ -35,12 +35,35 @@ def drake_h_inclusions(dp, f): ] +def drake_test_cpp_inclusions(dp, f): + """Returns true if the given source file in the drake git repo should be + copied into allwpilib + + Keyword arguments: + dp -- directory path + f -- filename + """ + return f == "discrete_algebraic_riccati_equation_test.cc" + + +def drake_test_h_inclusions(dp, f): + """Returns true if the given header file in the drake git repo should be + copied into allwpilib + + Keyword arguments: + dp -- directory path + f -- filename + """ + return f == "eigen_matrix_compare.h" + + def main(): wpimath = os.getcwd() clone_repo("https://github.com/RobotLocomotion/drake", "v0.31.0") + repo = os.getcwd() # Copy drake source files into allwpilib - files = walk_if(".", drake_cpp_inclusions) + files = walk_if(".", drake_main_cpp_inclusions) dest = os.path.join(wpimath, "src/main/native/cpp/drake") copy_to(files, dest) @@ -54,10 +77,31 @@ def main(): for f in files: comment_out_invalid_includes( - f, os.path.join(wpimath, "src/main/native/include")) + f, [os.path.join(wpimath, "src/main/native/include")]) + + # Copy drake test source files into allwpilib + os.chdir(os.path.join(repo, "math/test")) + files = walk_if(".", drake_test_cpp_inclusions) + dest = os.path.join(wpimath, "src/test/native/cpp/drake") + copy_to(files, dest) + os.chdir(repo) + + for i in range(len(files)): + # Files moved to dest + files[i] = os.path.join(dest, files[i]) + + # copy_to() renames .cc files to .cpp + if files[i].endswith(".cc"): + files[i] = os.path.splitext(files[i])[0] + ".cpp" + + for f in files: + comment_out_invalid_includes(f, [ + os.path.join(wpimath, "src/main/native/include"), + os.path.join(wpimath, "src/test/native/include") + ]) # Copy drake header files into allwpilib - files = walk_if(".", drake_h_inclusions) + files = walk_if(".", drake_main_h_inclusions) dest = os.path.join(wpimath, "src/main/native/include/drake") copy_to(files, dest) @@ -67,11 +111,25 @@ def main(): for f in files: comment_out_invalid_includes( - f, os.path.join(wpimath, "src/main/native/include")) + f, [os.path.join(wpimath, "src/main/native/include")]) + + # Copy drake test header files into allwpilib + files = walk_if(".", drake_test_h_inclusions) + dest = os.path.join(wpimath, "src/test/native/include/drake") + copy_to(files, dest) + + # Files moved to dest + for i in range(len(files)): + files[i] = os.path.join(dest, files[i]) + + for f in files: + comment_out_invalid_includes( + f, [os.path.join(wpimath, "src/main/native/include")]) # Apply patches os.chdir(wpimath) subprocess.check_output(["git", "apply", "drake-dare-qrn.patch"]) + subprocess.check_output(["git", "apply", "drake-dare-qrn-tests.patch"]) subprocess.check_output( ["git", "apply", "drake-replace-noreturn-attributes.patch"]) subprocess.check_output( diff --git a/wpimath/update_eigen.py b/wpimath/update_eigen.py index 3470ad22fb..905d8805b4 100755 --- a/wpimath/update_eigen.py +++ b/wpimath/update_eigen.py @@ -100,7 +100,7 @@ def main(): files[i] = os.path.join(dest, files[i]) for f in files: - comment_out_invalid_includes(f, dest) + comment_out_invalid_includes(f, [dest]) # Copy unsupported headers into allwpilib os.chdir(os.path.join(repo, "unsupported")) @@ -113,7 +113,7 @@ def main(): files[i] = os.path.join(dest, files[i]) for f in files: - comment_out_invalid_includes(f, dest) + comment_out_invalid_includes(f, [dest]) # Apply patches os.chdir(wpimath) diff --git a/wpimath/upstream_utils.py b/wpimath/upstream_utils.py index da0df212a9..928dce8edb 100644 --- a/wpimath/upstream_utils.py +++ b/wpimath/upstream_utils.py @@ -89,12 +89,12 @@ def copy_to(files, root): shutil.copyfile(f, dest_file) -def comment_out_invalid_includes(filename, include_root): +def comment_out_invalid_includes(filename, include_roots): """Comment out #include directives that include a nonexistent file Keyword arguments: filename -- file to search for includes - include_root -- the search path for includes + include_roots -- list of search paths for includes """ # Read header with open(filename) as f: @@ -111,8 +111,9 @@ def comment_out_invalid_includes(filename, include_root): # Comment out #include if the file doesn't exist in current directory or # include root if not os.path.exists(os.path.join( - os.path.dirname(filename), include)) and not os.path.exists( - os.path.join(include_root, include)): + os.path.dirname(filename), include)) and not any( + os.path.exists(os.path.join(include_root, include)) + for include_root in include_roots): new_contents += "// " new_contents += match.group()