diff --git a/upstream_utils/README.md b/upstream_utils/README.md index 587fc9e0b5..f55264b7ab 100644 --- a/upstream_utils/README.md +++ b/upstream_utils/README.md @@ -47,7 +47,7 @@ git rebase 2.0 Generate patch files for the new version. ```bash -git format-patch 2.0..HEAD +git format-patch 2.0..HEAD --no-signature ``` Move the patch files to `upstream_utils`. diff --git a/upstream_utils/drake_patches/0001-Replace-Eigen-Dense-with-Eigen-Core.patch b/upstream_utils/drake_patches/0001-Replace-Eigen-Dense-with-Eigen-Core.patch index dcd2e8f371..2992678dd1 100644 --- a/upstream_utils/drake_patches/0001-Replace-Eigen-Dense-with-Eigen-Core.patch +++ b/upstream_utils/drake_patches/0001-Replace-Eigen-Dense-with-Eigen-Core.patch @@ -1,4 +1,4 @@ -From 02d023c7cdfdfb72ccdbccbac0883b4a1f6ec6d5 Mon Sep 17 00:00:00 2001 +From 32dd1aa796fbb45d02f8bd445cc962e6a91318e7 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 18 May 2022 11:13:21 -0700 Subject: [PATCH 1/2] Replace with @@ -12,7 +12,7 @@ Subject: [PATCH 1/2] Replace with 5 files changed, 7 insertions(+), 3 deletions(-) diff --git a/common/is_approx_equal_abstol.h b/common/is_approx_equal_abstol.h -index 9af0c45..b3f369c 100644 +index 9af0c45252..b3f369ca01 100644 --- a/common/is_approx_equal_abstol.h +++ b/common/is_approx_equal_abstol.h @@ -2,7 +2,7 @@ @@ -25,20 +25,20 @@ index 9af0c45..b3f369c 100644 namespace drake { diff --git a/common/test_utilities/eigen_matrix_compare.h b/common/test_utilities/eigen_matrix_compare.h -index a595da9..c22567d 100644 +index 01821ff847..105f6b381c 100644 --- a/common/test_utilities/eigen_matrix_compare.h +++ b/common/test_utilities/eigen_matrix_compare.h -@@ -4,7 +4,7 @@ - #include +@@ -5,7 +5,7 @@ #include + #include -#include +#include #include - #include "drake/common/text_logging.h" + #include "drake/common/fmt_eigen.h" diff --git a/math/discrete_algebraic_riccati_equation.cc b/math/discrete_algebraic_riccati_equation.cc -index 901f2ef..20ea2b7 100644 +index 901f2ef240..20ea2b7bbe 100644 --- a/math/discrete_algebraic_riccati_equation.cc +++ b/math/discrete_algebraic_riccati_equation.cc @@ -1,5 +1,8 @@ @@ -51,7 +51,7 @@ index 901f2ef..20ea2b7 100644 #include "drake/common/drake_throw.h" #include "drake/common/is_approx_equal_abstol.h" diff --git a/math/discrete_algebraic_riccati_equation.h b/math/discrete_algebraic_riccati_equation.h -index 891373f..df7a58b 100644 +index 891373ff9d..df7a58b2b8 100644 --- a/math/discrete_algebraic_riccati_equation.h +++ b/math/discrete_algebraic_riccati_equation.h @@ -3,7 +3,7 @@ @@ -64,7 +64,7 @@ index 891373f..df7a58b 100644 namespace drake { namespace math { diff --git a/math/test/discrete_algebraic_riccati_equation_test.cc b/math/test/discrete_algebraic_riccati_equation_test.cc -index 533ced1..e4ecfd2 100644 +index 533ced151d..e4ecfd2eb5 100644 --- a/math/test/discrete_algebraic_riccati_equation_test.cc +++ b/math/test/discrete_algebraic_riccati_equation_test.cc @@ -1,5 +1,6 @@ diff --git a/upstream_utils/drake_patches/0002-Add-WPILIB_DLLEXPORT-to-DARE-function-declarations.patch b/upstream_utils/drake_patches/0002-Add-WPILIB_DLLEXPORT-to-DARE-function-declarations.patch index 1c7b469f48..0690caa865 100644 --- a/upstream_utils/drake_patches/0002-Add-WPILIB_DLLEXPORT-to-DARE-function-declarations.patch +++ b/upstream_utils/drake_patches/0002-Add-WPILIB_DLLEXPORT-to-DARE-function-declarations.patch @@ -1,4 +1,4 @@ -From b208372a18b37f6cbc49dd45d15adf63c9b60755 Mon Sep 17 00:00:00 2001 +From 48ed223a299304724a38ad9911f7a732bf574b71 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 18 May 2022 11:15:27 -0700 Subject: [PATCH 2/2] Add WPILIB_DLLEXPORT to DARE function declarations @@ -8,7 +8,7 @@ Subject: [PATCH 2/2] Add WPILIB_DLLEXPORT to DARE function declarations 1 file changed, 3 insertions(+) diff --git a/math/discrete_algebraic_riccati_equation.h b/math/discrete_algebraic_riccati_equation.h -index df7a58b..55b8442 100644 +index df7a58b2b8..55b8442bf4 100644 --- a/math/discrete_algebraic_riccati_equation.h +++ b/math/discrete_algebraic_riccati_equation.h @@ -4,6 +4,7 @@ diff --git a/upstream_utils/update_drake.py b/upstream_utils/update_drake.py index 1849494c53..3398d904a2 100755 --- a/upstream_utils/update_drake.py +++ b/upstream_utils/update_drake.py @@ -13,7 +13,7 @@ from upstream_utils import ( def main(): - upstream_root = clone_repo("https://github.com/RobotLocomotion/drake", "v1.6.0") + upstream_root = clone_repo("https://github.com/RobotLocomotion/drake", "v1.15.0") wpilib_root = get_repo_root() wpimath = os.path.join(wpilib_root, "wpimath") @@ -63,10 +63,14 @@ def main(): os.path.join(wpimath, "src/test/native/cpp/drake"), ) os.chdir(upstream_root) + test_src_files += walk_cwd_and_copy_if( + lambda dp, f: f == "fmt_eigen.cc", + os.path.join(wpimath, "src/test/native/cpp/drake"), + ) # Copy drake test header files into allwpilib test_include_files = walk_cwd_and_copy_if( - lambda dp, f: f == "eigen_matrix_compare.h", + lambda dp, f: f in ["eigen_matrix_compare.h", "fmt.h", "fmt_eigen.h"], os.path.join(wpimath, "src/test/native/include/drake"), ) diff --git a/wpimath/src/test/native/cpp/drake/common/fmt_eigen.cpp b/wpimath/src/test/native/cpp/drake/common/fmt_eigen.cpp new file mode 100644 index 0000000000..7ec8cd0f7e --- /dev/null +++ b/wpimath/src/test/native/cpp/drake/common/fmt_eigen.cpp @@ -0,0 +1,41 @@ +// TODO(jwnimmer-tri) Write our own formatting logic instead of using Eigen IO, +// and add customization flags for how to display the matrix data. +#undef EIGEN_NO_IO +#include "drake/common/fmt_eigen.h" + +#include +#include + +namespace drake { +namespace internal { + +template +using FormatterEigenRef = + Eigen::Ref>; + +template +std::string FormatEigenMatrix(const FormatterEigenRef& matrix) { + std::stringstream stream; + // We'll print our matrix data using as much precision as we can, so that + // console log output and/or error messages paint the full picture. Sadly, + // the ostream family of floating-point formatters doesn't know how to do + // "shortest round-trip precision". If we set the precision to max_digits, + // then simple numbers like "1.1" print as "1.1000000000000001"; instead, + // we'll use max_digits - 1 to avoid that problem, with the risk of losing + // the last ulps in the printout in case we needed that last digit. This + // will all be fixed once we stop using Eigen IO. + stream.precision(std::numeric_limits::max_digits10 - 1); + stream << matrix; + return stream.str(); +} + +// Explicitly instantiate for the allowed scalar types in our header. +template std::string FormatEigenMatrix( + const FormatterEigenRef& matrix); +template std::string FormatEigenMatrix( + const FormatterEigenRef& matrix); +template std::string FormatEigenMatrix( + const FormatterEigenRef& matrix); + +} // namespace internal +} // namespace drake diff --git a/wpimath/src/test/native/include/drake/common/fmt.h b/wpimath/src/test/native/include/drake/common/fmt.h new file mode 100644 index 0000000000..7211bc6d4e --- /dev/null +++ b/wpimath/src/test/native/include/drake/common/fmt.h @@ -0,0 +1,164 @@ +#pragma once + +#include + +#include + +// This file contains the essentials of fmt support in Drake, mainly +// compatibility code to inter-operate with different versions of fmt. + +namespace drake { + +#if FMT_VERSION >= 80000 || defined(DRAKE_DOXYGEN_CXX) +/** When using fmt >= 8, this is an alias for +fmt::runtime. +When using fmt < 8, this is a no-op. */ +inline auto fmt_runtime(std::string_view s) { + return fmt::runtime(s); +} +/** When using fmt >= 8, this is defined to be `const` to indicate that the +`fmt::formatter::format(...)` function should be object-const. +When using fmt < 8, the function signature was incorrect (lacking the const), +so this macro will be empty. */ +#define DRAKE_FMT8_CONST const +#else // FMT_VERSION +inline auto fmt_runtime(std::string_view s) { + return s; +} +#define DRAKE_FMT8_CONST +#endif // FMT_VERSION + +namespace internal::formatter_as { + +/* The DRAKE_FORMATTER_AS macro specializes this for it's format_as types. +This struct is a functor that performs a conversion. See below for details. +@tparam T is the TYPE to be formatted. */ +template +struct Converter; + +/* Provides convenient type shortcuts for the TYPE in DRAKE_FORMATTER_AS. */ +template +struct Traits { + /* The type being formatted. */ + using InputType = T; + /* The format_as functor that provides the alternative object to format. */ + using Functor = Converter; + /* The type of the alternative object. */ + using OutputType = decltype(Functor::call(std::declval())); + /* The fmt::formatter<> for the alternative object. */ + using OutputTypeFormatter = fmt::formatter; +}; + +/* Implements fmt::formatter for DRAKE_FORMATER_AS types. The macro +specializes this for it's format_as types. See below for details. +@tparam T is the TYPE to be formatted. */ +template +struct Formatter; + +} // namespace internal::formatter_as + +} // namespace drake + +/** Adds a `fmt::formatter` template specialization that +formats the `TYPE` by delegating the formatting using a transformed expression, +as if a conversion function like this were interposed during formatting: + +@code{cpp} +template +auto format_as(const NAMESPACE::TYPE& ARG) { + return EXPR; +} +@endcode + +For example, this declaration ... + +@code{cpp} +DRAKE_FORMATTER_AS(, my_namespace, MyType, x, x.to_string()) +@endcode + +... changes this code ... + +@code{cpp} +MyType foo; +fmt::print(foo); +@endcode + +... to be equivalent to ... + +@code{cpp} +MyType foo; +fmt::print(foo.to_string()); +@endcode + +... allowing user to format `my_namespace::MyType` objects without manually +adding the `to_string` call every time. + +This provides a convenient mechanism to add formatters for classes that already +have a `to_string` function, or classes that are just thin wrappers over simple +types (ints, enums, etc.). + +Always use this macro in the global namespace, and do not use a semicolon (`;`) +afterward. + +@param TEMPLATE_ARGS The optional first argument `TEMPLATE_ARGS` can be used in +case the `TYPE` is templated, e.g., it might commonly be set to `typename T`. It +should be left empty when the TYPE is not templated. In case `TYPE` has multiple +template arguments, note that macros will fight with commas so you should use +`typename... Ts` instead of writing them all out. + +@param NAMESPACE The namespace that encloses the `TYPE` being formatted. Cannot +be empty. For nested namespaces, use intemediate colons, e.g., `%drake::common`. +Do not place _leading_ colons on the `NAMESPACE`. + +@param TYPE The class name (or struct name, or enum name, etc.) being formatted. +Do not place _leading_ double-colons on the `TYPE`. If the type is templated, +use the template arguments here, e.g., `MyOptional` if `TEMPLATE_ARGS` was +chosen as `typename T`. + +@param ARG A placeholder variable name to use for the value (i.e., object) +being formatted within the `EXPR` expression. + +@param EXPR An expression to `return` from the format_as function; it can +refer to the given `ARG` name which will be of type `const TYPE& ARG`. + +@note In future versions of fmt (perhaps fmt >= 10) there might be an ADL +`format_as` customization point with this feature built-in. If so, then we can +update this macro to use that spelling, and eventually deprecate the macro once +Drake drops support for earlier version of fmt. */ +#define DRAKE_FORMATTER_AS(TEMPLATE_ARGS, NAMESPACE, TYPE, ARG, EXPR) \ + /* Specializes the Converter<> class template for our TYPE. */ \ + namespace drake::internal::formatter_as { \ + template \ + struct Converter { \ + using InputType = NAMESPACE::TYPE; \ + static auto call(const InputType& ARG) { return EXPR; } \ + }; \ + \ + /* Provides the fmt::formatter implementation. */ \ + template \ + struct Formatter \ + : fmt::formatter::OutputType> { \ + using MyTraits = Traits; \ + /* Shadow our base class member function template of the same name. */ \ + template \ + auto format(const typename MyTraits::InputType& x, \ + FormatContext& ctx) DRAKE_FMT8_CONST { \ + /* Call the base class member function after laundering the object */ \ + /* through the user's provided format_as function. Older versions of */ \ + /* fmt have const-correctness bugs, which we can fix with some good */ \ + /* old fashioned const_cast-ing here. */ \ + using Base = typename MyTraits::OutputTypeFormatter; \ + const Base* const self = this; \ + return const_cast(self)->format( \ + MyTraits::Functor::call(x), \ + ctx); \ + } \ + }; \ + } /* namespace drake::internal::formatter_as */ \ + \ + /* Specializes the fmt::formatter<> class template for TYPE. */ \ + namespace fmt { \ + template \ + struct formatter \ + : drake::internal::formatter_as::Formatter {}; \ + } /* namespace fmt */ diff --git a/wpimath/src/test/native/include/drake/common/fmt_eigen.h b/wpimath/src/test/native/include/drake/common/fmt_eigen.h new file mode 100644 index 0000000000..e56d66417b --- /dev/null +++ b/wpimath/src/test/native/include/drake/common/fmt_eigen.h @@ -0,0 +1,87 @@ +#pragma once + +#include +#include + +#include + +#include "drake/common/fmt.h" + +namespace drake { +namespace internal { + +/* A tag type to be used in fmt::format("{}", fmt_eigen(...)) calls. +Below we'll add a fmt::formatter<> specialization for this tag. */ +template +struct fmt_eigen_ref { + const Eigen::MatrixBase& matrix; +}; + +/* Returns the string formatting of the given matrix. +@tparam T must be either double, float, or string */ +template +std::string FormatEigenMatrix( + const Eigen::Ref>& + matrix); + +} // namespace internal + +/** When passing an Eigen::Matrix to fmt, use this wrapper function to instruct +fmt to use Drake's custom formatter for Eigen types. + +Within Drake, when formatting an Eigen matrix into a string you must wrap the +Eigen object as `fmt_eigen(M)`. This holds true whether it be for logging, error +messages, debugging, or etc. + +For example: +@code +if (!CheckValid(M)) { + throw std::logic_error(fmt::format("Invalid M = {}", fmt_eigen(M))); +} +@endcode + +@warning The return value of this function should only ever be used as a +temporary object, i.e., in a fmt argument list or a logging statement argument +list. Never store it as a local variable, member field, etc. + +@note To ensure floating-point data is formatted without losing any digits, +Drake's code is compiled using -DEIGEN_NO_IO, which enforces that nothing within +Drake is allowed to use Eigen's `operator<<`. Downstream code that calls into +Drake is not required to use that option; it is only enforced by Drake's build +system, not by Drake's headers. */ +template +internal::fmt_eigen_ref fmt_eigen( + const Eigen::MatrixBase& matrix) { + return {matrix}; +} + +} // namespace drake + +#ifndef DRAKE_DOXYGEN_CXX +// Formatter specialization for drake::fmt_eigen. +namespace fmt { +template +struct formatter> + : formatter { + template + auto format(const drake::internal::fmt_eigen_ref& ref, + // NOLINTNEXTLINE(runtime/references) To match fmt API. + FormatContext& ctx) DRAKE_FMT8_CONST -> decltype(ctx.out()) { + using Scalar = typename Derived::Scalar; + const auto& matrix = ref.matrix; + if constexpr (std::is_same_v || + std::is_same_v) { + return formatter{}.format( + drake::internal::FormatEigenMatrix(matrix), ctx); + } else { + return formatter{}.format( + drake::internal::FormatEigenMatrix( + matrix.unaryExpr([](const auto& element) -> std::string { + return fmt::to_string(element); + })), + ctx); + } + } +}; +} // namespace fmt +#endif 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 d6bcbb8ec9..cd5dae8fd7 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 @@ -3,10 +3,12 @@ #include #include #include +#include #include #include +#include "drake/common/fmt_eigen.h" // #include "drake/common/text_logging.h" namespace drake { @@ -33,9 +35,10 @@ template const Eigen::MatrixBase& m2, double tolerance = 0.0, MatrixCompareType compare_type = MatrixCompareType::absolute) { if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) { - return ::testing::AssertionFailure() - << "Matrix size mismatch: (" << m1.rows() << " x " << m1.cols() - << " vs. " << m2.rows() << " x " << m2.cols() << ")"; + const std::string message = + fmt::format("Matrix size mismatch: ({} x {} vs. {} x {})", m1.rows(), + m1.cols(), m2.rows(), m2.cols()); + return ::testing::AssertionFailure() << message; } for (int ii = 0; ii < m1.rows(); ii++) { @@ -59,10 +62,10 @@ template // Check for case where one value is NaN and the other is not if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) || (!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) { - return ::testing::AssertionFailure() << "NaN mismatch at (" << ii - << ", " << jj << "):\nm1 =\n" - << m1 << "\nm2 =\n" - << m2; + const std::string message = + fmt::format("NaN mismatch at ({}, {}):\nm1 =\n{}\nm2 =\n{}", ii, jj, + fmt_eigen(m1), fmt_eigen(m2)); + return ::testing::AssertionFailure() << message; } // Determine whether the difference between the two matrices is less than @@ -72,16 +75,13 @@ template if (compare_type == MatrixCompareType::absolute) { // Perform comparison using absolute tolerance. - if (delta > tolerance) { - return ::testing::AssertionFailure() - << "Values at (" << ii << ", " << jj - << ") exceed tolerance: " << m1(ii, jj) << " vs. " - << m2(ii, jj) << ", diff = " << delta - << ", tolerance = " << tolerance << "\nm1 =\n" - << m1 << "\nm2 =\n" - << m2 << "\ndelta=\n" - << (m1 - m2); + const std::string message = fmt::format( + "Values at ({}, {}) exceed tolerance: {} vs. {}, diff = {}, " + "tolerance = {}\nm1 =\n{}\nm2 =\n{}\ndelta=\n{}", + ii, jj, m1(ii, jj), m2(ii, jj), delta, tolerance, fmt_eigen(m1), + fmt_eigen(m2), fmt_eigen(m1 - m2)); + return ::testing::AssertionFailure() << message; } } else { // Perform comparison using relative tolerance, see: @@ -90,27 +90,24 @@ template const auto max_value = max(abs(m1(ii, jj)), abs(m2(ii, jj))); const auto relative_tolerance = tolerance * max(decltype(max_value){1}, max_value); - if (delta > relative_tolerance) { - return ::testing::AssertionFailure() - << "Values at (" << ii << ", " << jj - << ") exceed tolerance: " << m1(ii, jj) << " vs. " - << m2(ii, jj) << ", diff = " << delta - << ", tolerance = " << tolerance - << ", relative tolerance = " << relative_tolerance - << "\nm1 =\n" - << m1 << "\nm2 =\n" - << m2 << "\ndelta=\n" - << (m1 - m2); + const std::string message = fmt::format( + "Values at ({}, {}) exceed tolerance: {} vs. {}, diff = {}, " + "tolerance = {}, relative tolerance = {}\nm1 =\n{}\nm2 " + "=\n{}\ndelta=\n{}", + ii, jj, m1(ii, jj), m2(ii, jj), delta, tolerance, + relative_tolerance, fmt_eigen(m1), fmt_eigen(m2), + fmt_eigen(m1 - m2)); + return ::testing::AssertionFailure() << message; } } } } - return ::testing::AssertionSuccess() << "m1 =\n" - << m1 - << "\nis approximately equal to m2 =\n" - << m2; + const std::string message = + fmt::format("m1 =\n{}\nis approximately equal to m2 =\n{}", fmt_eigen(m1), + fmt_eigen(m2)); + return ::testing::AssertionSuccess() << message; } } // namespace drake