[wpiutil] Add Drake and Drake JNI (#2613)

Drake is a collection of tools for analyzing robot dynamics and building control systems.
See https://drake.mit.edu/ for details.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpiutil/src/main/native/include/drake/common/drake_assert.h

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

wpiutil/src/main/native/include/drake/common/drake_assertion_error.h

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

wpiutil/src/main/native/include/drake/common/drake_copyable.h

@@ -0,0 +1,112 @@
+#pragma once
+
+// ============================================================================
+// N.B. The spelling of the macro names between doc/Doxyfile_CXX.in and this
+// file must be kept in sync!
+// ============================================================================
+
+/** @file
+Provides careful macros to selectively enable or disable the special member
+functions for copy-construction, copy-assignment, move-construction, and
+move-assignment.
+
+http://en.cppreference.com/w/cpp/language/member_functions#Special_member_functions
+
+When enabled via these macros, the `= default` implementation is provided.
+Code that needs custom copy or move functions should not use these macros.
+*/
+
+/** DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for
+copy-construction, copy-assignment, move-construction, and move-assignment.
+Drake's Doxygen is customized to render the deletions in detail, with
+appropriate comments.  Invoke this this macro in the public section of the
+class declaration, e.g.:
+<pre>
+class Foo {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Foo)
+
+  // ...
+};
+</pre>
+*/
+#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)      \
+  Classname(const Classname&) = delete;                 \
+  void operator=(const Classname&) = delete;            \
+  Classname(Classname&&) = delete;                      \
+  void operator=(Classname&&) = delete;
+
+/** DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member
+functions for copy-construction, copy-assignment, move-construction, and
+move-assignment.  This macro should be used only when copy-construction and
+copy-assignment defaults are well-formed.  Note that the defaulted move
+functions could conceivably still be ill-formed, in which case they will
+effectively not be declared or used -- but because the copy constructor exists
+the type will still be MoveConstructible.  Drake's Doxygen is customized to
+render the functions in detail, with appropriate comments.  Invoke this this
+macro in the public section of the class declaration, e.g.:
+<pre>
+class Foo {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Foo)
+
+  // ...
+};
+</pre>
+*/
+#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)       \
+  Classname(const Classname&) = default;                        \
+  Classname& operator=(const Classname&) = default;             \
+  Classname(Classname&&) = default;                             \
+  Classname& operator=(Classname&&) = default;                  \
+  /* Fails at compile-time if default-copy doesn't work. */     \
+  static void DRAKE_COPYABLE_DEMAND_COPY_CAN_COMPILE() {        \
+    (void) static_cast<Classname& (Classname::*)(               \
+        const Classname&)>(&Classname::operator=);              \
+  }
+
+/** DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN declares (but does not define) the
+special member functions for copy-construction, copy-assignment,
+move-construction, and move-assignment.  Drake's Doxygen is customized to
+render the declarations with appropriate comments.
+
+This is useful when paired with DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T
+to work around https://gcc.gnu.org/bugzilla/show_bug.cgi?id=57728 whereby the
+declaration and definition must be split.  Once Drake no longer supports GCC
+versions prior to 6.3, this macro could be removed.
+
+Invoke this this macro in the public section of the class declaration, e.g.:
+<pre>
+template <typename T>
+class Foo {
+ public:
+  DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN(Foo)
+
+  // ...
+};
+DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T(Foo)
+</pre>
+*/
+#define DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN(Classname)       \
+  Classname(const Classname&);                                          \
+  Classname& operator=(const Classname&);                               \
+  Classname(Classname&&);                                               \
+  Classname& operator=(Classname&&);                                    \
+  /* Fails at compile-time if default-copy doesn't work. */             \
+  static void DRAKE_COPYABLE_DEMAND_COPY_CAN_COMPILE() {                \
+    (void) static_cast<Classname& (Classname::*)(                       \
+        const Classname&)>(&Classname::operator=);                      \
+  }
+
+/** Helper for DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN.  Provides defaulted
+definitions for the four special member functions of a templated class.
+*/
+#define DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T(Classname)      \
+  template <typename T>                                                 \
+  Classname<T>::Classname(const Classname<T>&) = default;               \
+  template <typename T>                                                 \
+  Classname<T>& Classname<T>::operator=(const Classname<T>&) = default; \
+  template <typename T>                                                 \
+  Classname<T>::Classname(Classname<T>&&) = default;                    \
+  template <typename T>                                                 \
+  Classname<T>& Classname<T>::operator=(Classname<T>&&) = default;

wpiutil/src/main/native/include/drake/common/drake_throw.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <type_traits>
+
+#include "drake/common/drake_assert.h"
+
+/// @file
+/// Provides a convenient wrapper to throw an exception when a condition is
+/// unmet.  This is similar to an assertion, but uses exceptions instead of
+/// ::abort(), and cannot be disabled.
+
+namespace drake {
+namespace internal {
+// Throw an error message.
+[[noreturn]]
+void Throw(const char* condition, const char* func, const char* file, int line);
+}  // namespace internal
+}  // namespace drake
+
+/// Evaluates @p condition and iff the value is false will throw an exception
+/// with a message showing at least the condition text, function name, file,
+/// and line.
+#define DRAKE_THROW_UNLESS(condition)                                        \
+  do {                                                                       \
+    typedef ::drake::assert::ConditionTraits<                                \
+        typename std::remove_cv<decltype(condition)>::type> Trait;           \
+    static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
+    if (!Trait::Evaluate(condition)) {                                       \
+      ::drake::internal::Throw(#condition, __func__, __FILE__, __LINE__);    \
+    }                                                                        \
+  } while (0)

wpiutil/src/main/native/include/drake/common/is_approx_equal_abstol.h

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

wpiutil/src/main/native/include/drake/common/never_destroyed.h

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

wpiutil/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <cmath>
+#include <cstdlib>
+
+#include <Eigen/Core>
+
+namespace drake {
+namespace math {
+
+/// Computes the unique stabilizing solution X to the discrete-time algebraic
+/// Riccati equation:
+///
+/// \f[
+/// A'XA - X - A'XB(B'XB+R)^{-1}B'XA + Q = 0
+/// \f]
+///
+/// @throws std::runtime_error if Q is not positive semi-definite.
+/// @throws std::runtime_error if R is not positive definite.
+///
+/// Based on the Schur Vector approach outlined in this paper:
+/// "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
+/// by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
+///
+Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
+    const Eigen::Ref<const Eigen::MatrixXd>& A,
+    const Eigen::Ref<const Eigen::MatrixXd>& B,
+    const Eigen::Ref<const Eigen::MatrixXd>& Q,
+    const Eigen::Ref<const Eigen::MatrixXd>& R);
+}  // namespace math
+}  // namespace drake
+