Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Move Drake and Eigen to thirdparty folders (#4307)

Browse Source
This commit is contained in:
Tyler Veness
2022-06-11 21:07:15 -07:00
committed by GitHub
parent c9e620a920
commit 9b1bf5c7f1
204 changed files with 53 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

162
wpimath/src/main/native/include/drake/common/drake_assert.h
View File

@@ -1,162 +0,0 @@
#pragma once
#include <type_traits>
/// @file
/// Provides Drake's assertion implementation. This is intended to be used
/// both within Drake and by other software. Drake's asserts can be armed
/// and disarmed independently from the system-wide asserts.
#ifdef DRAKE_DOXYGEN_CXX
/// @p DRAKE_ASSERT(condition) is similar to the built-in @p assert(condition)
/// from the C++ system header @p <cassert>. Unless Drake's assertions are
/// disarmed by the pre-processor definitions listed below, @p DRAKE_ASSERT
/// will evaluate @p condition and iff the value is false will trigger an
/// assertion failure with a message showing at least the condition text,
/// function name, file, and line.
///
/// By default, assertion failures will :abort() the program. However, when
/// using the pydrake python bindings, assertion failures will instead throw a
/// C++ exception that causes a python SystemExit exception.
///
/// Assertions are enabled or disabled using the following pre-processor macros:
///
/// - If @p DRAKE_ENABLE_ASSERTS is defined, then @p DRAKE_ASSERT is armed.
/// - If @p DRAKE_DISABLE_ASSERTS is defined, then @p DRAKE_ASSERT is disarmed.
/// - If both macros are defined, then it is a compile-time error.
/// - If neither are defined, then NDEBUG governs assertions as usual.
///
/// This header will define exactly one of either @p DRAKE_ASSERT_IS_ARMED or
/// @p DRAKE_ASSERT_IS_DISARMED to indicate whether @p DRAKE_ASSERT is armed.
///
/// This header will define both `constexpr bool drake::kDrakeAssertIsArmed`
/// and `constexpr bool drake::kDrakeAssertIsDisarmed` globals.
///
/// One difference versus the standard @p assert(condition) is that the
/// @p condition within @p DRAKE_ASSERT is always syntax-checked, even if
/// Drake's assertions are disarmed.
///
/// Treat @p DRAKE_ASSERT like a statement -- it must always be used
/// in block scope, and must always be followed by a semicolon.
#define DRAKE_ASSERT(condition)
/// Like @p DRAKE_ASSERT, except that the expression must be void-valued; this
/// allows for guarding expensive assertion-checking subroutines using the same
/// macros as stand-alone assertions.
#define DRAKE_ASSERT_VOID(expression)
/// Evaluates @p condition and iff the value is false will trigger an assertion
/// failure with a message showing at least the condition text, function name,
/// file, and line.
#define DRAKE_DEMAND(condition)
/// Silences a "no return value" compiler warning by calling a function that
/// always raises an exception or aborts (i.e., a function marked noreturn).
/// Only use this macro at a point where (1) a point in the code is truly
/// unreachable, (2) the fact that it's unreachable is knowable from only
/// reading the function itself (and not, e.g., some larger design invariant),
/// and (3) there is a compiler warning if this macro were removed. The most
/// common valid use is with a switch-case-return block where all cases are
/// accounted for but the enclosing function is supposed to return a value. Do
/// *not* use this macro as a "logic error" assertion; it should *only* be used
/// to silence false positive warnings. When in doubt, throw an exception
/// manually instead of using this macro.
#define DRAKE_UNREACHABLE()
#else // DRAKE_DOXYGEN_CXX
// Users should NOT set these; only this header should set them.
#ifdef DRAKE_ASSERT_IS_ARMED
# error Unexpected DRAKE_ASSERT_IS_ARMED defined.
#endif
#ifdef DRAKE_ASSERT_IS_DISARMED
# error Unexpected DRAKE_ASSERT_IS_DISARMED defined.
#endif
// Decide whether Drake assertions are enabled.
#if defined(DRAKE_ENABLE_ASSERTS) && defined(DRAKE_DISABLE_ASSERTS)
# error Conflicting assertion toggles.
#elif defined(DRAKE_ENABLE_ASSERTS)
# define DRAKE_ASSERT_IS_ARMED
#elif defined(DRAKE_DISABLE_ASSERTS) || defined(NDEBUG)
# define DRAKE_ASSERT_IS_DISARMED
#else
# define DRAKE_ASSERT_IS_ARMED
#endif
namespace drake {
namespace internal {
// Abort the program with an error message.
[[noreturn]] void Abort(const char* condition, const char* func,
const char* file, int line);
// Report an assertion failure; will either Abort(...) or throw.
[[noreturn]] void AssertionFailed(const char* condition, const char* func,
const char* file, int line);
} // namespace internal
namespace assert {
// Allows for specialization of how to bool-convert Conditions used in
// assertions, in case they are not intrinsically convertible. See
// symbolic_formula.h for an example use. This is a public interface to
// extend; it is intended to be specialized by unusual Scalar types that
// require special handling.
template <typename Condition>
struct ConditionTraits {
static constexpr bool is_valid = std::is_convertible_v<Condition, bool>;
static bool Evaluate(const Condition& value) {
return value;
}
};
} // namespace assert
} // namespace drake
#define DRAKE_UNREACHABLE() \
::drake::internal::Abort( \
"Unreachable code was reached?!", __func__, __FILE__, __LINE__)
#define DRAKE_DEMAND(condition) \
do { \
typedef ::drake::assert::ConditionTraits< \
typename std::remove_cv_t<decltype(condition)>> Trait; \
static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
static_assert( \
!std::is_pointer_v<decltype(condition)>, \
"When using DRAKE_DEMAND on a raw pointer, always write out " \
"DRAKE_DEMAND(foo != nullptr), do not write DRAKE_DEMAND(foo) " \
"and rely on implicit pointer-to-bool conversion."); \
if (!Trait::Evaluate(condition)) { \
::drake::internal::AssertionFailed( \
#condition, __func__, __FILE__, __LINE__); \
} \
} while (0)
#ifdef DRAKE_ASSERT_IS_ARMED
// Assertions are enabled.
namespace drake {
constexpr bool kDrakeAssertIsArmed = true;
constexpr bool kDrakeAssertIsDisarmed = false;
} // namespace drake
# define DRAKE_ASSERT(condition) DRAKE_DEMAND(condition)
# define DRAKE_ASSERT_VOID(expression) do { \
static_assert( \
std::is_convertible_v<decltype(expression), void>, \
"Expression should be void."); \
expression; \
} while (0)
#else
// Assertions are disabled, so just typecheck the expression.
namespace drake {
constexpr bool kDrakeAssertIsArmed = false;
constexpr bool kDrakeAssertIsDisarmed = true;
} // namespace drake
# define DRAKE_ASSERT(condition) do { \
typedef ::drake::assert::ConditionTraits< \
typename std::remove_cv_t<decltype(condition)>> Trait; \
static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
static_assert( \
!std::is_pointer_v<decltype(condition)>, \
"When using DRAKE_ASSERT on a raw pointer, always write out " \
"DRAKE_ASSERT(foo != nullptr), do not write DRAKE_ASSERT(foo) " \
"and rely on implicit pointer-to-bool conversion."); \
} while (0)
# define DRAKE_ASSERT_VOID(expression) static_assert( \
std::is_convertible_v<decltype(expression), void>, \
"Expression should be void.")
#endif
#endif // DRAKE_DOXYGEN_CXX

