diff --git a/.styleguide b/.styleguide index e05779bf6e..f1b849bb0d 100644 --- a/.styleguide +++ b/.styleguide @@ -22,6 +22,7 @@ repoRootNameOverride { includeOtherLibs { ^cameraserver/ ^cscore + ^drake/ ^hal/ ^imgui ^mockdata/ diff --git a/ThirdPartyNotices.txt b/ThirdPartyNotices.txt index 7dbe1c91da..89e34582a1 100644 --- a/ThirdPartyNotices.txt +++ b/ThirdPartyNotices.txt @@ -45,6 +45,8 @@ Team 254 Library wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineP wpilibc/src/main/native/include/trajectory/TrajectoryParameterizer.h wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp Portable File Dialogs simulation/halsim_gui/src/main/native/include/portable-file-dialogs.h +Drake wpiutil/src/main/native/cpp/drake/drake_assert_and_throw.cpp + wpiutil/src/main/native/cpp/drake/discrete_algebraic_riccati_equation.cpp ============================================================================== @@ -807,3 +809,38 @@ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +============= +Drake Library +============= +All components of Drake are licensed under the BSD 3-Clause License +shown below. Where noted in the source code, some portions may +be subject to other permissive, non-viral licenses. + +Copyright 2012-2016 Robot Locomotion Group @ CSAIL +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are +met: + +Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimer. Redistributions +in binary form must reproduce the above copyright notice, this list of +conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. Neither the name of +the Massachusetts Institute of Technology nor the names of its +contributors may be used to endorse or promote products derived from +this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/simulation/halsim_gui/src/main/native/cpp/EncoderGui.h b/simulation/halsim_gui/src/main/native/cpp/EncoderGui.h index 3e8297bf7b..b7c9dbf6ae 100644 --- a/simulation/halsim_gui/src/main/native/cpp/EncoderGui.h +++ b/simulation/halsim_gui/src/main/native/cpp/EncoderGui.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ diff --git a/wpiutil/.styleguide b/wpiutil/.styleguide index b505fbfb6d..0b96dc5236 100644 --- a/wpiutil/.styleguide +++ b/wpiutil/.styleguide @@ -9,9 +9,11 @@ cppSrcFileInclude { } generatedFileExclude { + src/main/native/cpp/drake/ src/main/native/cpp/http_parser\.cpp$ src/main/native/cpp/llvm/ src/main/native/eigeninclude/ + src/main/native/include/drake/ src/main/native/include/llvm/ src/main/native/include/units/base\.h$ src/main/native/include/units/units\.h$ @@ -72,6 +74,8 @@ generatedFileExclude { src/main/native/resources/ src/test/native/cpp/UnitsTest\.cpp$ src/test/native/cpp/llvm/ + src/test/native/cpp/drake/ + src/test/native/include/drake/ src/main/native/windows/StackWalker } diff --git a/wpiutil/CMakeLists.txt b/wpiutil/CMakeLists.txt index ba1af82b3f..91bbbbaf5a 100644 --- a/wpiutil/CMakeLists.txt +++ b/wpiutil/CMakeLists.txt @@ -5,7 +5,7 @@ include(GenResources) include(CompileWarnings) include(AddTest) -file(GLOB wpiutil_jni_src src/main/native/cpp/jni/WPIUtilJNI.cpp) +file(GLOB wpiutil_jni_src src/main/native/cpp/jni/DrakeJNI.cpp src/main/native/cpp/jni/WPIUtilJNI.cpp) # Java bindings if (NOT WITHOUT_JAVA) @@ -175,7 +175,7 @@ set_property(TARGET wpiutil PROPERTY FOLDER "libraries") target_compile_features(wpiutil PUBLIC cxx_std_17) if (MSVC) - target_compile_options(wpiutil PUBLIC /permissive- /Zc:throwingNew /MP) + target_compile_options(wpiutil PUBLIC /permissive- /Zc:throwingNew /MP /bigobj) target_compile_definitions(wpiutil PRIVATE -D_CRT_SECURE_NO_WARNINGS) endif() wpilib_target_warnings(wpiutil) @@ -277,5 +277,6 @@ set_property(TARGET netconsoleTee PROPERTY FOLDER "examples") if (WITH_TESTS) wpilib_add_test(wpiutil src/test/native/cpp) + target_include_directories(wpiutil_test PRIVATE src/test/native/include) target_link_libraries(wpiutil_test wpiutil ${LIBUTIL} gmock_main) endif() diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Drake.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Drake.java new file mode 100644 index 0000000000..a98fa57337 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Drake.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpiutil.math; + +import org.ejml.simple.SimpleMatrix; + +public final class Drake { + private Drake() { + } + + /** + * Solves the discrete alegebraic Riccati equation. + * + * @param A System matrix. + * @param B Input matrix. + * @param Q State cost matrix. + * @param R Input cost matrix. + * @return Solution of DARE. + */ + @SuppressWarnings({"LocalVariableName", "ParameterName"}) + public static SimpleMatrix discreteAlgebraicRiccatiEquation( + SimpleMatrix A, + SimpleMatrix B, + SimpleMatrix Q, + SimpleMatrix R) { + var S = new SimpleMatrix(A.numRows(), A.numCols()); + DrakeJNI.discreteAlgebraicRiccatiEquation(A.getDDRM().getData(), B.getDDRM().getData(), + Q.getDDRM().getData(), R.getDDRM().getData(), A.numCols(), B.numCols(), + S.getDDRM().getData()); + return S; + } + + /** + * Solves the discrete alegebraic Riccati equation. + * + * @param A System matrix. + * @param B Input matrix. + * @param Q State cost matrix. + * @param R Input cost matrix. + * @return Solution of DARE. + */ + @SuppressWarnings("ParameterName") + public static SimpleMatrix discreteAlgebraicRiccatiEquation( + Matrix A, + Matrix B, + Matrix Q, + Matrix R) { + return discreteAlgebraicRiccatiEquation(A.getStorage(), B.getStorage(), Q.getStorage(), + R.getStorage()); + } +} diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/DrakeJNI.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/DrakeJNI.java new file mode 100644 index 0000000000..d2c706ff9b --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/DrakeJNI.java @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpiutil.math; + +import java.io.IOException; +import java.util.concurrent.atomic.AtomicBoolean; + +import edu.wpi.first.wpiutil.RuntimeLoader; + +public final class DrakeJNI { + static boolean libraryLoaded = false; + static RuntimeLoader loader = null; + + static { + if (Helper.getExtractOnStaticLoad()) { + try { + loader = new RuntimeLoader<>("wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(), + DrakeJNI.class); + loader.loadLibrary(); + } catch (IOException ex) { + ex.printStackTrace(); + System.exit(1); + } + libraryLoaded = true; + } + } + + /** + * Force load the library. + * + * @throws IOException If the library could not be loaded or found. + */ + public static synchronized void forceLoad() throws IOException { + if (libraryLoaded) { + return; + } + loader = new RuntimeLoader<>("wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(), + DrakeJNI.class); + loader.loadLibrary(); + libraryLoaded = true; + } + + /** + * Solves the discrete alegebraic Riccati equation. + * + * @param A Array containing elements of A in row-major order. + * @param B Array containing elements of B in row-major order. + * @param Q Array containing elements of Q in row-major order. + * @param R Array containing elements of R in row-major order. + * @param states Number of states in A matrix. + * @param inputs Number of inputs in B matrix. + * @param S Array storage for DARE solution. + */ + public static native void discreteAlgebraicRiccatiEquation( + double[] A, + double[] B, + double[] Q, + double[] R, + int states, + int inputs, + double[] S); + + public static class Helper { + private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true); + + public static boolean getExtractOnStaticLoad() { + return extractOnStaticLoad.get(); + } + + public static void setExtractOnStaticLoad(boolean load) { + extractOnStaticLoad.set(load); + } + } +} diff --git a/wpiutil/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp b/wpiutil/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp new file mode 100644 index 0000000000..88e7e66289 --- /dev/null +++ b/wpiutil/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp @@ -0,0 +1,87 @@ +// This file contains the implementation of both drake_assert and drake_throw. +/* clang-format off to disable clang-format-includes */ +#include "drake/common/drake_assert.h" +#include "drake/common/drake_throw.h" +/* clang-format on */ + +#include +#include +#include +#include +#include +#include + +#include "drake/common/drake_assertion_error.h" +#include "drake/common/never_destroyed.h" + +namespace drake { +namespace internal { +namespace { + +// Singleton to manage assertion configuration. +struct AssertionConfig { + static AssertionConfig& singleton() { + static never_destroyed global; + return global.access(); + } + + std::atomic assertion_failures_are_exceptions; +}; + +// Stream into @p out the given failure details; only @p condition may be null. +void PrintFailureDetailTo(std::ostream& out, const char* condition, + const char* func, const char* file, int line) { + out << "Failure at " << file << ":" << line << " in " << func << "()"; + if (condition) { + out << ": condition '" << condition << "' failed."; + } else { + out << "."; + } +} +} // namespace + +// Declared in drake_assert.h. +void Abort(const char* condition, const char* func, const char* file, + int line) { + std::cerr << "abort: "; + PrintFailureDetailTo(std::cerr, condition, func, file, line); + std::cerr << std::endl; + std::abort(); +} + +// Declared in drake_throw.h. +void Throw(const char* condition, const char* func, const char* file, + int line) { + std::ostringstream what; + PrintFailureDetailTo(what, condition, func, file, line); + throw assertion_error(what.str().c_str()); +} + +// Declared in drake_assert.h. +void AssertionFailed(const char* condition, const char* func, const char* file, + int line) { + if (AssertionConfig::singleton().assertion_failures_are_exceptions) { + Throw(condition, func, file, line); + } else { + Abort(condition, func, file, line); + } +} + +} // namespace internal +} // namespace drake + +// Configures the DRAKE_ASSERT and DRAKE_DEMAND assertion failure handling +// behavior. +// +// By default, assertion failures will result in an ::abort(). If this method +// has ever been called, failures will result in a thrown exception instead. +// +// Assertion configuration has process-wide scope. Changes here will affect +// all assertions within the current process. +// +// This method is intended ONLY for use by pydrake bindings, and thus is not +// declared in any header file, to discourage anyone from using it. +extern "C" void drake_set_assertion_failure_to_throw_exception() { + drake::internal::AssertionConfig::singleton(). + assertion_failures_are_exceptions = true; +} diff --git a/wpiutil/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp b/wpiutil/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp new file mode 100644 index 0000000000..e80cadc34f --- /dev/null +++ b/wpiutil/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp @@ -0,0 +1,463 @@ +#include "drake/math/discrete_algebraic_riccati_equation.h" + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_throw.h" +#include "drake/common/is_approx_equal_abstol.h" + +#include +#include + +// This code has https://github.com/RobotLocomotion/drake/pull/11118 applied to +// fix an infinite loop in reorder_eigen(). + +namespace drake { +namespace math { +namespace { +/* helper functions */ +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} +void check_stabilizable(const Eigen::Ref& A, + const Eigen::Ref& B) { + // This function checks if (A,B) is a stabilizable pair. + // (A,B) is stabilizable if and only if the uncontrollable eigenvalues of + // A, if any, have absolute values less than one, where an eigenvalue is + // uncontrollable if Rank[lambda * I - A, B] < n. + int n = B.rows(), m = B.cols(); + Eigen::EigenSolver es(A); + for (int i = 0; i < n; i++) { + if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() + + es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() < + 1) + continue; + Eigen::MatrixXcd E(n, n + m); + E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B; + Eigen::ColPivHouseholderQR qr(E); + DRAKE_THROW_UNLESS(qr.rank() == n); + } +} +void check_detectable(const Eigen::Ref& A, + const Eigen::Ref& Q) { + // This function check if (A,C) is a detectable pair, where Q = C' * C. + // (A,C) is detectable if and only if the unobservable eigenvalues of A, + // if any, have absolute values less than one, where an eigenvalue is + // unobservable if Rank[lambda * I - A; C] < n. + // Also, (A,C) is detectable if and only if (A',C') is stabilizable. + int n = A.rows(); + Eigen::LDLT ldlt(Q); + Eigen::MatrixXd L = ldlt.matrixL(); + Eigen::MatrixXd D = ldlt.vectorD(); + Eigen::MatrixXd D_sqrt = Eigen::MatrixXd::Zero(n, n); + for (int i = 0; i < n; i++) { + D_sqrt(i, i) = sqrt(D(i)); + } + Eigen::MatrixXd C = L * D_sqrt; + check_stabilizable(A.transpose(), C.transpose()); +} + +// "Givens rotation" computes an orthogonal 2x2 matrix R such that +// it eliminates the 2nd coordinate of the vector [a,b]', i.e., +// R * [ a ] = [ a_hat ] +// [ b ] [ 0 ] +// The implementation is based on +// https://en.wikipedia.org/wiki/Givens_rotation#Stable_calculation +void Givens_rotation(double a, double b, Eigen::Ref R, + double eps = 1e-10) { + double c, s; + if (fabs(b) < eps) { + c = (a < -eps ? -1 : 1); + s = 0; + } else if (fabs(a) < eps) { + c = 0; + s = -sgn(b); + } else if (fabs(a) > fabs(b)) { + double t = b / a; + double u = sgn(a) * fabs(sqrt(1 + t * t)); + c = 1 / u; + s = -c * t; + } else { + double t = a / b; + double u = sgn(b) * fabs(sqrt(1 + t * t)); + s = -1 / u; + c = -s * t; + } + R(0, 0) = c, R(0, 1) = -s, R(1, 0) = s, R(1, 1) = c; +} + +// The arguments S, T, and Z will be changed. +void swap_block_11(Eigen::Ref S, Eigen::Ref T, + Eigen::Ref Z, int p) { + // Dooren, Case I, p124-125. + int n2 = S.rows(); + Eigen::Matrix2d A = S.block<2, 2>(p, p); + Eigen::Matrix2d B = T.block<2, 2>(p, p); + Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2); + Eigen::Matrix2d H = A(1, 1) * B - B(1, 1) * A; + Givens_rotation(H(0, 1), H(0, 0), Z1.block<2, 2>(p, p)); + S = (S * Z1).eval(); + T = (T * Z1).eval(); + Z = (Z * Z1).eval(); + Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(n2, n2); + Givens_rotation(T(p, p), T(p + 1, p), Q.block<2, 2>(p, p)); + S = (Q * S).eval(); + T = (Q * T).eval(); + S(p + 1, p) = 0; + T(p + 1, p) = 0; +} + +// The arguments S, T, and Z will be changed. +void swap_block_21(Eigen::Ref S, Eigen::Ref T, + Eigen::Ref Z, int p) { + // Dooren, Case II, p126-127. + int n2 = S.rows(); + Eigen::Matrix3d A = S.block<3, 3>(p, p); + Eigen::Matrix3d B = T.block<3, 3>(p, p); + // Compute H and eliminate H(1,0) by row operation. + Eigen::Matrix3d H = A(2, 2) * B - B(2, 2) * A; + Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3); + Givens_rotation(H(0, 0), H(1, 0), R.block<2, 2>(0, 0)); + H = (R * H).eval(); + Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2); + Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2); + Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2); + Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2); + // Compute Z1, Z2, Q1, Q2. + Givens_rotation(H(1, 2), H(1, 1), Z1.block<2, 2>(p + 1, p + 1)); + H = (H * Z1.block<3, 3>(p, p)).eval(); + Givens_rotation(H(0, 1), H(0, 0), Z2.block<2, 2>(p, p)); + S = (S * Z1).eval(); + T = (T * Z1).eval(); + Z = (Z * Z1 * Z2).eval(); + Givens_rotation(T(p + 1, p + 1), T(p + 2, p + 1), + Q1.block<2, 2>(p + 1, p + 1)); + S = (Q1 * S * Z2).eval(); + T = (Q1 * T * Z2).eval(); + Givens_rotation(T(p, p), T(p + 1, p), Q2.block<2, 2>(p, p)); + S = (Q2 * S).eval(); + T = (Q2 * T).eval(); + S(p + 1, p) = 0; + S(p + 2, p) = 0; + T(p + 1, p) = 0; + T(p + 2, p) = 0; + T(p + 2, p + 1) = 0; +} + +// The arguments S, T, and Z will be changed. +void swap_block_12(Eigen::Ref S, Eigen::Ref T, + Eigen::Ref Z, int p) { + int n2 = S.rows(); + // Swap the role of S and T. + Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2); + Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2); + Eigen::MatrixXd Q0 = Eigen::MatrixXd::Identity(n2, n2); + Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2); + Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2); + Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2); + Givens_rotation(S(p + 1, p + 1), S(p + 2, p + 1), + Q0.block<2, 2>(p + 1, p + 1)); + S = (Q0 * S).eval(); + T = (Q0 * T).eval(); + Eigen::Matrix3d A = S.block<3, 3>(p, p); + Eigen::Matrix3d B = T.block<3, 3>(p, p); + // Compute H and eliminate H(2,1) by column operation. + Eigen::Matrix3d H = B(0, 0) * A - A(0, 0) * B; + Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3); + Givens_rotation(H(2, 2), H(2, 1), R.block<2, 2>(1, 1)); + H = (H * R).eval(); + // Compute Q1, Q2, Z1, Z2. + Givens_rotation(H(0, 1), H(1, 1), Q1.block<2, 2>(p, p)); + H = (Q1.block<3, 3>(p, p) * H).eval(); + Givens_rotation(H(1, 2), H(2, 2), Q2.block<2, 2>(p + 1, p + 1)); + S = (Q1 * S).eval(); + T = (Q1 * T).eval(); + Givens_rotation(S(p + 1, p + 1), S(p + 1, p), Z1.block<2, 2>(p, p)); + S = (Q2 * S * Z1).eval(); + T = (Q2 * T * Z1).eval(); + Givens_rotation(S(p + 2, p + 2), S(p + 2, p + 1), + Z2.block<2, 2>(p + 1, p + 1)); + S = (S * Z2).eval(); + T = (T * Z2).eval(); + Z = (Z * Z1 * Z2).eval(); + // Swap back the role of S and T. + Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p)); + S = (Q3 * S).eval(); + T = (Q3 * T).eval(); + S(p + 2, p) = 0; + S(p + 2, p + 1) = 0; + T(p + 1, p) = 0; + T(p + 2, p) = 0; + T(p + 2, p + 1) = 0; +} + +// The arguments S, T, and Z will be changed. +void swap_block_22(Eigen::Ref S, Eigen::Ref T, + Eigen::Ref Z, int p) { + // Direct Swapping Algorithm based on + // "Numerical Methods for General and Structured Eigenvalue Problems" by + // Daniel Kressner, p108-111. + // ( http://sma.epfl.ch/~anchpcommon/publications/kressner_eigenvalues.pdf ). + // Also relevant but not applicable here: + // "On Swapping Diagonal Blocks in Real Schur Form" by Zhaojun Bai and James + // W. Demmelt; + int n2 = S.rows(); + Eigen::MatrixXd A = S.block<4, 4>(p, p); + Eigen::MatrixXd B = T.block<4, 4>(p, p); + // Solve + // A11 * X - Y A22 = A12 + // B11 * X - Y B22 = B12 + // Reduce to solve Cx=D, where x=[x1;...;x4;y1;...;y4]. + Eigen::Matrix C = Eigen::Matrix::Zero(); + Eigen::Matrix D; + // clang-format off + C << A(0, 0), 0, A(0, 1), 0, -A(2, 2), -A(3, 2), 0, 0, + 0, A(0, 0), 0, A(0, 1), -A(2, 3), -A(3, 3), 0, 0, + A(1, 0), 0, A(1, 1), 0, 0, 0, -A(2, 2), -A(3, 2), + 0, A(1, 0), 0, A(1, 1), 0, 0, -A(2, 3), -A(3, 3), + B(0, 0), 0, B(0, 1), 0, -B(2, 2), -B(3, 2), 0, 0, + 0, B(0, 0), 0, B(0, 1), -B(2, 3), -B(3, 3), 0, 0, + B(1, 0), 0, B(1, 1), 0, 0, 0, -B(2, 2), -B(3, 2), + 0, B(1, 0), 0, B(1, 1), 0, 0, -B(2, 3), -B(3, 3); + // clang-format on + D << A(0, 2), A(0, 3), A(1, 2), A(1, 3), B(0, 2), B(0, 3), B(1, 2), B(1, 3); + Eigen::MatrixXd x = C.colPivHouseholderQr().solve(D); + // Q * [ -Y ] = [ R_Y ] , Z' * [ -X ] = [ R_X ] . + // [ I ] [ 0 ] [ I ] = [ 0 ] + Eigen::Matrix X, Y; + X << -x(0, 0), -x(1, 0), -x(2, 0), -x(3, 0), Eigen::Matrix2d::Identity(); + Y << -x(4, 0), -x(5, 0), -x(6, 0), -x(7, 0), Eigen::Matrix2d::Identity(); + Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2); + Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2); + Eigen::ColPivHouseholderQR > qr1(X); + Z1.block<4, 4>(p, p) = qr1.householderQ(); + Eigen::ColPivHouseholderQR > qr2(Y); + Q1.block<4, 4>(p, p) = qr2.householderQ().adjoint(); + // Apply transform Q1 * (S,T) * Z1. + S = (Q1 * S * Z1).eval(); + T = (Q1 * T * Z1).eval(); + Z = (Z * Z1).eval(); + // Eliminate the T(p+3,p+2) entry. + Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2); + Givens_rotation(T(p + 2, p + 2), T(p + 3, p + 2), + Q2.block<2, 2>(p + 2, p + 2)); + S = (Q2 * S).eval(); + T = (Q2 * T).eval(); + // Eliminate the T(p+1,p) entry. + Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2); + Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p)); + S = (Q3 * S).eval(); + T = (Q3 * T).eval(); + S(p + 2, p) = 0; + S(p + 2, p + 1) = 0; + S(p + 3, p) = 0; + S(p + 3, p + 1) = 0; + T(p + 1, p) = 0; + T(p + 2, p) = 0; + T(p + 2, p + 1) = 0; + T(p + 3, p) = 0; + T(p + 3, p + 1) = 0; + T(p + 3, p + 2) = 0; +} + +// Functionality of "swap_block" function: +// swap the 1x1 or 2x2 blocks pointed by p and q. +// There are four cases: swapping 1x1 and 1x1 matrices, swapping 2x2 and 1x1 +// matrices, swapping 1x1 and 2x2 matrices, and swapping 2x2 and 2x2 matrices. +// Algorithms are described in the papers +// "A generalized eigenvalue approach for solving Riccati equations" by P. Van +// Dooren, 1981 ( http://epubs.siam.org/doi/pdf/10.1137/0902010 ), and +// "Numerical Methods for General and Structured Eigenvalue Problems" by +// Daniel Kressner, 2005. +void swap_block(Eigen::Ref S, Eigen::Ref T, + Eigen::Ref Z, int p, int q, int q_block_size, + double eps = 1e-10) { + int p_tmp = q, p_block_size; + while (p_tmp-- > p) { + p_block_size = 1; + if (p_tmp >= 1 && fabs(S(p_tmp, p_tmp - 1)) > eps) { + p_block_size = 2; + p_tmp--; + } + switch (p_block_size * 10 + q_block_size) { + case 11: + swap_block_11(S, T, Z, p_tmp); + break; + case 21: + swap_block_21(S, T, Z, p_tmp); + break; + case 12: + swap_block_12(S, T, Z, p_tmp); + break; + case 22: + swap_block_22(S, T, Z, p_tmp); + break; + } + } +} + +// Functionality of "reorder_eigen" function: +// Reorder the eigenvalues of (S,T) such that the top-left n by n matrix has +// stable eigenvalues by multiplying Q's and Z's on the left and the right, +// respectively. +// Stable eigenvalues are inside the unit disk. +// +// Algorithm: +// Go along the diagonals of (S,T) from the top left to the bottom right. +// Once find a stable eigenvalue, push it to top left. +// In implementation, use two pointers, p and q. +// p points to the current block (1x1 or 2x2) and q points to the block with the +// stable eigenvalue(s). +// Push the block pointed by q to the position pointed by p. +// Finish when n stable eigenvalues are placed at the top-left n by n matrix. +// The algorithm for swapping blocks is described in the papers +// "A generalized eigenvalue approach for solving Riccati equations" by P. Van +// Dooren, 1981, and "Numerical Methods for General and Structured Eigenvalue +// Problems" by Daniel Kressner, 2005. +void reorder_eigen(Eigen::Ref S, Eigen::Ref T, + Eigen::Ref Z, double eps = 1e-10) { + // abs(a) < eps => a = 0 + int n2 = S.rows(); + int n = n2 / 2, p = 0, q = 0; + + // Find the first unstable p block. + while (p < n) { + if (fabs(S(p + 1, p)) < eps) { // p block size = 1 + if (fabs(T(p, p)) > eps && fabs(S(p, p)) <= fabs(T(p, p))) { // stable + p++; + continue; + } + } else { // p block size = 2 + const double det_T = + T(p, p) * T(p + 1, p + 1) - T(p + 1, p) * T(p, p + 1); + if (fabs(det_T) > eps) { + const double det_S = + S(p, p) * S(p + 1, p + 1) - S(p + 1, p) * S(p, p + 1); + if (fabs(det_S) <= fabs(det_T)) { // stable + p += 2; + continue; + } + } + } + break; + } + q = p; + + // Make the first n generalized eigenvalues stable. + while (p < n && q < n2) { + // Update q. + int q_block_size = 0; + while (q < n2) { + if (q == n2 - 1 || fabs(S(q + 1, q)) < eps) { // block size = 1 + if (fabs(T(q, q)) > eps && fabs(S(q, q)) <= fabs(T(q, q))) { + q_block_size = 1; + break; + } + q++; + } else { // block size = 2 + const double det_T = + T(q, q) * T(q + 1, q + 1) - T(q + 1, q) * T(q, q + 1); + if (fabs(det_T) > eps) { + const double det_S = + S(q, q) * S(q + 1, q + 1) - S(q + 1, q) * S(q, q + 1); + if (fabs(det_S) <= fabs(det_T)) { + q_block_size = 2; + break; + } + } + q += 2; + } + } + if (q >= n2) + throw std::runtime_error("fail to find enough stable eigenvalues"); + // Swap blocks pointed by p and q. + if (p != q) { + swap_block(S, T, Z, p, q, q_block_size); + p += q_block_size; + q += q_block_size; + } + } + if (p < n && q >= n2) + throw std::runtime_error("fail to find enough stable eigenvalues"); +} +} // namespace + +/** + * DiscreteAlgebraicRiccatiEquation function + * 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, in TAC, 1980, + * http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1102434 + * + * Note: When, for example, n = 100, m = 80, and entries of A, B, Q_half, + * R_half are sampled from standard normal distributions, where + * Q = Q_half'*Q_half and similar for R, the absolute error of the solution + * is 10^{-6}, while the absolute error of the solution computed by Matlab is + * 10^{-8}. + * + * TODO(weiqiao.han): I may overwrite the RealQZ function to improve the + * accuracy, together with more thorough tests. + */ + +Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( + const Eigen::Ref& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R) { + int n = B.rows(), m = B.cols(); + + DRAKE_DEMAND(m <= n); + DRAKE_DEMAND(A.rows() == n && A.cols() == n); + DRAKE_DEMAND(Q.rows() == n && Q.cols() == n); + DRAKE_DEMAND(R.rows() == m && R.cols() == m); + DRAKE_DEMAND(is_approx_equal_abstol(Q, Q.transpose(), 1e-10)); + Eigen::SelfAdjointEigenSolver es(Q); + for (int i = 0; i < n; i++) { + DRAKE_THROW_UNLESS(es.eigenvalues()[i] >= 0); + } + DRAKE_DEMAND(is_approx_equal_abstol(R, R.transpose(), 1e-10)); + Eigen::LLT R_cholesky(R); + DRAKE_THROW_UNLESS(R_cholesky.info() == Eigen::Success); + check_stabilizable(A, B); + check_detectable(A, Q); + + Eigen::MatrixXd M(2 * n, 2 * n), L(2 * n, 2 * n); + M << A, Eigen::MatrixXd::Zero(n, n), -Q, Eigen::MatrixXd::Identity(n, n); + L << Eigen::MatrixXd::Identity(n, n), B * R.inverse() * B.transpose(), + Eigen::MatrixXd::Zero(n, n), A.transpose(); + + // QZ decomposition of M and L + // QMZ = S, QLZ = T + // where Q and Z are real orthogonal matrixes + // T is upper-triangular matrix, and S is upper quasi-triangular matrix + Eigen::RealQZ qz(2 * n); + qz.compute(M, L); // M = Q S Z, L = Q T Z (Q and Z computed by Eigen package + // are adjoints of Q and Z above) + Eigen::MatrixXd S = qz.matrixS(), T = qz.matrixT(), + Z = qz.matrixZ().adjoint(); + + // Reorder the generalized eigenvalues of (S,T). + Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(2 * n, 2 * n); + reorder_eigen(S, T, Z2); + Z = (Z * Z2).eval(); + + // The first n columns of Z is ( U1 ) . + // ( U2 ) + // -1 + // X = U2 * U1 is a solution of the discrete time Riccati equation. + Eigen::MatrixXd U1 = Z.block(0, 0, n, n), U2 = Z.block(n, 0, n, n); + Eigen::MatrixXd X = U2 * U1.inverse(); + X = (X + X.adjoint().eval()) / 2.0; + return X; +} + +} // namespace math +} // namespace drake diff --git a/wpiutil/src/main/native/cpp/jni/DrakeJNI.cpp b/wpiutil/src/main/native/cpp/jni/DrakeJNI.cpp new file mode 100644 index 0000000000..8e5f4da4f9 --- /dev/null +++ b/wpiutil/src/main/native/cpp/jni/DrakeJNI.cpp @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include + +#include +#include + +#include "drake/math/discrete_algebraic_riccati_equation.h" +#include "edu_wpi_first_wpiutil_math_DrakeJNI.h" +#include "wpi/jni_util.h" + +using namespace wpi::java; + +extern "C" { + +/* + * Class: edu_wpi_first_wpiutil_math_DrakeJNI + * Method: discreteAlgebraicRiccatiEquation + * Signature: ([D[D[D[DII[D)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpiutil_math_DrakeJNI_discreteAlgebraicRiccatiEquation + (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q, + jdoubleArray R, jint states, jint inputs, jdoubleArray S) +{ + jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr); + jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr); + jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr); + jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr); + + Eigen::Map< + Eigen::Matrix> + Amat{nativeA, states, states}; + Eigen::Map< + Eigen::Matrix> + Bmat{nativeB, states, inputs}; + Eigen::Map< + Eigen::Matrix> + Qmat{nativeQ, states, states}; + Eigen::Map< + Eigen::Matrix> + Rmat{nativeR, inputs, inputs}; + + Eigen::MatrixXd result = + drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat); + + env->ReleaseDoubleArrayElements(A, nativeA, 0); + env->ReleaseDoubleArrayElements(B, nativeB, 0); + env->ReleaseDoubleArrayElements(Q, nativeQ, 0); + env->ReleaseDoubleArrayElements(R, nativeR, 0); + + env->SetDoubleArrayRegion(S, 0, states * states, result.data()); +} + +} // extern "C" diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff b/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff new file mode 100644 index 0000000000..abf5b7d678 --- /dev/null +++ b/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff @@ -0,0 +1,40 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud +// +// 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 EIGEN_AUTODIFF_MODULE +#define EIGEN_AUTODIFF_MODULE + +namespace Eigen { + +/** + * \defgroup AutoDiff_Module Auto Diff module + * + * This module features forward automatic differentation via a simple + * templated scalar type wrapper AutoDiffScalar. + * + * Warning : this should NOT be confused with numerical differentiation, which + * is a different method and has its own module in Eigen : \ref NumericalDiff_Module. + * + * \code + * #include + * \endcode + */ +//@{ + +} + +#include "src/AutoDiff/AutoDiffScalar.h" +// #include "src/AutoDiff/AutoDiffVector.h" +#include "src/AutoDiff/AutoDiffJacobian.h" + +namespace Eigen { +//@} +} + +#endif // EIGEN_AUTODIFF_MODULE diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h b/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h new file mode 100644 index 0000000000..33b6c393f4 --- /dev/null +++ b/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h @@ -0,0 +1,108 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Gael Guennebaud +// +// 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 EIGEN_AUTODIFF_JACOBIAN_H +#define EIGEN_AUTODIFF_JACOBIAN_H + +namespace Eigen +{ + +template class AutoDiffJacobian : public Functor +{ +public: + AutoDiffJacobian() : Functor() {} + AutoDiffJacobian(const Functor& f) : Functor(f) {} + + // forward constructors +#if EIGEN_HAS_VARIADIC_TEMPLATES + template + AutoDiffJacobian(const T& ...Values) : Functor(Values...) {} +#else + template + AutoDiffJacobian(const T0& a0) : Functor(a0) {} + template + AutoDiffJacobian(const T0& a0, const T1& a1) : Functor(a0, a1) {} + template + AutoDiffJacobian(const T0& a0, const T1& a1, const T2& a2) : Functor(a0, a1, a2) {} +#endif + + typedef typename Functor::InputType InputType; + typedef typename Functor::ValueType ValueType; + typedef typename ValueType::Scalar Scalar; + + enum { + InputsAtCompileTime = InputType::RowsAtCompileTime, + ValuesAtCompileTime = ValueType::RowsAtCompileTime + }; + + typedef Matrix JacobianType; + typedef typename JacobianType::Index Index; + + typedef Matrix DerivativeType; + typedef AutoDiffScalar ActiveScalar; + + typedef Matrix ActiveInput; + typedef Matrix ActiveValue; + +#if EIGEN_HAS_VARIADIC_TEMPLATES + // Some compilers don't accept variadic parameters after a default parameter, + // i.e., we can't just write _jac=0 but we need to overload operator(): + EIGEN_STRONG_INLINE + void operator() (const InputType& x, ValueType* v) const + { + this->operator()(x, v, 0); + } + template + void operator() (const InputType& x, ValueType* v, JacobianType* _jac, + const ParamsType&... Params) const +#else + void operator() (const InputType& x, ValueType* v, JacobianType* _jac=0) const +#endif + { + eigen_assert(v!=0); + + if (!_jac) + { +#if EIGEN_HAS_VARIADIC_TEMPLATES + Functor::operator()(x, v, Params...); +#else + Functor::operator()(x, v); +#endif + return; + } + + JacobianType& jac = *_jac; + + ActiveInput ax = x.template cast(); + ActiveValue av(jac.rows()); + + if(InputsAtCompileTime==Dynamic) + for (Index j=0; j +// +// 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 EIGEN_AUTODIFF_SCALAR_H +#define EIGEN_AUTODIFF_SCALAR_H + +namespace Eigen { + +namespace internal { + +template +struct make_coherent_impl { + static void run(A&, B&) {} +}; + +// resize a to match b is a.size()==0, and conversely. +template +void make_coherent(const A& a, const B&b) +{ + make_coherent_impl::run(a.const_cast_derived(), b.const_cast_derived()); +} + +template struct auto_diff_special_op; + +} // end namespace internal + +template class AutoDiffScalar; + +template +inline AutoDiffScalar MakeAutoDiffScalar(const typename NewDerType::Scalar& value, const NewDerType &der) { + return AutoDiffScalar(value,der); +} + +/** \class AutoDiffScalar + * \brief A scalar type replacement with automatic differentation capability + * + * \param _DerType the vector type used to store/represent the derivatives. The base scalar type + * as well as the number of derivatives to compute are determined from this type. + * Typical choices include, e.g., \c Vector4f for 4 derivatives, or \c VectorXf + * if the number of derivatives is not known at compile time, and/or, the number + * of derivatives is large. + * Note that _DerType can also be a reference (e.g., \c VectorXf&) to wrap a + * existing vector into an AutoDiffScalar. + * Finally, _DerType can also be any Eigen compatible expression. + * + * This class represents a scalar value while tracking its respective derivatives using Eigen's expression + * template mechanism. + * + * It supports the following list of global math function: + * - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos, + * - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos, + * - internal::conj, internal::real, internal::imag, numext::abs2. + * + * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However, + * in that case, the expression template mechanism only occurs at the top Matrix level, + * while derivatives are computed right away. + * + */ + +template +class AutoDiffScalar + : public internal::auto_diff_special_op + <_DerType, !internal::is_same::type>::Scalar, + typename NumTraits::type>::Scalar>::Real>::value> +{ + public: + typedef internal::auto_diff_special_op + <_DerType, !internal::is_same::type>::Scalar, + typename NumTraits::type>::Scalar>::Real>::value> Base; + typedef typename internal::remove_all<_DerType>::type DerType; + typedef typename internal::traits::Scalar Scalar; + typedef typename NumTraits::Real Real; + + using Base::operator+; + using Base::operator*; + + /** Default constructor without any initialization. */ + AutoDiffScalar() {} + + /** Constructs an active scalar from its \a value, + and initializes the \a nbDer derivatives such that it corresponds to the \a derNumber -th variable */ + AutoDiffScalar(const Scalar& value, int nbDer, int derNumber) + : m_value(value), m_derivatives(DerType::Zero(nbDer)) + { + m_derivatives.coeffRef(derNumber) = Scalar(1); + } + + /** Conversion from a scalar constant to an active scalar. + * The derivatives are set to zero. */ + /*explicit*/ AutoDiffScalar(const Real& value) + : m_value(value) + { + if(m_derivatives.size()>0) + m_derivatives.setZero(); + } + + /** Constructs an active scalar from its \a value and derivatives \a der */ + 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::type>::Scalar>::value + && internal::is_convertible::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 operator const Scalar& () const { return m_value; } +// inline operator Scalar& () { return m_value; } + + 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 const AutoDiffScalar operator+(const Real& other) const +// { +// return AutoDiffScalar(m_value + other, m_derivatives); +// } + +// friend inline const AutoDiffScalar operator+(const Real& 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,const DerType,const typename internal::remove_all::type> > + operator+(const AutoDiffScalar& other) const + { + internal::make_coherent(m_derivatives, other.derivatives()); + return AutoDiffScalar,const DerType,const typename internal::remove_all::type> >( + m_value + other.value(), + m_derivatives + 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, const DerType> > + operator-(const Scalar& a, const AutoDiffScalar& b) + { + return AutoDiffScalar, const DerType> > + (a - b.value(), -b.derivatives()); + } + + inline AutoDiffScalar& operator-=(const Scalar& other) + { + value() -= other; + return *this; + } + + template + inline const AutoDiffScalar, const DerType,const typename internal::remove_all::type> > + operator-(const AutoDiffScalar& other) const + { + internal::make_coherent(m_derivatives, other.derivatives()); + return AutoDiffScalar, const DerType,const typename internal::remove_all::type> >( + m_value - other.value(), + m_derivatives - other.derivatives()); + } + + template + inline AutoDiffScalar& + operator-=(const AutoDiffScalar& other) + { + *this = *this - other; + return *this; + } + + inline const AutoDiffScalar, const DerType> > + operator-() const + { + return AutoDiffScalar, const DerType> >( + -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, DerType>::Type > +// operator*(const Real& other) const +// { +// return AutoDiffScalar, DerType>::Type >( +// m_value * other, +// (m_derivatives * other)); +// } +// +// friend inline const AutoDiffScalar, DerType>::Type > +// operator*(const Real& other, const AutoDiffScalar& a) +// { +// return AutoDiffScalar, DerType>::Type >( +// 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()))); + } + +// inline const AutoDiffScalar, DerType>::Type > +// operator/(const Real& other) const +// { +// return AutoDiffScalar, DerType>::Type >( +// m_value / other, +// (m_derivatives * (Real(1)/other))); +// } +// +// friend inline const AutoDiffScalar, DerType>::Type > +// operator/(const Real& other, const AutoDiffScalar& a) +// { +// return AutoDiffScalar, DerType>::Type >( +// other / a.value(), +// a.derivatives() * (-Real(1)/other)); +// } + + template + inline const AutoDiffScalar EIGEN_COMMA + const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) EIGEN_COMMA + const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all::type,Scalar,product) >,Scalar,product) > + operator/(const AutoDiffScalar& other) const + { + internal::make_coherent(m_derivatives, other.derivatives()); + return MakeAutoDiffScalar( + m_value / other.value(), + ((m_derivatives * other.value()) - (other.derivatives() * m_value)) + * (Scalar(1)/(other.value()*other.value()))); + } + + template + inline const AutoDiffScalar, + const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product), + const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all::type,Scalar,product) > > + operator*(const AutoDiffScalar& other) const + { + internal::make_coherent(m_derivatives, other.derivatives()); + return MakeAutoDiffScalar( + m_value * other.value(), + (m_derivatives * other.value()) + (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; + +}; + +namespace internal { + +template +struct auto_diff_special_op<_DerType, true> +// : auto_diff_scalar_op<_DerType, typename NumTraits::Real, +// is_same::Real>::value> +{ + typedef typename remove_all<_DerType>::type DerType; + typedef typename traits::Scalar Scalar; + typedef typename NumTraits::Real Real; + +// typedef auto_diff_scalar_op<_DerType, typename NumTraits::Real, +// is_same::Real>::value> Base; + +// using Base::operator+; +// using Base::operator+=; +// using Base::operator-; +// using Base::operator-=; +// using Base::operator*; +// using Base::operator*=; + + const AutoDiffScalar<_DerType>& derived() const { return *static_cast*>(this); } + AutoDiffScalar<_DerType>& derived() { return *static_cast*>(this); } + + + inline const AutoDiffScalar operator+(const Real& other) const + { + return AutoDiffScalar(derived().value() + other, derived().derivatives()); + } + + friend inline const AutoDiffScalar operator+(const Real& a, const AutoDiffScalar<_DerType>& b) + { + return AutoDiffScalar(a + b.value(), b.derivatives()); + } + + inline AutoDiffScalar<_DerType>& operator+=(const Real& other) + { + derived().value() += other; + return derived(); + } + + + inline const AutoDiffScalar >, DerType>::Type > + operator*(const Real& other) const + { + return AutoDiffScalar >, DerType>::Type >( + derived().value() * other, + derived().derivatives() * other); + } + + friend inline const AutoDiffScalar >, DerType>::Type > + operator*(const Real& other, const AutoDiffScalar<_DerType>& a) + { + return AutoDiffScalar >, DerType>::Type >( + a.value() * other, + a.derivatives() * other); + } + + inline AutoDiffScalar<_DerType>& operator*=(const Scalar& other) + { + *this = *this * other; + return derived(); + } +}; + +template +struct auto_diff_special_op<_DerType, false> +{ + void operator*() const; + void operator-() const; + void operator+() const; +}; + +template +struct make_coherent_impl, B> { + typedef Matrix A; + static void run(A& a, B& b) { + if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0)) + { + a.resize(b.size()); + a.setZero(); + } + } +}; + +template +struct make_coherent_impl > { + typedef Matrix B; + static void run(A& a, B& b) { + if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0)) + { + b.resize(a.size()); + b.setZero(); + } + } +}; + +template +struct make_coherent_impl, + Matrix > { + typedef Matrix A; + typedef Matrix B; + static void run(A& a, B& b) { + if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0)) + { + a.resize(b.size()); + a.setZero(); + } + else if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0)) + { + b.resize(a.size()); + b.setZero(); + } + } +}; + +} // end namespace internal + +template +struct ScalarBinaryOpTraits,typename DerType::Scalar,BinOp> +{ + typedef AutoDiffScalar ReturnType; +}; + +template +struct ScalarBinaryOpTraits, BinOp> +{ + typedef AutoDiffScalar ReturnType; +}; + + +// The following is an attempt to let Eigen's known about expression template, but that's more tricky! + +// template +// struct ScalarBinaryOpTraits,AutoDiffScalar, BinOp> +// { +// enum { Defined = 1 }; +// typedef AutoDiffScalar ReturnType; +// }; +// +// template +// struct ScalarBinaryOpTraits,AutoDiffScalar, BinOp> +// { +// enum { Defined = 1 };//internal::is_same::value }; +// typedef AutoDiffScalar ReturnType; +// }; + +#define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \ + template \ + inline const Eigen::AutoDiffScalar< \ + EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename Eigen::internal::remove_all::type, typename Eigen::internal::traits::type>::Scalar, product) > \ + FUNC(const Eigen::AutoDiffScalar& x) { \ + using namespace Eigen; \ + typedef typename Eigen::internal::traits::type>::Scalar Scalar; \ + EIGEN_UNUSED_VARIABLE(sizeof(Scalar)); \ + CODE; \ + } + +template +inline const AutoDiffScalar& conj(const AutoDiffScalar& x) { return x; } +template +inline const AutoDiffScalar& real(const AutoDiffScalar& x) { return x; } +template +inline typename DerType::Scalar imag(const AutoDiffScalar&) { return 0.; } +template +inline AutoDiffScalar::type::PlainObject> (min)(const AutoDiffScalar& x, const T& y) { + typedef AutoDiffScalar::type::PlainObject> ADS; + return (x <= y ? ADS(x) : ADS(y)); +} +template +inline AutoDiffScalar::type::PlainObject> (max)(const AutoDiffScalar& x, const T& y) { + typedef AutoDiffScalar::type::PlainObject> ADS; + return (x >= y ? ADS(x) : ADS(y)); +} +template +inline AutoDiffScalar::type::PlainObject> (min)(const T& x, const AutoDiffScalar& y) { + typedef AutoDiffScalar::type::PlainObject> ADS; + return (x < y ? ADS(x) : ADS(y)); +} +template +inline AutoDiffScalar::type::PlainObject> (max)(const T& x, const AutoDiffScalar& y) { + typedef AutoDiffScalar::type::PlainObject> ADS; + return (x > y ? ADS(x) : ADS(y)); +} +template +inline AutoDiffScalar::type::PlainObject> (min)(const AutoDiffScalar& x, const AutoDiffScalar& y) { + return (x.value() < y.value() ? x : y); +} +template +inline AutoDiffScalar::type::PlainObject> (max)(const AutoDiffScalar& x, const AutoDiffScalar& y) { + return (x.value() >= y.value() ? x : y); +} + + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs, + using std::abs; + return Eigen::MakeAutoDiffScalar(abs(x.value()), x.derivatives() * (x.value()<0 ? -1 : 1) );) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs2, + using numext::abs2; + return Eigen::MakeAutoDiffScalar(abs2(x.value()), x.derivatives() * (Scalar(2)*x.value()));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sqrt, + using std::sqrt; + Scalar sqrtx = sqrt(x.value()); + return Eigen::MakeAutoDiffScalar(sqrtx,x.derivatives() * (Scalar(0.5) / sqrtx));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cos, + using std::cos; + using std::sin; + return Eigen::MakeAutoDiffScalar(cos(x.value()), x.derivatives() * (-sin(x.value())));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sin, + using std::sin; + using std::cos; + return Eigen::MakeAutoDiffScalar(sin(x.value()),x.derivatives() * cos(x.value()));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(exp, + using std::exp; + Scalar expx = exp(x.value()); + return Eigen::MakeAutoDiffScalar(expx,x.derivatives() * expx);) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(log, + using std::log; + return Eigen::MakeAutoDiffScalar(log(x.value()),x.derivatives() * (Scalar(1)/x.value()));) + +template +inline const Eigen::AutoDiffScalar< +EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all::type,typename internal::traits::type>::Scalar,product) > +pow(const Eigen::AutoDiffScalar &x, const typename internal::traits::type>::Scalar &y) +{ + using namespace Eigen; + using std::pow; + return Eigen::MakeAutoDiffScalar(pow(x.value(),y), x.derivatives() * (y * pow(x.value(),y-1))); +} + + +template +inline const AutoDiffScalar::type>::Scalar,Dynamic,1> > +atan2(const AutoDiffScalar& a, const AutoDiffScalar& b) +{ + using std::atan2; + typedef typename internal::traits::type>::Scalar Scalar; + typedef AutoDiffScalar > PlainADS; + PlainADS ret; + ret.value() = atan2(a.value(), b.value()); + + Scalar squared_hypot = a.value() * a.value() + b.value() * b.value(); + + // if (squared_hypot==0) the derivation is undefined and the following results in a NaN: + ret.derivatives() = (a.derivatives() * b.value() - a.value() * b.derivatives()) / squared_hypot; + + return ret; +} + +EIGEN_AUTODIFF_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()))));) + +EIGEN_AUTODIFF_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()))));) + +EIGEN_AUTODIFF_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()))));) + +EIGEN_AUTODIFF_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()))));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sinh, + using std::sinh; + using std::cosh; + return Eigen::MakeAutoDiffScalar(sinh(x.value()),x.derivatives() * cosh(x.value()));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cosh, + using std::sinh; + using std::cosh; + return Eigen::MakeAutoDiffScalar(cosh(x.value()),x.derivatives() * sinh(x.value()));) + +#undef EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY + +template struct NumTraits > + : NumTraits< typename NumTraits::type::Scalar>::Real > +{ + typedef typename internal::remove_all::type DerTypeCleaned; + typedef AutoDiffScalar::Real,DerTypeCleaned::RowsAtCompileTime,DerTypeCleaned::ColsAtCompileTime, + 0, DerTypeCleaned::MaxRowsAtCompileTime, DerTypeCleaned::MaxColsAtCompileTime> > Real; + typedef AutoDiffScalar NonInteger; + typedef AutoDiffScalar Nested; + typedef typename NumTraits::Literal Literal; + enum{ + RequireInitialization = 1 + }; +}; + +} + +namespace std { +template +class numeric_limits > + : public numeric_limits {}; + +} // namespace std + +#endif // EIGEN_AUTODIFF_SCALAR_H diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h b/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h new file mode 100644 index 0000000000..8c2d04830d --- /dev/null +++ b/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h @@ -0,0 +1,220 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Gael Guennebaud +// +// 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 EIGEN_AUTODIFF_VECTOR_H +#define EIGEN_AUTODIFF_VECTOR_H + +namespace Eigen { + +/* \class AutoDiffScalar + * \brief A scalar type replacement with automatic differentation capability + * + * \param DerType the vector type used to store/represent the derivatives (e.g. Vector3f) + * + * This class represents a scalar value while tracking its respective derivatives. + * + * It supports the following list of global math function: + * - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos, + * - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos, + * - internal::conj, internal::real, internal::imag, numext::abs2. + * + * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However, + * in that case, the expression template mechanism only occurs at the top Matrix level, + * while derivatives are computed right away. + * + */ +template +class AutoDiffVector +{ + public: + //typedef typename internal::traits::Scalar Scalar; + typedef typename internal::traits::Scalar BaseScalar; + typedef AutoDiffScalar > ActiveScalar; + typedef ActiveScalar Scalar; + typedef AutoDiffScalar CoeffType; + typedef typename JacobianType::Index Index; + + inline AutoDiffVector() {} + + inline AutoDiffVector(const ValueType& values) + : m_values(values) + { + m_jacobian.setZero(); + } + + + CoeffType operator[] (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); } + const CoeffType operator[] (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); } + + CoeffType operator() (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); } + const CoeffType operator() (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); } + + CoeffType coeffRef(Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); } + const CoeffType coeffRef(Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); } + + Index size() const { return m_values.size(); } + + // FIXME here we could return an expression of the sum + Scalar sum() const { /*std::cerr << "sum \n\n";*/ /*std::cerr << m_jacobian.rowwise().sum() << "\n\n";*/ return Scalar(m_values.sum(), m_jacobian.rowwise().sum()); } + + + inline AutoDiffVector(const ValueType& values, const JacobianType& jac) + : m_values(values), m_jacobian(jac) + {} + + template + inline AutoDiffVector(const AutoDiffVector& other) + : m_values(other.values()), m_jacobian(other.jacobian()) + {} + + inline AutoDiffVector(const AutoDiffVector& other) + : m_values(other.values()), m_jacobian(other.jacobian()) + {} + + template + inline AutoDiffVector& operator=(const AutoDiffVector& other) + { + m_values = other.values(); + m_jacobian = other.jacobian(); + return *this; + } + + inline AutoDiffVector& operator=(const AutoDiffVector& other) + { + m_values = other.values(); + m_jacobian = other.jacobian(); + return *this; + } + + inline const ValueType& values() const { return m_values; } + inline ValueType& values() { return m_values; } + + inline const JacobianType& jacobian() const { return m_jacobian; } + inline JacobianType& jacobian() { return m_jacobian; } + + template + inline const AutoDiffVector< + typename MakeCwiseBinaryOp,ValueType,OtherValueType>::Type, + typename MakeCwiseBinaryOp,JacobianType,OtherJacobianType>::Type > + operator+(const AutoDiffVector& other) const + { + return AutoDiffVector< + typename MakeCwiseBinaryOp,ValueType,OtherValueType>::Type, + typename MakeCwiseBinaryOp,JacobianType,OtherJacobianType>::Type >( + m_values + other.values(), + m_jacobian + other.jacobian()); + } + + template + inline AutoDiffVector& + operator+=(const AutoDiffVector& other) + { + m_values += other.values(); + m_jacobian += other.jacobian(); + return *this; + } + + template + inline const AutoDiffVector< + typename MakeCwiseBinaryOp,ValueType,OtherValueType>::Type, + typename MakeCwiseBinaryOp,JacobianType,OtherJacobianType>::Type > + operator-(const AutoDiffVector& other) const + { + return AutoDiffVector< + typename MakeCwiseBinaryOp,ValueType,OtherValueType>::Type, + typename MakeCwiseBinaryOp,JacobianType,OtherJacobianType>::Type >( + m_values - other.values(), + m_jacobian - other.jacobian()); + } + + template + inline AutoDiffVector& + operator-=(const AutoDiffVector& other) + { + m_values -= other.values(); + m_jacobian -= other.jacobian(); + return *this; + } + + inline const AutoDiffVector< + typename MakeCwiseUnaryOp, ValueType>::Type, + typename MakeCwiseUnaryOp, JacobianType>::Type > + operator-() const + { + return AutoDiffVector< + typename MakeCwiseUnaryOp, ValueType>::Type, + typename MakeCwiseUnaryOp, JacobianType>::Type >( + -m_values, + -m_jacobian); + } + + inline const AutoDiffVector< + typename MakeCwiseUnaryOp, ValueType>::Type, + typename MakeCwiseUnaryOp, JacobianType>::Type> + operator*(const BaseScalar& other) const + { + return AutoDiffVector< + typename MakeCwiseUnaryOp, ValueType>::Type, + typename MakeCwiseUnaryOp, JacobianType>::Type >( + m_values * other, + m_jacobian * other); + } + + friend inline const AutoDiffVector< + typename MakeCwiseUnaryOp, ValueType>::Type, + typename MakeCwiseUnaryOp, JacobianType>::Type > + operator*(const Scalar& other, const AutoDiffVector& v) + { + return AutoDiffVector< + typename MakeCwiseUnaryOp, ValueType>::Type, + typename MakeCwiseUnaryOp, JacobianType>::Type >( + v.values() * other, + v.jacobian() * other); + } + +// template +// inline const AutoDiffVector< +// CwiseBinaryOp, ValueType, OtherValueType> +// CwiseBinaryOp, +// CwiseUnaryOp, JacobianType>, +// CwiseUnaryOp, OtherJacobianType> > > +// operator*(const AutoDiffVector& other) const +// { +// return AutoDiffVector< +// CwiseBinaryOp, ValueType, OtherValueType> +// CwiseBinaryOp, +// CwiseUnaryOp, JacobianType>, +// CwiseUnaryOp, OtherJacobianType> > >( +// m_values.cwise() * other.values(), +// (m_jacobian * other.values()) + (m_values * other.jacobian())); +// } + + inline AutoDiffVector& operator*=(const Scalar& other) + { + m_values *= other; + m_jacobian *= other; + return *this; + } + + template + inline AutoDiffVector& operator*=(const AutoDiffVector& other) + { + *this = *this * other; + return *this; + } + + protected: + ValueType m_values; + JacobianType m_jacobian; + +}; + +} + +#endif // EIGEN_AUTODIFF_VECTOR_H diff --git a/wpiutil/src/main/native/include/drake/common/drake_assert.h b/wpiutil/src/main/native/include/drake/common/drake_assert.h new file mode 100644 index 0000000000..21e7bd100c --- /dev/null +++ b/wpiutil/src/main/native/include/drake/common/drake_assert.h @@ -0,0 +1,152 @@ +#pragma once + +#include + +/// @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 . 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 +struct ConditionTraits { + static constexpr bool is_valid = std::is_convertible::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::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::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::type>::is_valid, \ + "Condition should be bool-convertible."); +# define DRAKE_ASSERT_VOID(expression) static_assert( \ + std::is_convertible::value, \ + "Expression should be void.") +#endif + +#endif // DRAKE_DOXYGEN_CXX diff --git a/wpiutil/src/main/native/include/drake/common/drake_assertion_error.h b/wpiutil/src/main/native/include/drake/common/drake_assertion_error.h new file mode 100644 index 0000000000..541b118ecf --- /dev/null +++ b/wpiutil/src/main/native/include/drake/common/drake_assertion_error.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include + +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 diff --git a/wpiutil/src/main/native/include/drake/common/drake_copyable.h b/wpiutil/src/main/native/include/drake/common/drake_copyable.h new file mode 100644 index 0000000000..086f1f78ee --- /dev/null +++ b/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.: +
+class Foo {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Foo)
+
+  // ...
+};
+
+*/ +#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.: +
+class Foo {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Foo)
+
+  // ...
+};
+
+*/ +#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::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.: +
+template 
+class Foo {
+ public:
+  DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN(Foo)
+
+  // ...
+};
+DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T(Foo)
+
+*/ +#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::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 \ + Classname::Classname(const Classname&) = default; \ + template \ + Classname& Classname::operator=(const Classname&) = default; \ + template \ + Classname::Classname(Classname&&) = default; \ + template \ + Classname& Classname::operator=(Classname&&) = default; diff --git a/wpiutil/src/main/native/include/drake/common/drake_throw.h b/wpiutil/src/main/native/include/drake/common/drake_throw.h new file mode 100644 index 0000000000..ff42bb7410 --- /dev/null +++ b/wpiutil/src/main/native/include/drake/common/drake_throw.h @@ -0,0 +1,31 @@ +#pragma once + +#include + +#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::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) diff --git a/wpiutil/src/main/native/include/drake/common/is_approx_equal_abstol.h b/wpiutil/src/main/native/include/drake/common/is_approx_equal_abstol.h new file mode 100644 index 0000000000..b3f369ca01 --- /dev/null +++ b/wpiutil/src/main/native/include/drake/common/is_approx_equal_abstol.h @@ -0,0 +1,62 @@ +#pragma once + +#include + +#include + +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 +bool is_approx_equal_abstol(const Eigen::MatrixBase& m1, + const Eigen::MatrixBase& m2, + double tolerance) { + return ( + (m1.rows() == m2.rows()) && + (m1.cols() == m2.cols()) && + ((m1 - m2).template lpNorm() <= 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 +/// 
+///    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.
+///
+/// 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 +bool IsApproxEqualAbsTolWithPermutedColumns( + const Eigen::MatrixBase& m1, + const Eigen::MatrixBase& m2, double tolerance) { + if ((m1.cols() != m2.cols()) || (m1.rows() != m2.rows())) return false; + + std::vector 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 diff --git a/wpiutil/src/main/native/include/drake/common/never_destroyed.h b/wpiutil/src/main/native/include/drake/common/never_destroyed.h new file mode 100644 index 0000000000..2033fd0738 --- /dev/null +++ b/wpiutil/src/main/native/include/drake/common/never_destroyed.h @@ -0,0 +1,84 @@ +#pragma once + +#include +#include +#include + +#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 +/// Static +/// and Global Variables 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 instance; +/// return instance.access(); +/// } +/// private: +/// friend never_destroyed; +/// 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; +/// static const drake::never_destroyed string_to_enum{ +/// std::initializer_list{ +/// {"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 +class never_destroyed { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(never_destroyed) + + /// Passes the constructor arguments along to T using perfect forwarding. + template + explicit never_destroyed(Args&&... args) { + // Uses "placement new" to construct a `T` in `storage_`. + new (&storage_) T(std::forward(args)...); + } + + /// Does nothing. Guaranteed! + ~never_destroyed() = default; + + /// Returns the underlying T reference. + T& access() { return *reinterpret_cast(&storage_); } + const T& access() const { return *reinterpret_cast(&storage_); } + + private: + typename std::aligned_storage::type storage_; +}; + +} // namespace drake diff --git a/wpiutil/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h b/wpiutil/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h new file mode 100644 index 0000000000..e45cdc8659 --- /dev/null +++ b/wpiutil/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#include + +#include + +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& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R); +} // namespace math +} // namespace drake + diff --git a/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeJNITest.java b/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeJNITest.java new file mode 100644 index 0000000000..1f32857963 --- /dev/null +++ b/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeJNITest.java @@ -0,0 +1,19 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpiutil.math; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; + +public class DrakeJNITest { + @Test + public void testLink() { + assertDoesNotThrow(DrakeJNI::forceLoad); + } +} diff --git a/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeTest.java b/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeTest.java new file mode 100644 index 0000000000..9c5a670f43 --- /dev/null +++ b/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeTest.java @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpiutil.math; + +import org.ejml.simple.SimpleMatrix; +import org.junit.jupiter.api.Test; + + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +@SuppressWarnings({"ParameterName", "LocalVariableName"}) +public class DrakeTest { + public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) { + for (int i = 0; i < A.numRows(); i++) { + for (int j = 0; j < A.numCols(); j++) { + assertEquals(A.get(i, j), B.get(i, j), 1e-4); + } + } + } + + private boolean solveDAREandVerify(SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, + SimpleMatrix R) { + var X = Drake.discreteAlgebraicRiccatiEquation(A, B, Q, R); + + // expect that x is the same as it's transpose + assertEquals(X.numRows(), X.numCols()); + assertMatrixEqual(X, X.transpose()); + + // Verify that this is a solution to the DARE. + SimpleMatrix Y = A.transpose().mult(X).mult(A) + .minus(X) + .minus(A.transpose().mult(X).mult(B) + .mult(((B.transpose().mult(X).mult(B)).plus(R)) + .invert()).mult(B.transpose()).mult(X).mult(A)) + .plus(Q); + assertMatrixEqual(Y, new SimpleMatrix(Y.numRows(), Y.numCols())); + + return true; + } + + @Test + public void testDiscreteAlgebraicRicattiEquation() { + int n1 = 4; + int m1 = 1; + + // we know from Scipy that this should be [[0.05048525 0.10097051 0.20194102 0.40388203]] + SimpleMatrix A1 = new SimpleMatrix(n1, n1, true, new double[]{0.5, 1, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 1, 0, 0, 0, 0}).transpose(); + SimpleMatrix B1 = new SimpleMatrix(n1, m1, true, new double[]{0, 0, 0, 1}); + SimpleMatrix Q1 = new SimpleMatrix(n1, n1, true, new double[]{1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0}); + SimpleMatrix R1 = new SimpleMatrix(m1, m1, true, new double[]{0.25}); + assertTrue(solveDAREandVerify(A1, B1, Q1, R1)); + + SimpleMatrix A2 = new SimpleMatrix(2, 2, true, new double[]{1, 1, 0, 1}); + SimpleMatrix B2 = new SimpleMatrix(2, 1, true, new double[]{0, 1}); + SimpleMatrix Q2 = new SimpleMatrix(2, 2, true, new double[]{1, 0, 0, 0}); + SimpleMatrix R2 = new SimpleMatrix(1, 1, true, new double[]{0.3}); + assertTrue(solveDAREandVerify(A2, B2, Q2, R2)); + + } +} diff --git a/wpiutil/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp b/wpiutil/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp new file mode 100644 index 0000000000..edcb77290b --- /dev/null +++ b/wpiutil/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp @@ -0,0 +1,76 @@ +#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" + +using Eigen::MatrixXd; + +namespace drake { +namespace math { +namespace { +void SolveDAREandVerify(const Eigen::Ref& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R) { + MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R); + // 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 * (B.transpose() * X * B + R).inverse() * + B.transpose() * X * A + + 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 + // Riccati Equation" + int n1 = 4, m1 = 1; + MatrixXd A1(n1, n1), B1(n1, m1), Q1(n1, n1), R1(m1, m1); + A1 << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0; + B1 << 0, 0, 0, 1; + 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); + // Test 2: invertible A + int n2 = 2, m2 = 1; + MatrixXd A2(n2, n2), B2(n2, m2), Q2(n2, n2), R2(m2, m2); + A2 << 1, 1, 0, 1; + B2 << 0, 1; + Q2 << 1, 0, 0, 0; + R2 << 0.3; + SolveDAREandVerify(A2, B2, Q2, R2); + // 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); + A3 << 0, 1, 0, 0; + B3 << 0, 1; + Q3 << 1, 0, 0, 1; + R3 << 1; + SolveDAREandVerify(A3, B3, Q3, R3); + // Test 4: A = B = Q = R = I_2 (2-by-2 identity matrix) + int n4 = 2, m4 = 2; + MatrixXd A4(n4, n4), B4(n4, m4), Q4(n4, n4), R4(m4, m4); + A4 << 1, 0, 0, 1; + B4 << 1, 0, 0, 1; + Q4 << 1, 0, 0, 1; + R4 << 1, 0, 0, 1; + SolveDAREandVerify(A4, B4, Q4, R4); +} +} // namespace +} // namespace math +} // namespace drake diff --git a/wpiutil/src/test/native/include/drake/common/autodiff.h b/wpiutil/src/test/native/include/drake/common/autodiff.h new file mode 100644 index 0000000000..66fd88a64e --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/autodiff.h @@ -0,0 +1,34 @@ +#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/wpiutil/src/test/native/include/drake/common/autodiff_overloads.h b/wpiutil/src/test/native/include/drake/common/autodiff_overloads.h new file mode 100644 index 0000000000..7eaeb3f221 --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/autodiff_overloads.h @@ -0,0 +1,203 @@ +/// @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/wpiutil/src/test/native/include/drake/common/autodiffxd.h b/wpiutil/src/test/native/include/drake/common/autodiffxd.h new file mode 100644 index 0000000000..a99b2f0502 --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/autodiffxd.h @@ -0,0 +1,423 @@ +#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/wpiutil/src/test/native/include/drake/common/cond.h b/wpiutil/src/test/native/include/drake/common/cond.h new file mode 100644 index 0000000000..16dd21ed5f --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/cond.h @@ -0,0 +1,44 @@ +#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/wpiutil/src/test/native/include/drake/common/constants.h b/wpiutil/src/test/native/include/drake/common/constants.h new file mode 100644 index 0000000000..0ccddca66a --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/constants.h @@ -0,0 +1,19 @@ +#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/wpiutil/src/test/native/include/drake/common/double_overloads.h b/wpiutil/src/test/native/include/drake/common/double_overloads.h new file mode 100644 index 0000000000..125f11362e --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/double_overloads.h @@ -0,0 +1,21 @@ +/// @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/wpiutil/src/test/native/include/drake/common/drake_deprecated.h b/wpiutil/src/test/native/include/drake/common/drake_deprecated.h new file mode 100644 index 0000000000..5ce6328d99 --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/drake_deprecated.h @@ -0,0 +1,65 @@ +#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/wpiutil/src/test/native/include/drake/common/drake_nodiscard.h b/wpiutil/src/test/native/include/drake/common/drake_nodiscard.h new file mode 100644 index 0000000000..29f078dd22 --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/drake_nodiscard.h @@ -0,0 +1,13 @@ +#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/wpiutil/src/test/native/include/drake/common/dummy_value.h b/wpiutil/src/test/native/include/drake/common/dummy_value.h new file mode 100644 index 0000000000..b9c616a316 --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/dummy_value.h @@ -0,0 +1,35 @@ +#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/wpiutil/src/test/native/include/drake/common/eigen_autodiff_limits.h b/wpiutil/src/test/native/include/drake/common/eigen_autodiff_limits.h new file mode 100644 index 0000000000..49175cee30 --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/eigen_autodiff_limits.h @@ -0,0 +1,20 @@ +#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/wpiutil/src/test/native/include/drake/common/eigen_autodiff_types.h b/wpiutil/src/test/native/include/drake/common/eigen_autodiff_types.h new file mode 100644 index 0000000000..10ffec6987 --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/eigen_autodiff_types.h @@ -0,0 +1,38 @@ +#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/wpiutil/src/test/native/include/drake/common/eigen_types.h b/wpiutil/src/test/native/include/drake/common/eigen_types.h new file mode 100644 index 0000000000..abe3e0bb27 --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/eigen_types.h @@ -0,0 +1,461 @@ +#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/wpiutil/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h b/wpiutil/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h new file mode 100644 index 0000000000..8847d87daa --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h @@ -0,0 +1,116 @@ +#pragma once + +#include +#include +#include + +#include +#include + +#include "drake/common/drake_nodiscard.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 +DRAKE_NODISCARD ::testing::AssertionResult CompareMatrices( + const Eigen::MatrixBase& m1, + 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() << ")"; + } + + 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::infinity() && + m2(ii, jj) == std::numeric_limits::infinity(); + + const auto both_negative_infinity = + m1(ii, jj) == -std::numeric_limits::infinity() && + m2(ii, jj) == -std::numeric_limits::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)))) { + return ::testing::AssertionFailure() << "NaN mismatch at (" << ii + << ", " << jj << "):\nm1 =\n" + << m1 << "\nm2 =\n" + << m2; + } + + // 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) { + 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); + } + } 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) { + 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); + } + } + } + } + + return ::testing::AssertionSuccess() << "m1 =\n" + << m1 + << "\nis approximately equal to m2 =\n" + << m2; +} + +} // namespace drake diff --git a/wpiutil/src/test/native/include/drake/common/unused.h b/wpiutil/src/test/native/include/drake/common/unused.h new file mode 100644 index 0000000000..5a28b01627 --- /dev/null +++ b/wpiutil/src/test/native/include/drake/common/unused.h @@ -0,0 +1,53 @@ +#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/wpiutil/src/test/native/include/drake/math/autodiff.h b/wpiutil/src/test/native/include/drake/math/autodiff.h new file mode 100644 index 0000000000..52edb11f87 --- /dev/null +++ b/wpiutil/src/test/native/include/drake/math/autodiff.h @@ -0,0 +1,332 @@ +/// @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