mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-22 01:11:42 +00:00
[wpimath] Rewrite DARE solver (#5328)
I timed the DARE unit tests, and the new solver is 0 to 100% faster in all cases (that is, it's at least as fast as Drake's and up to 2x faster in some cases). The new solver is also much simpler, takes less time to compile, and drops the libwpimath.so size from 325 MB to 301 MB. I think most of the compilation time is coming from the eigenvalue decompositions used to enforce argument preconditions.
This commit is contained in:
Tyler Veness
@@ -1,164 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <string_view>
|
||||
|
||||
#include <fmt/format.h>
|
||||
|
||||
// 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
|
||||
<a href="https://fmt.dev/latest/api.html#compile-time-format-string-checks">fmt::runtime</a>.
|
||||
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<T>::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 <typename T>
|
||||
struct Converter;
|
||||
|
||||
/* Provides convenient type shortcuts for the TYPE in DRAKE_FORMATTER_AS. */
|
||||
template <typename T>
|
||||
struct Traits {
|
||||
/* The type being formatted. */
|
||||
using InputType = T;
|
||||
/* The format_as functor that provides the alternative object to format. */
|
||||
using Functor = Converter<T>;
|
||||
/* The type of the alternative object. */
|
||||
using OutputType = decltype(Functor::call(std::declval<InputType>()));
|
||||
/* The fmt::formatter<> for the alternative object. */
|
||||
using OutputTypeFormatter = fmt::formatter<OutputType>;
|
||||
};
|
||||
|
||||
/* Implements fmt::formatter<TYPE> 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 <typename T>
|
||||
struct Formatter;
|
||||
|
||||
} // namespace internal::formatter_as
|
||||
|
||||
} // namespace drake
|
||||
|
||||
/** Adds a `fmt::formatter<NAMESPACE::TYPE>` 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 <TEMPLATE_ARGS>
|
||||
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<T>` 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 <TEMPLATE_ARGS> \
|
||||
struct Converter<NAMESPACE::TYPE> { \
|
||||
using InputType = NAMESPACE::TYPE; \
|
||||
static auto call(const InputType& ARG) { return EXPR; } \
|
||||
}; \
|
||||
\
|
||||
/* Provides the fmt::formatter<TYPE> implementation. */ \
|
||||
template <TEMPLATE_ARGS> \
|
||||
struct Formatter<NAMESPACE::TYPE> \
|
||||
: fmt::formatter<typename Traits<NAMESPACE::TYPE>::OutputType> { \
|
||||
using MyTraits = Traits<NAMESPACE::TYPE>; \
|
||||
/* Shadow our base class member function template of the same name. */ \
|
||||
template <typename FormatContext> \
|
||||
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<Base*>(self)->format( \
|
||||
MyTraits::Functor::call(x), \
|
||||
ctx); \
|
||||
} \
|
||||
}; \
|
||||
} /* namespace drake::internal::formatter_as */ \
|
||||
\
|
||||
/* Specializes the fmt::formatter<> class template for TYPE. */ \
|
||||
namespace fmt { \
|
||||
template <TEMPLATE_ARGS> \
|
||||
struct formatter<NAMESPACE::TYPE> \
|
||||
: drake::internal::formatter_as::Formatter<NAMESPACE::TYPE> {}; \
|
||||
} /* namespace fmt */
|
||||
@@ -1,87 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
#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 <typename Derived>
|
||||
struct fmt_eigen_ref {
|
||||
const Eigen::MatrixBase<Derived>& matrix;
|
||||
};
|
||||
|
||||
/* Returns the string formatting of the given matrix.
|
||||
@tparam T must be either double, float, or string */
|
||||
template <typename T>
|
||||
std::string FormatEigenMatrix(
|
||||
const Eigen::Ref<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>>&
|
||||
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 <typename Derived>
|
||||
internal::fmt_eigen_ref<Derived> fmt_eigen(
|
||||
const Eigen::MatrixBase<Derived>& matrix) {
|
||||
return {matrix};
|
||||
}
|
||||
|
||||
} // namespace drake
|
||||
|
||||
#ifndef DRAKE_DOXYGEN_CXX
|
||||
// Formatter specialization for drake::fmt_eigen.
|
||||
namespace fmt {
|
||||
template <typename Derived>
|
||||
struct formatter<drake::internal::fmt_eigen_ref<Derived>>
|
||||
: formatter<std::string_view> {
|
||||
template <typename FormatContext>
|
||||
auto format(const drake::internal::fmt_eigen_ref<Derived>& 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<Scalar, double> ||
|
||||
std::is_same_v<Scalar, float>) {
|
||||
return formatter<std::string_view>{}.format(
|
||||
drake::internal::FormatEigenMatrix<Scalar>(matrix), ctx);
|
||||
} else {
|
||||
return formatter<std::string_view>{}.format(
|
||||
drake::internal::FormatEigenMatrix<std::string>(
|
||||
matrix.unaryExpr([](const auto& element) -> std::string {
|
||||
return fmt::to_string(element);
|
||||
})),
|
||||
ctx);
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace fmt
|
||||
#endif
|
||||
@@ -1,113 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "drake/common/fmt_eigen.h"
|
||||
// #include "drake/common/text_logging.h"
|
||||
|
||||
namespace drake {
|
||||
|
||||
enum class MatrixCompareType { absolute, relative };
|
||||
|
||||
/**
|
||||
* Compares two matrices to determine whether they are equal to within a certain
|
||||
* threshold.
|
||||
*
|
||||
* @param m1 The first matrix to compare.
|
||||
* @param m2 The second matrix to compare.
|
||||
* @param tolerance The tolerance for determining equivalence.
|
||||
* @param compare_type Whether the tolereance is absolute or relative.
|
||||
* @param explanation A pointer to a string variable for saving an explanation
|
||||
* of why @p m1 and @p m2 are unequal. This parameter is optional and defaults
|
||||
* to `nullptr`. If this is `nullptr` and @p m1 and @p m2 are not equal, an
|
||||
* explanation is logged as an error message.
|
||||
* @return true if the two matrices are equal based on the specified tolerance.
|
||||
*/
|
||||
template <typename DerivedA, typename DerivedB>
|
||||
[[nodiscard]] ::testing::AssertionResult CompareMatrices(
|
||||
const Eigen::MatrixBase<DerivedA>& m1,
|
||||
const Eigen::MatrixBase<DerivedB>& m2, double tolerance = 0.0,
|
||||
MatrixCompareType compare_type = MatrixCompareType::absolute) {
|
||||
if (m1.rows() != m2.rows() || m1.cols() != 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++) {
|
||||
for (int jj = 0; jj < m1.cols(); jj++) {
|
||||
// First handle the corner cases of positive infinity, negative infinity,
|
||||
// and NaN
|
||||
const auto both_positive_infinity =
|
||||
m1(ii, jj) == std::numeric_limits<double>::infinity() &&
|
||||
m2(ii, jj) == std::numeric_limits<double>::infinity();
|
||||
|
||||
const auto both_negative_infinity =
|
||||
m1(ii, jj) == -std::numeric_limits<double>::infinity() &&
|
||||
m2(ii, jj) == -std::numeric_limits<double>::infinity();
|
||||
|
||||
using std::isnan;
|
||||
const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj));
|
||||
|
||||
if (both_positive_infinity || both_negative_infinity || both_nan)
|
||||
continue;
|
||||
|
||||
// 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)))) {
|
||||
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
|
||||
// the tolerance.
|
||||
using std::abs;
|
||||
const auto delta = abs(m1(ii, jj) - m2(ii, jj));
|
||||
|
||||
if (compare_type == MatrixCompareType::absolute) {
|
||||
// Perform comparison using absolute tolerance.
|
||||
if (delta > tolerance) {
|
||||
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:
|
||||
// http://realtimecollisiondetection.net/blog/?p=89
|
||||
using std::max;
|
||||
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) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
Reference in New Issue
Block a user