18
wpimath/src/main/native/include/drake/common/drake_assertion_error.h
View File

@@ -1,18 +0,0 @@
#pragma once
#include <stdexcept>
#include <string>
namespace drake {
namespace internal {
// This is what DRAKE_ASSERT and DRAKE_DEMAND throw when our assertions are
// configured to throw.
class assertion_error : public std::runtime_error {
public:
explicit assertion_error(const std::string& what_arg)
: std::runtime_error(what_arg) {}
};
} // namespace internal
} // namespace drake

90
wpimath/src/main/native/include/drake/common/drake_copyable.h
View File

@@ -1,90 +0,0 @@
#pragma once
// ============================================================================
// N.B. The spelling of the macro names between doc/Doxyfile_CXX.in and this
// file must be kept in sync!
// ============================================================================
/** @file
Provides careful macros to selectively enable or disable the special member
functions for copy-construction, copy-assignment, move-construction, and
move-assignment.
http://en.cppreference.com/w/cpp/language/member_functions#Special_member_functions
When enabled via these macros, the `= default` implementation is provided.
Code that needs custom copy or move functions should not use these macros.
*/
/** DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for
copy-construction, copy-assignment, move-construction, and move-assignment.
Drake's Doxygen is customized to render the deletions in detail, with
appropriate comments. Invoke this macro in the public section of the class
declaration, e.g.:
<pre>
class Foo {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Foo)
// ...
};
</pre>
*/
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname) \
Classname(const Classname&) = delete; \
void operator=(const Classname&) = delete; \
Classname(Classname&&) = delete; \
void operator=(Classname&&) = delete;
/** DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member
functions for copy-construction, copy-assignment, move-construction, and
move-assignment. This macro should be used only when copy-construction and
copy-assignment defaults are well-formed. Note that the defaulted move
functions could conceivably still be ill-formed, in which case they will
effectively not be declared or used -- but because the copy constructor exists
the type will still be MoveConstructible. Drake's Doxygen is customized to
render the functions in detail, with appropriate comments. Typically, you
should invoke this macro in the public section of the class declaration, e.g.:
<pre>
class Foo {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Foo)
// ...
};
</pre>
However, if Foo has a virtual destructor (i.e., is subclassable), then
typically you should use either DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN in the
public section or else DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN in the
protected section, to prevent
<a href="https://en.wikipedia.org/wiki/Object_slicing">object slicing</a>.
The macro contains a built-in self-check that copy-assignment is well-formed.
This self-check proves that the Classname is CopyConstructible, CopyAssignable,
MoveConstructible, and MoveAssignable (in all but the most arcane cases where a
member of the Classname is somehow CopyAssignable but not CopyConstructible).
Therefore, classes that use this macro typically will not need to have any unit
tests that check for the presence nor correctness of these functions.
However, the self-check does not provide any checks of the runtime efficiency
of the functions. If it is important for performance that the move functions
actually move (instead of making a copy), then you should consider capturing
that in a unit test.
*/
#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname) \
Classname(const Classname&) = default; \
Classname& operator=(const Classname&) = default; \
Classname(Classname&&) = default; \
Classname& operator=(Classname&&) = default; \
/* Fails at compile-time if copy-assign doesn't compile. */ \
/* Note that we do not test the copy-ctor here, because */ \
/* it will not exist when Classname is abstract. */ \
static void DrakeDefaultCopyAndMoveAndAssign_DoAssign( \
Classname* a, const Classname& b) { *a = b; } \
static_assert( \
&DrakeDefaultCopyAndMoveAndAssign_DoAssign == \
&DrakeDefaultCopyAndMoveAndAssign_DoAssign, \
"This assertion is never false; its only purpose is to " \
"generate 'use of deleted function: operator=' errors " \
"when Classname is a template.");

47
wpimath/src/main/native/include/drake/common/drake_throw.h
View File

@@ -1,47 +0,0 @@
#pragma once
#include <type_traits>
#include "drake/common/drake_assert.h"
/// @file
/// Provides a convenient wrapper to throw an exception when a condition is
/// unmet. This is similar to an assertion, but uses exceptions instead of
/// ::abort(), and cannot be disabled.
namespace drake {
namespace internal {
// Throw an error message.
[[noreturn]] void Throw(const char* condition, const char* func,
const char* file, int line);
} // namespace internal
} // namespace drake
/// Evaluates @p condition and iff the value is false will throw an exception
/// with a message showing at least the condition text, function name, file,
/// and line.
///
/// The condition must not be a pointer, where we'd implicitly rely on its
/// nullness. Instead, always write out "!= nullptr" to be precise.
///
/// Correct: `DRAKE_THROW_UNLESS(foo != nullptr);`
/// Incorrect: `DRAKE_THROW_UNLESS(foo);`
///
/// Because this macro is intended to provide a useful exception message to
/// users, we should err on the side of extra detail about the failure. The
/// meaning of "foo" isolated within error message text does not make it
/// clear that a null pointer is the proximate cause of the problem.
#define DRAKE_THROW_UNLESS(condition) \
do { \
typedef ::drake::assert::ConditionTraits< \
typename std::remove_cv_t<decltype(condition)>> Trait; \
static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
static_assert( \
!std::is_pointer_v<decltype(condition)>, \
"When using DRAKE_THROW_UNLESS on a raw pointer, always write out " \
"DRAKE_THROW_UNLESS(foo != nullptr), do not write DRAKE_THROW_UNLESS" \
"(foo) and rely on implicit pointer-to-bool conversion."); \
if (!Trait::Evaluate(condition)) { \
::drake::internal::Throw(#condition, __func__, __FILE__, __LINE__); \
} \
} while (0)

62
wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h
View File

@@ -1,62 +0,0 @@
#pragma once
#include <vector>
#include <Eigen/Core>
namespace drake {
/// Returns true if and only if the two matrices are equal to within a certain
/// absolute elementwise @p tolerance. Special values (infinities, NaN, etc.)
/// do not compare as equal elements.
template <typename DerivedA, typename DerivedB>
bool is_approx_equal_abstol(const Eigen::MatrixBase<DerivedA>& m1,
const Eigen::MatrixBase<DerivedB>& m2,
double tolerance) {
return (
(m1.rows() == m2.rows()) &&
(m1.cols() == m2.cols()) &&
((m1 - m2).template lpNorm<Eigen::Infinity>() <= tolerance));
}
/// Returns true if and only if a simple greedy search reveals a permutation
/// of the columns of m2 to make the matrix equal to m1 to within a certain
/// absolute elementwise @p tolerance. E.g., there exists a P such that
/// <pre>
/// forall i,j, |m1 - m2*P|_{i,j} <= tolerance
/// where P is a permutation matrix:
/// P(i,j)={0,1}, sum_i P(i,j)=1, sum_j P(i,j)=1.
/// </pre>
/// Note: Returns false for matrices of different sizes.
/// Note: The current implementation is O(n^2) in the number of columns.
/// Note: In marginal cases (with similar but not identical columns) this
/// algorithm can fail to find a permutation P even if it exists because it
/// accepts the first column match (m1(i),m2(j)) and removes m2(j) from the
/// pool. It is possible that other columns of m2 would also match m1(i) but
/// that m2(j) is the only match possible for a later column of m1.
template <typename DerivedA, typename DerivedB>
bool IsApproxEqualAbsTolWithPermutedColumns(
const Eigen::MatrixBase<DerivedA>& m1,
const Eigen::MatrixBase<DerivedB>& m2, double tolerance) {
if ((m1.cols() != m2.cols()) || (m1.rows() != m2.rows())) return false;
std::vector<bool> available(m2.cols());
for (int i = 0; i < m2.cols(); i++) available[i] = true;
for (int i = 0; i < m1.cols(); i++) {
bool found_match = false;
for (int j = 0; j < m2.cols(); j++) {
if (available[j] &&
is_approx_equal_abstol(m1.col(i), m2.col(j), tolerance)) {
found_match = true;
available[j] = false;
break;
}
}
if (!found_match) return false;
}
return true;
}
} // namespace drake

103
wpimath/src/main/native/include/drake/common/never_destroyed.h
View File

@@ -1,103 +0,0 @@
#pragma once
#include <new>
#include <type_traits>
#include <utility>
#include "drake/common/drake_copyable.h"
namespace drake {
/// Wraps an underlying type T such that its storage is a direct member field
/// of this object (i.e., without any indirection into the heap), but *unlike*
/// most member fields T's destructor is never invoked.
///
/// This is especially useful for function-local static variables that are not
/// trivially destructable. We shouldn't call their destructor at program exit
/// because of the "indeterminate order of ... destruction" as mentioned in
/// cppguide's
/// <a href="https://drake.mit.edu/styleguide/cppguide.html#Static_and_Global_Variables">Static
/// and Global Variables</a> section, but other solutions to this problem place
/// the objects on the heap through an indirection.
///
/// Compared with other approaches, this mechanism more clearly describes the
/// intent to readers, avoids "possible leak" warnings from memory-checking
/// tools, and is probably slightly faster.
///
/// Example uses:
///
/// The singleton pattern:
/// @code
/// class Singleton {
/// public:
/// DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Singleton)
/// static Singleton& getInstance() {
/// static never_destroyed<Singleton> instance;
/// return instance.access();
/// }
/// private:
/// friend never_destroyed<Singleton>;
/// Singleton() = default;
/// };
/// @endcode
///
/// A lookup table, created on demand the first time its needed, and then
/// reused thereafter:
/// @code
/// enum class Foo { kBar, kBaz };
/// Foo ParseFoo(const std::string& foo_string) {
/// using Dict = std::unordered_map<std::string, Foo>;
/// static const drake::never_destroyed<Dict> string_to_enum{
/// std::initializer_list<Dict::value_type>{
/// {"bar", Foo::kBar},
/// {"baz", Foo::kBaz},
/// }
/// };
/// return string_to_enum.access().at(foo_string);
/// }
/// @endcode
///
/// In cases where computing the static data is more complicated than an
/// initializer_list, you can use a temporary lambda to populate the value:
/// @code
/// const std::vector<double>& GetConstantMagicNumbers() {
/// static const drake::never_destroyed<std::vector<double>> result{[]() {
/// std::vector<double> prototype;
/// std::mt19937 random_generator;
/// for (int i = 0; i < 10; ++i) {
/// double new_value = random_generator();
/// prototype.push_back(new_value);
/// }
/// return prototype;
/// }()};
/// return result.access();
/// }
/// @endcode
///
/// Note in particular the `()` after the lambda. That causes it to be invoked.
//
// The above examples are repeated in the unit test; keep them in sync.
template <typename T>
class never_destroyed {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(never_destroyed)
/// Passes the constructor arguments along to T using perfect forwarding.
template <typename... Args>
explicit never_destroyed(Args&&... args) {
// Uses "placement new" to construct a `T` in `storage_`.
new (&storage_) T(std::forward<Args>(args)...);
}
/// Does nothing. Guaranteed!
~never_destroyed() = default;
/// Returns the underlying T reference.
T& access() { return *reinterpret_cast<T*>(&storage_); }
const T& access() const { return *reinterpret_cast<const T*>(&storage_); }
private:
typename std::aligned_storage<sizeof(T), alignof(T)>::type storage_;
};
} // namespace drake

85
wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
View File

@@ -1,85 +0,0 @@
#pragma once
#include <cmath>
#include <cstdlib>
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
namespace drake {
namespace math {
/**
Computes the unique stabilizing solution X to the discrete-time algebraic
Riccati equation:
AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
@throws std::exception if Q is not positive semi-definite.
@throws std::exception if R is not positive definite.
Based on the Schur Vector approach outlined in this paper:
"On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
*/
WPILIB_DLLEXPORT
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R);
/**
Computes the unique stabilizing solution X to the discrete-time algebraic
Riccati equation:
AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
This is equivalent to solving the original DARE:
A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
where A₂ and Q₂ are a change of variables:
A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
This overload of the DARE is useful for finding the control law uₖ that
minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
@verbatim
∞ [xₖ]ᵀ[Q N][xₖ]
J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
k=0
@endverbatim
This is a more general form of the following. The linear-quadratic regulator
is the feedback control law uₖ that minimizes the following cost function
subject to xₖ₊₁ = Axₖ + Buₖ:
@verbatim
∞
J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
k=0
@endverbatim
This can be refactored as:
@verbatim
∞ [xₖ]ᵀ[Q 0][xₖ]
J = Σ [uₖ] [0 R][uₖ] ΔT
k=0
@endverbatim
@throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
@throws std::runtime_error if R is not positive definite.
*/
WPILIB_DLLEXPORT
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& N);
} // namespace math
} // namespace drake