Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

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

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2020-07-24 08:34:30 -07:00
committed by GitHub
parent 0c18abed33
commit e759dad019
39 changed files with 4387 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.styleguide
View File

@@ -22,6 +22,7 @@ repoRootNameOverride {
includeOtherLibs {
^cameraserver/
^cscore
^drake/
^hal/
^imgui
^mockdata/

37
ThirdPartyNotices.txt
View File

@@ -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.

2
simulation/halsim_gui/src/main/native/cpp/EncoderGui.h
View File

@@ -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. */

4
wpiutil/.styleguide
View File

@@ -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
}

5
wpiutil/CMakeLists.txt
View File

@@ -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()

56
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Drake.java Normal file
View File

@@ -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());
}
}

79
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/DrakeJNI.java Normal file
View File

@@ -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<DrakeJNI> 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);
}
}
}

87
wpiutil/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp Normal file
View File

@@ -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 <atomic>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#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<AssertionConfig> global;
return global.access();
}
std::atomic<bool> 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;
}

463
wpiutil/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp Normal file
View File

@@ -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 <Eigen/Eigenvalues>
#include <Eigen/QR>
// 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 <typename T>
int sgn(T val) {
return (T(0) < val) - (val < T(0));
}
void check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& 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<Eigen::MatrixXd> 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<Eigen::MatrixXcd> qr(E);
DRAKE_THROW_UNLESS(qr.rank() == n);
}
}
void check_detectable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& 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<Eigen::MatrixXd> 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<Eigen::Matrix2d> 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<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> 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<double, 8, 8> C = Eigen::Matrix<double, 8, 8>::Zero();
Eigen::Matrix<double, 8, 1> 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<double, 4, 2> 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<Eigen::Matrix<double, 4, 2> > qr1(X);
Z1.block<4, 4>(p, p) = qr1.householderQ();
Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > 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<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> 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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R) {
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<Eigen::MatrixXd> 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<Eigen::MatrixXd> 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<Eigen::MatrixXd> 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

62
wpiutil/src/main/native/cpp/jni/DrakeJNI.cpp Normal file
View File

@@ -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 <jni.h>
#include <iostream>
#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions>
#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<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Amat{nativeA, states, states};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Bmat{nativeB, states, inputs};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Qmat{nativeQ, states, states};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
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"

40
wpiutil/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff Normal file
View File

@@ -0,0 +1,40 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
//
// 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 <unsupported/Eigen/AutoDiff>
* \endcode
*/
//@{
}
#include "src/AutoDiff/AutoDiffScalar.h"
// #include "src/AutoDiff/AutoDiffVector.h"
#include "src/AutoDiff/AutoDiffJacobian.h"
namespace Eigen {
//@}
}
#endif // EIGEN_AUTODIFF_MODULE

108
wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h Normal file
View File

@@ -0,0 +1,108 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// 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<typename Functor> class AutoDiffJacobian : public Functor
{
public:
AutoDiffJacobian() : Functor() {}
AutoDiffJacobian(const Functor& f) : Functor(f) {}
// forward constructors
#if EIGEN_HAS_VARIADIC_TEMPLATES
template<typename... T>
AutoDiffJacobian(const T& ...Values) : Functor(Values...) {}
#else
template<typename T0>
AutoDiffJacobian(const T0& a0) : Functor(a0) {}
template<typename T0, typename T1>
AutoDiffJacobian(const T0& a0, const T1& a1) : Functor(a0, a1) {}
template<typename T0, typename T1, typename T2>
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<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;
typedef typename JacobianType::Index Index;
typedef Matrix<Scalar, InputsAtCompileTime, 1> DerivativeType;
typedef AutoDiffScalar<DerivativeType> ActiveScalar;
typedef Matrix<ActiveScalar, InputsAtCompileTime, 1> ActiveInput;
typedef Matrix<ActiveScalar, ValuesAtCompileTime, 1> 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<typename... ParamsType>
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<ActiveScalar>();
ActiveValue av(jac.rows());
if(InputsAtCompileTime==Dynamic)
for (Index j=0; j<jac.rows(); j++)
av[j].derivatives().resize(x.rows());
for (Index i=0; i<jac.cols(); i++)
ax[i].derivatives() = DerivativeType::Unit(x.rows(),i);
#if EIGEN_HAS_VARIADIC_TEMPLATES
Functor::operator()(ax, &av, Params...);
#else
Functor::operator()(ax, &av);
#endif
for (Index i=0; i<jac.rows(); i++)
{
(*v)[i] = av[i].value();
jac.row(i) = av[i].derivatives();
}
}
};
}
#endif // EIGEN_AUTODIFF_JACOBIAN_H

694
wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h Normal file
View File

@@ -0,0 +1,694 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// 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<typename A, typename B>
struct make_coherent_impl {
static void run(A&, B&) {}
};
// resize a to match b is a.size()==0, and conversely.
template<typename A, typename B>
void make_coherent(const A& a, const B&b)
{
make_coherent_impl<A,B>::run(a.const_cast_derived(), b.const_cast_derived());
}
template<typename _DerType, bool Enable> struct auto_diff_special_op;
} // end namespace internal
template<typename _DerType> class AutoDiffScalar;
template<typename NewDerType>
inline AutoDiffScalar<NewDerType> MakeAutoDiffScalar(const typename NewDerType::Scalar& value, const NewDerType &der) {
return AutoDiffScalar<NewDerType>(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<typename _DerType>
class AutoDiffScalar
: public internal::auto_diff_special_op
<_DerType, !internal::is_same<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar,
typename NumTraits<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar>::Real>::value>
{
public:
typedef internal::auto_diff_special_op
<_DerType, !internal::is_same<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar,
typename NumTraits<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar>::Real>::value> Base;
typedef typename internal::remove_all<_DerType>::type DerType;
typedef typename internal::traits<DerType>::Scalar Scalar;
typedef typename NumTraits<Scalar>::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<typename OtherDerType>
AutoDiffScalar(const AutoDiffScalar<OtherDerType>& other
#ifndef EIGEN_PARSED_BY_DOXYGEN
, typename internal::enable_if<
internal::is_same<Scalar, typename internal::traits<typename internal::remove_all<OtherDerType>::type>::Scalar>::value
&& internal::is_convertible<OtherDerType,DerType>::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<typename OtherDerType>
inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& 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<typename OtherDerType> inline bool operator< (const AutoDiffScalar<OtherDerType>& b) const { return m_value < b.value(); }
template<typename OtherDerType> inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const { return m_value <= b.value(); }
template<typename OtherDerType> inline bool operator> (const AutoDiffScalar<OtherDerType>& b) const { return m_value > b.value(); }
template<typename OtherDerType> inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const { return m_value >= b.value(); }
template<typename OtherDerType> inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const { return m_value == b.value(); }
template<typename OtherDerType> inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const { return m_value != b.value(); }
inline const AutoDiffScalar<DerType&> operator+(const Scalar& other) const
{
return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
}
friend inline const AutoDiffScalar<DerType&> operator+(const Scalar& a, const AutoDiffScalar& b)
{
return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
}
// inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
// {
// return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
// }
// friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar& b)
// {
// return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
// }
inline AutoDiffScalar& operator+=(const Scalar& other)
{
value() += other;
return *this;
}
template<typename OtherDerType>
inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >
operator+(const AutoDiffScalar<OtherDerType>& other) const
{
internal::make_coherent(m_derivatives, other.derivatives());
return AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >(
m_value + other.value(),
m_derivatives + other.derivatives());
}
template<typename OtherDerType>
inline AutoDiffScalar&
operator+=(const AutoDiffScalar<OtherDerType>& other)
{
(*this) = (*this) + other;
return *this;
}
inline const AutoDiffScalar<DerType&> operator-(const Scalar& b) const
{
return AutoDiffScalar<DerType&>(m_value - b, m_derivatives);
}
friend inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
operator-(const Scalar& a, const AutoDiffScalar& b)
{
return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
(a - b.value(), -b.derivatives());
}
inline AutoDiffScalar& operator-=(const Scalar& other)
{
value() -= other;
return *this;
}
template<typename OtherDerType>
inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >
operator-(const AutoDiffScalar<OtherDerType>& other) const
{
internal::make_coherent(m_derivatives, other.derivatives());
return AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >(
m_value - other.value(),
m_derivatives - other.derivatives());
}
template<typename OtherDerType>
inline AutoDiffScalar&
operator-=(const AutoDiffScalar<OtherDerType>& other)
{
*this = *this - other;
return *this;
}
inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
operator-() const
{
return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >(
-m_value,
-m_derivatives);
}
inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
operator*(const Scalar& other) const
{
return MakeAutoDiffScalar(m_value * other, m_derivatives * other);
}
friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
operator*(const Scalar& other, const AutoDiffScalar& a)
{
return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other);
}
// inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
// operator*(const Real& other) const
// {
// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
// m_value * other,
// (m_derivatives * other));
// }
//
// friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
// operator*(const Real& other, const AutoDiffScalar& a)
// {
// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
// a.value() * other,
// a.derivatives() * other);
// }
inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
operator/(const Scalar& other) const
{
return MakeAutoDiffScalar(m_value / other, (m_derivatives * (Scalar(1)/other)));
}
friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
operator/(const Scalar& other, const AutoDiffScalar& a)
{
return MakeAutoDiffScalar(other / a.value(), a.derivatives() * (Scalar(-other) / (a.value()*a.value())));
}
// inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
// operator/(const Real& other) const
// {
// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
// m_value / other,
// (m_derivatives * (Real(1)/other)));
// }
//
// friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
// operator/(const Real& other, const AutoDiffScalar& a)
// {
// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
// other / a.value(),
// a.derivatives() * (-Real(1)/other));
// }
template<typename OtherDerType>
inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(
CwiseBinaryOp<internal::scalar_difference_op<Scalar> 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<OtherDerType>::type,Scalar,product) >,Scalar,product) >
operator/(const AutoDiffScalar<OtherDerType>& 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<typename OtherDerType>
inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product),
const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) > >
operator*(const AutoDiffScalar<OtherDerType>& 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<typename OtherDerType>
inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other)
{
*this = *this * other;
return *this;
}
inline AutoDiffScalar& operator/=(const Scalar& other)
{
*this = *this / other;
return *this;
}
template<typename OtherDerType>
inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& other)
{
*this = *this / other;
return *this;
}
protected:
Scalar m_value;
DerType m_derivatives;
};
namespace internal {
template<typename _DerType>
struct auto_diff_special_op<_DerType, true>
// : auto_diff_scalar_op<_DerType, typename NumTraits<Scalar>::Real,
// is_same<Scalar,typename NumTraits<Scalar>::Real>::value>
{
typedef typename remove_all<_DerType>::type DerType;
typedef typename traits<DerType>::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real Real;
// typedef auto_diff_scalar_op<_DerType, typename NumTraits<Scalar>::Real,
// is_same<Scalar,typename NumTraits<Scalar>::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<const AutoDiffScalar<_DerType>*>(this); }
AutoDiffScalar<_DerType>& derived() { return *static_cast<AutoDiffScalar<_DerType>*>(this); }
inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
{
return AutoDiffScalar<DerType&>(derived().value() + other, derived().derivatives());
}
friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar<_DerType>& b)
{
return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
}
inline AutoDiffScalar<_DerType>& operator+=(const Real& other)
{
derived().value() += other;
return derived();
}
inline const AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >
operator*(const Real& other) const
{
return AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >(
derived().value() * other,
derived().derivatives() * other);
}
friend inline const AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >
operator*(const Real& other, const AutoDiffScalar<_DerType>& a)
{
return AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >(
a.value() * other,
a.derivatives() * other);
}
inline AutoDiffScalar<_DerType>& operator*=(const Scalar& other)
{
*this = *this * other;
return derived();
}
};
template<typename _DerType>
struct auto_diff_special_op<_DerType, false>
{
void operator*() const;
void operator-() const;
void operator+() const;
};
template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols, typename B>
struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>, B> {
typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> 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<typename A, typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
struct make_coherent_impl<A, Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> 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<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols,
typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>,
Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> 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<typename DerType, typename BinOp>
struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,typename DerType::Scalar,BinOp>
{
typedef AutoDiffScalar<DerType> ReturnType;
};
template<typename DerType, typename BinOp>
struct ScalarBinaryOpTraits<typename DerType::Scalar,AutoDiffScalar<DerType>, BinOp>
{
typedef AutoDiffScalar<DerType> ReturnType;
};
// The following is an attempt to let Eigen's known about expression template, but that's more tricky!
// template<typename DerType, typename BinOp>
// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,AutoDiffScalar<DerType>, BinOp>
// {
// enum { Defined = 1 };
// typedef AutoDiffScalar<typename DerType::PlainObject> ReturnType;
// };
//
// template<typename DerType1,typename DerType2, typename BinOp>
// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType1>,AutoDiffScalar<DerType2>, BinOp>
// {
// enum { Defined = 1 };//internal::is_same<typename DerType1::Scalar,typename DerType2::Scalar>::value };
// typedef AutoDiffScalar<typename DerType1::PlainObject> ReturnType;
// };
#define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \
template<typename DerType> \
inline const Eigen::AutoDiffScalar< \
EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename Eigen::internal::remove_all<DerType>::type, typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar, product) > \
FUNC(const Eigen::AutoDiffScalar<DerType>& x) { \
using namespace Eigen; \
typedef typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar Scalar; \
EIGEN_UNUSED_VARIABLE(sizeof(Scalar)); \
CODE; \
}
template<typename DerType>
inline const AutoDiffScalar<DerType>& conj(const AutoDiffScalar<DerType>& x) { return x; }
template<typename DerType>
inline const AutoDiffScalar<DerType>& real(const AutoDiffScalar<DerType>& x) { return x; }
template<typename DerType>
inline typename DerType::Scalar imag(const AutoDiffScalar<DerType>&) { return 0.; }
template<typename DerType, typename T>
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const AutoDiffScalar<DerType>& x, const T& y) {
typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
return (x <= y ? ADS(x) : ADS(y));
}
template<typename DerType, typename T>
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const AutoDiffScalar<DerType>& x, const T& y) {
typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
return (x >= y ? ADS(x) : ADS(y));
}
template<typename DerType, typename T>
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const T& x, const AutoDiffScalar<DerType>& y) {
typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
return (x < y ? ADS(x) : ADS(y));
}
template<typename DerType, typename T>
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const T& x, const AutoDiffScalar<DerType>& y) {
typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
return (x > y ? ADS(x) : ADS(y));
}
template<typename DerType>
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
return (x.value() < y.value() ? x : y);
}
template<typename DerType>
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& 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<typename DerType>
inline const Eigen::AutoDiffScalar<
EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<DerType>::type,typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar,product) >
pow(const Eigen::AutoDiffScalar<DerType> &x, const typename internal::traits<typename internal::remove_all<DerType>::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<typename DerTypeA,typename DerTypeB>
inline const AutoDiffScalar<Matrix<typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar,Dynamic,1> >
atan2(const AutoDiffScalar<DerTypeA>& a, const AutoDiffScalar<DerTypeB>& b)
{
using std::atan2;
typedef typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar Scalar;
typedef AutoDiffScalar<Matrix<Scalar,Dynamic,1> > 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<typename DerType> struct NumTraits<AutoDiffScalar<DerType> >
: NumTraits< typename NumTraits<typename internal::remove_all<DerType>::type::Scalar>::Real >
{
typedef typename internal::remove_all<DerType>::type DerTypeCleaned;
typedef AutoDiffScalar<Matrix<typename NumTraits<typename DerTypeCleaned::Scalar>::Real,DerTypeCleaned::RowsAtCompileTime,DerTypeCleaned::ColsAtCompileTime,
0, DerTypeCleaned::MaxRowsAtCompileTime, DerTypeCleaned::MaxColsAtCompileTime> > Real;
typedef AutoDiffScalar<DerType> NonInteger;
typedef AutoDiffScalar<DerType> Nested;
typedef typename NumTraits<typename DerTypeCleaned::Scalar>::Literal Literal;
enum{
RequireInitialization = 1
};
};
}
namespace std {
template <typename T>
class numeric_limits<Eigen::AutoDiffScalar<T> >
: public numeric_limits<typename T::Scalar> {};
} // namespace std
#endif // EIGEN_AUTODIFF_SCALAR_H

220
wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h Normal file
View File

@@ -0,0 +1,220 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// 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<typename ValueType, typename JacobianType>
class AutoDiffVector
{
public:
//typedef typename internal::traits<ValueType>::Scalar Scalar;
typedef typename internal::traits<ValueType>::Scalar BaseScalar;
typedef AutoDiffScalar<Matrix<BaseScalar,JacobianType::RowsAtCompileTime,1> > ActiveScalar;
typedef ActiveScalar Scalar;
typedef AutoDiffScalar<typename JacobianType::ColXpr> 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<typename OtherValueType, typename OtherJacobianType>
inline AutoDiffVector(const AutoDiffVector<OtherValueType, OtherJacobianType>& other)
: m_values(other.values()), m_jacobian(other.jacobian())
{}
inline AutoDiffVector(const AutoDiffVector& other)
: m_values(other.values()), m_jacobian(other.jacobian())
{}
template<typename OtherValueType, typename OtherJacobianType>
inline AutoDiffVector& operator=(const AutoDiffVector<OtherValueType, OtherJacobianType>& 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<typename OtherValueType,typename OtherJacobianType>
inline const AutoDiffVector<
typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >
operator+(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
{
return AutoDiffVector<
typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >(
m_values + other.values(),
m_jacobian + other.jacobian());
}
template<typename OtherValueType, typename OtherJacobianType>
inline AutoDiffVector&
operator+=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
{
m_values += other.values();
m_jacobian += other.jacobian();
return *this;
}
template<typename OtherValueType,typename OtherJacobianType>
inline const AutoDiffVector<
typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >
operator-(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
{
return AutoDiffVector<
typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >(
m_values - other.values(),
m_jacobian - other.jacobian());
}
template<typename OtherValueType, typename OtherJacobianType>
inline AutoDiffVector&
operator-=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
{
m_values -= other.values();
m_jacobian -= other.jacobian();
return *this;
}
inline const AutoDiffVector<
typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >
operator-() const
{
return AutoDiffVector<
typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >(
-m_values,
-m_jacobian);
}
inline const AutoDiffVector<
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type>
operator*(const BaseScalar& other) const
{
return AutoDiffVector<
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
m_values * other,
m_jacobian * other);
}
friend inline const AutoDiffVector<
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >
operator*(const Scalar& other, const AutoDiffVector& v)
{
return AutoDiffVector<
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
v.values() * other,
v.jacobian() * other);
}
// template<typename OtherValueType,typename OtherJacobianType>
// inline const AutoDiffVector<
// CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
// CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >
// operator*(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
// {
// return AutoDiffVector<
// CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
// CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, 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<typename OtherValueType,typename OtherJacobianType>
inline AutoDiffVector& operator*=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
{
*this = *this * other;
return *this;
}
protected:
ValueType m_values;
JacobianType m_jacobian;
};
}
#endif // EIGEN_AUTODIFF_VECTOR_H

152
wpiutil/src/main/native/include/drake/common/drake_assert.h Normal file
View File

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

18
wpiutil/src/main/native/include/drake/common/drake_assertion_error.h Normal file
View File

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

112
wpiutil/src/main/native/include/drake/common/drake_copyable.h Normal file
View File

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

31
wpiutil/src/main/native/include/drake/common/drake_throw.h Normal file
View File

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

62
wpiutil/src/main/native/include/drake/common/is_approx_equal_abstol.h Normal file
View File

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

84
wpiutil/src/main/native/include/drake/common/never_destroyed.h Normal file
View File

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

32
wpiutil/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h Normal file
View File

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

19
wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeJNITest.java Normal file
View File

@@ -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);
}
}

69
wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeTest.java Normal file
View File

@@ -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));
}
}

76
wpiutil/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp Normal file
View File

@@ -0,0 +1,76 @@
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <gtest/gtest.h>
#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<const MatrixXd>& A,
const Eigen::Ref<const MatrixXd>& B,
const Eigen::Ref<const MatrixXd>& Q,
const Eigen::Ref<const MatrixXd>& 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<MatrixXd> 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

34
wpiutil/src/test/native/include/drake/common/autodiff.h Normal file
View File

@@ -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 <Eigen/Core>
#include <unsupported/Eigen/AutoDiff>
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

203
wpiutil/src/test/native/include/drake/common/autodiff_overloads.h Normal file
View File

@@ -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 <cmath>
#include <limits>
#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 <cmath>.
template <typename DerType>
double nexttoward(const Eigen::AutoDiffScalar<DerType>& from, long double to) {
using std::nexttoward;
return nexttoward(from.value(), to);
}
/// Overloads round to mimic std::round from <cmath>.
template <typename DerType>
double round(const Eigen::AutoDiffScalar<DerType>& x) {
using std::round;
return round(x.value());
}
/// Overloads isinf to mimic std::isinf from <cmath>.
template <typename DerType>
bool isinf(const Eigen::AutoDiffScalar<DerType>& x) {
using std::isinf;
return isinf(x.value());
}
/// Overloads isfinite to mimic std::isfinite from <cmath>.
template <typename DerType>
bool isfinite(const Eigen::AutoDiffScalar<DerType>& x) {
using std::isfinite;
return isfinite(x.value());
}
/// Overloads isnan to mimic std::isnan from <cmath>.
template <typename DerType>
bool isnan(const Eigen::AutoDiffScalar<DerType>& x) {
using std::isnan;
return isnan(x.value());
}
/// Overloads floor to mimic std::floor from <cmath>.
template <typename DerType>
double floor(const Eigen::AutoDiffScalar<DerType>& x) {
using std::floor;
return floor(x.value());
}
/// Overloads ceil to mimic std::ceil from <cmath>.
template <typename DerType>
double ceil(const Eigen::AutoDiffScalar<DerType>& x) {
using std::ceil;
return ceil(x.value());
}
/// Overloads copysign from <cmath>.
template <typename DerType, typename T>
Eigen::AutoDiffScalar<DerType> copysign(const Eigen::AutoDiffScalar<DerType>& 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 <cmath>.
template <typename DerType>
double copysign(double x, const Eigen::AutoDiffScalar<DerType>& 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 <typename DerTypeA, typename DerTypeB>
Eigen::AutoDiffScalar<
typename internal::remove_all<DerTypeA>::type::PlainObject>
pow(const Eigen::AutoDiffScalar<DerTypeA>& base,
const Eigen::AutoDiffScalar<DerTypeB>& 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<DerTypeA>::type::PlainObject,
typename internal::remove_all<DerTypeB>::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<double>::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 <typename DerType>
double ExtractDoubleOrThrow(const Eigen::AutoDiffScalar<DerType>& scalar) {
return static_cast<double>(scalar.value());
}
/// Specializes common/dummy_value.h.
template <typename DerType>
struct dummy_value<Eigen::AutoDiffScalar<DerType>> {
static constexpr Eigen::AutoDiffScalar<DerType> get() {
constexpr double kNaN = std::numeric_limits<double>::quiet_NaN();
DerType derivatives;
derivatives.fill(kNaN);
return Eigen::AutoDiffScalar<DerType>(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 <typename DerType1, typename DerType2>
inline Eigen::AutoDiffScalar<
typename Eigen::internal::remove_all<DerType1>::type::PlainObject>
if_then_else(bool f_cond, const Eigen::AutoDiffScalar<DerType1>& x,
const Eigen::AutoDiffScalar<DerType2>& y) {
typedef Eigen::AutoDiffScalar<
typename Eigen::internal::remove_all<DerType1>::type::PlainObject>
ADS1;
typedef Eigen::AutoDiffScalar<
typename Eigen::internal::remove_all<DerType2>::type::PlainObject>
ADS2;
static_assert(std::is_same<ADS1, ADS2>::value,
"The derivative types must match.");
return f_cond ? ADS1(x) : ADS2(y);
}
/// Provides special case of cond expression for Eigen::AutoDiffScalar type.
template <typename DerType, typename... Rest>
Eigen::AutoDiffScalar<
typename Eigen::internal::remove_all<DerType>::type::PlainObject>
cond(bool f_cond, const Eigen::AutoDiffScalar<DerType>& e_then, Rest... rest) {
return if_then_else(f_cond, e_then, cond(rest...));
}
} // namespace drake

423
wpiutil/src/test/native/include/drake/common/autodiffxd.h Normal file
View File

@@ -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 <gael.guennebaud@inria.fr>
// 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 <cmath>
#include <ostream>
#include <Eigen/Core>
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<VectorXd> 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<VectorXd>
// which return AutoDiffScalar<VectorXd> 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<VectorXd>
: public internal::auto_diff_special_op<VectorXd, false> {
public:
typedef internal::auto_diff_special_op<VectorXd, false> Base;
typedef typename internal::remove_all<VectorXd>::type DerType;
typedef typename internal::traits<DerType>::Scalar Scalar;
typedef typename NumTraits<Scalar>::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 <typename OtherDerType>
AutoDiffScalar(
const AutoDiffScalar<OtherDerType>& other
#ifndef EIGEN_PARSED_BY_DOXYGEN
,
typename internal::enable_if<
internal::is_same<
Scalar, typename internal::traits<typename internal::remove_all<
OtherDerType>::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 <typename OtherDerType>
inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& 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 <typename OtherDerType>
inline bool operator<(const AutoDiffScalar<OtherDerType>& b) const {
return m_value < b.value();
}
template <typename OtherDerType>
inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const {
return m_value <= b.value();
}
template <typename OtherDerType>
inline bool operator>(const AutoDiffScalar<OtherDerType>& b) const {
return m_value > b.value();
}
template <typename OtherDerType>
inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const {
return m_value >= b.value();
}
template <typename OtherDerType>
inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const {
return m_value == b.value();
}
template <typename OtherDerType>
inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const {
return m_value != b.value();
}
inline const AutoDiffScalar<DerType> operator+(const Scalar& other) const {
return AutoDiffScalar<DerType>(m_value + other, m_derivatives);
}
friend inline const AutoDiffScalar<DerType> operator+(
const Scalar& a, const AutoDiffScalar& b) {
return AutoDiffScalar<DerType>(a + b.value(), b.derivatives());
}
inline AutoDiffScalar& operator+=(const Scalar& other) {
value() += other;
return *this;
}
template <typename OtherDerType>
inline const AutoDiffScalar<DerType> operator+(
const AutoDiffScalar<OtherDerType>& 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 <typename OtherDerType>
inline AutoDiffScalar& operator+=(const AutoDiffScalar<OtherDerType>& other) {
(*this) = (*this) + other;
return *this;
}
inline const AutoDiffScalar<DerType> operator-(const Scalar& b) const {
return AutoDiffScalar<DerType>(m_value - b, m_derivatives);
}
friend inline const AutoDiffScalar<DerType> operator-(
const Scalar& a, const AutoDiffScalar& b) {
return AutoDiffScalar<DerType>(a - b.value(), -b.derivatives());
}
inline AutoDiffScalar& operator-=(const Scalar& other) {
value() -= other;
return *this;
}
template <typename OtherDerType>
inline const AutoDiffScalar<DerType> operator-(
const AutoDiffScalar<OtherDerType>& 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 <typename OtherDerType>
inline AutoDiffScalar& operator-=(const AutoDiffScalar<OtherDerType>& other) {
*this = *this - other;
return *this;
}
inline const AutoDiffScalar<DerType> operator-() const {
return AutoDiffScalar<DerType>(-m_value, -m_derivatives);
}
inline const AutoDiffScalar<DerType> operator*(const Scalar& other) const {
return MakeAutoDiffScalar(m_value * other, m_derivatives * other);
}
friend inline const AutoDiffScalar<DerType> operator*(
const Scalar& other, const AutoDiffScalar& a) {
return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other);
}
inline const AutoDiffScalar<DerType> operator/(const Scalar& other) const {
return MakeAutoDiffScalar(m_value / other,
(m_derivatives * (Scalar(1) / other)));
}
friend inline const AutoDiffScalar<DerType> operator/(
const Scalar& other, const AutoDiffScalar& a) {
return MakeAutoDiffScalar(
other / a.value(),
a.derivatives() * (Scalar(-other) / (a.value() * a.value())));
}
template <typename OtherDerType>
inline const AutoDiffScalar<DerType> operator/(
const AutoDiffScalar<OtherDerType>& 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 <typename OtherDerType>
inline const AutoDiffScalar<DerType> operator*(
const AutoDiffScalar<OtherDerType>& 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 <typename OtherDerType>
inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other) {
*this = *this * other;
return *this;
}
inline AutoDiffScalar& operator/=(const Scalar& other) {
*this = *this / other;
return *this;
}
template <typename OtherDerType>
inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& 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<VectorXd> FUNC( \
const AutoDiffScalar<VectorXd>& 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<VectorXd> atan2(const AutoDiffScalar<VectorXd>& a,
const AutoDiffScalar<VectorXd>& 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<VectorXd> pow(const AutoDiffScalar<VectorXd>& a,
double b) {
using std::pow;
return MakeAutoDiffScalar(pow(a.value(), b),
a.derivatives() * (b * pow(a.value(), b - 1)));
}
#endif
} // namespace Eigen

44
wpiutil/src/test/native/include/drake/common/cond.h Normal file
View File

@@ -0,0 +1,44 @@
#pragma once
#include <functional>
#include <type_traits>
#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 <tt>operator<(ScalarType,
ScalarType)</tt>. For example, @c symbolic::Expression can be used as a @p
ScalarType because it provides <tt>symbolic::Formula
operator<(symbolic::Expression, symbolic::Expression)</tt>.
@{
*/
template <typename ScalarType>
ScalarType cond(const ScalarType& e) {
return e;
}
template <typename ScalarType, typename... Rest>
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

19
wpiutil/src/test/native/include/drake/common/constants.h Normal file
View File

@@ -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

21
wpiutil/src/test/native/include/drake/common/double_overloads.h Normal file
View File

@@ -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
/// <tt>if_then_else(f_cond, v_then, v_else)</tt>, 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 <tt>f_cond ? v_then : v_else</tt>.
inline double if_then_else(bool f_cond, double v_then, double v_else) {
return f_cond ? v_then : v_else;
}
} // namespace drake

65
wpiutil/src/test/native/include/drake/common/drake_deprecated.h Normal file
View File

@@ -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

13
wpiutil/src/test/native/include/drake/common/drake_nodiscard.h Normal file
View File

@@ -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

35
wpiutil/src/test/native/include/drake/common/dummy_value.h Normal file
View File

@@ -0,0 +1,35 @@
#pragma once
#include <limits>
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 <typename T>
struct dummy_value {
static constexpr T get() {
static_assert(std::numeric_limits<T>::has_quiet_NaN,
"Custom scalar types should specialize this struct");
return std::numeric_limits<T>::quiet_NaN();
}
};
template <>
struct dummy_value<int> {
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

20
wpiutil/src/test/native/include/drake/common/eigen_autodiff_limits.h Normal file
View File

@@ -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 <limits>
// Eigen provides `numeric_limits<AutoDiffScalar<T>>` starting with v3.3.4.
#if !EIGEN_VERSION_AT_LEAST(3, 3, 4) // Eigen Version < v3.3.4
namespace std {
template <typename T>
class numeric_limits<Eigen::AutoDiffScalar<T>>
: public numeric_limits<typename T::Scalar> {};
} // namespace std
#endif // Eigen Version < v3.3.4

38
wpiutil/src/test/native/include/drake/common/eigen_autodiff_types.h Normal file
View File

@@ -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 <type_traits>
#include <Eigen/Core>
#include "drake/common/eigen_types.h"
namespace drake {
/// An autodiff variable with a dynamic number of partials.
using AutoDiffXd = Eigen::AutoDiffScalar<Eigen::VectorXd>;
// TODO(hongkai-dai): Recursive template to get arbitrary gradient order.
/// An autodiff variable with `num_vars` partials.
template <int num_vars>
using AutoDiffd = Eigen::AutoDiffScalar<Eigen::Matrix<double, num_vars, 1> >;
/// A vector of `rows` autodiff variables, each with `num_vars` partials.
template <int num_vars, int rows>
using AutoDiffVecd = Eigen::Matrix<AutoDiffd<num_vars>, rows, 1>;
/// A dynamic-sized vector of autodiff variables, each with a dynamic-sized
/// vector of partials.
typedef AutoDiffVecd<Eigen::Dynamic, Eigen::Dynamic> AutoDiffVecXd;
} // namespace drake

461
wpiutil/src/test/native/include/drake/common/eigen_types.h Normal file
View File

@@ -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 `<T>` for scalar types across the entire code base
/// we decided in this file to use `<Scalar>` to be more consistent with the
/// usage of `<Scalar>` in Eigen's code base.
/// @see also eigen_autodiff_types.h
#include <utility>
#include <Eigen/Core>
#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 <typename Scalar>
using Vector0 = Eigen::Matrix<Scalar, 0, 1>;
/// A column vector of size 1 (that is, a scalar), templated on scalar type.
template <typename Scalar>
using Vector1 = Eigen::Matrix<Scalar, 1, 1>;
/// A column vector of size 1 of doubles.
using Vector1d = Eigen::Matrix<double, 1, 1>;
/// A column vector of size 2, templated on scalar type.
template <typename Scalar>
using Vector2 = Eigen::Matrix<Scalar, 2, 1>;
/// A column vector of size 3, templated on scalar type.
template <typename Scalar>
using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
/// A column vector of size 4, templated on scalar type.
template <typename Scalar>
using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
/// A column vector of size 6.
template <typename Scalar>
using Vector6 = Eigen::Matrix<Scalar, 6, 1>;
/// A column vector templated on the number of rows.
template <typename Scalar, int Rows>
using Vector = Eigen::Matrix<Scalar, Rows, 1>;
/// A column vector of any size, templated on scalar type.
template <typename Scalar>
using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
/// A vector of dynamic size templated on scalar type, up to a maximum of 6
/// elements.
template <typename Scalar>
using VectorUpTo6 = Eigen::Matrix<Scalar, Eigen::Dynamic, 1, 0, 6, 1>;
/// A row vector of size 2, templated on scalar type.
template <typename Scalar>
using RowVector2 = Eigen::Matrix<Scalar, 1, 2>;
/// A row vector of size 3, templated on scalar type.
template <typename Scalar>
using RowVector3 = Eigen::Matrix<Scalar, 1, 3>;
/// A row vector of size 4, templated on scalar type.
template <typename Scalar>
using RowVector4 = Eigen::Matrix<Scalar, 1, 4>;
/// A row vector of size 6.
template <typename Scalar>
using RowVector6 = Eigen::Matrix<Scalar, 1, 6>;
/// A row vector templated on the number of columns.
template <typename Scalar, int Cols>
using RowVector = Eigen::Matrix<Scalar, 1, Cols>;
/// A row vector of any size, templated on scalar type.
template <typename Scalar>
using RowVectorX = Eigen::Matrix<Scalar, 1, Eigen::Dynamic>;
/// A matrix of 2 rows and 2 columns, templated on scalar type.
template <typename Scalar>
using Matrix2 = Eigen::Matrix<Scalar, 2, 2>;
/// A matrix of 3 rows and 3 columns, templated on scalar type.
template <typename Scalar>
using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
/// A matrix of 4 rows and 4 columns, templated on scalar type.
template <typename Scalar>
using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
/// A matrix of 6 rows and 6 columns, templated on scalar type.
template <typename Scalar>
using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
/// A matrix of 2 rows, dynamic columns, templated on scalar type.
template <typename Scalar>
using Matrix2X = Eigen::Matrix<Scalar, 2, Eigen::Dynamic>;
/// A matrix of 3 rows, dynamic columns, templated on scalar type.
template <typename Scalar>
using Matrix3X = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>;
/// A matrix of 4 rows, dynamic columns, templated on scalar type.
template <typename Scalar>
using Matrix4X = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>;
/// A matrix of 6 rows, dynamic columns, templated on scalar type.
template <typename Scalar>
using Matrix6X = Eigen::Matrix<Scalar, 6, Eigen::Dynamic>;
/// A matrix of dynamic size, templated on scalar type.
template <typename Scalar>
using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
/// 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 <typename Scalar>
using MatrixUpTo6 =
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, 6, 6>;
/// A quaternion templated on scalar type.
template <typename Scalar>
using Quaternion = Eigen::Quaternion<Scalar>;
/// An AngleAxis templated on scalar type.
template <typename Scalar>
using AngleAxis = Eigen::AngleAxis<Scalar>;
/// An Isometry templated on scalar type.
template <typename Scalar>
using Isometry3 = Eigen::Transform<Scalar, 3, Eigen::Isometry>;
/// A translation in 3D templated on scalar type.
template <typename Scalar>
using Translation3 = Eigen::Translation<Scalar, 3>;
/// A column vector consisting of one twist.
template <typename Scalar>
using TwistVector = Eigen::Matrix<Scalar, kTwistSize, 1>;
/// A matrix with one twist per column, and dynamically many columns.
template <typename Scalar>
using TwistMatrix = Eigen::Matrix<Scalar, kTwistSize, Eigen::Dynamic>;
/// A six-by-six matrix.
template <typename Scalar>
using SquareTwistMatrix = Eigen::Matrix<Scalar, kTwistSize, kTwistSize>;
/// 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 <typename Scalar>
using WrenchVector = Eigen::Matrix<Scalar, 6, 1>;
/// 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 <typename Scalar>
using SpatialForce
DRAKE_DEPRECATED("2019-10-15", "Please use Vector6<> instead.")
= Eigen::Matrix<Scalar, 6, 1>;
/// EigenSizeMinPreferDynamic<a, b>::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 <int a, int b>
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 <int a, int b>
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<a, b> gives a * b if both of a and b are fixed
/// sizes. Otherwise it gives Eigen::Dynamic.
template <int a, int b>
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 <typename Derived>
struct is_eigen_type : std::is_base_of<Eigen::EigenBase<Derived>, Derived> {};
/*
* Determines if an EigenBase<> has a specific scalar type.
*/
template <typename Derived, typename Scalar>
struct is_eigen_scalar_same
: std::integral_constant<
bool, is_eigen_type<Derived>::value &&
std::is_same<typename Derived::Scalar, Scalar>::value> {};
/*
* Determines if an EigenBase<> type is a compile-time (column) vector.
* This will not check for run-time size.
*/
template <typename Derived>
struct is_eigen_vector
: std::integral_constant<bool, is_eigen_type<Derived>::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 <typename Derived, typename Scalar>
struct is_eigen_vector_of
: std::integral_constant<
bool, is_eigen_scalar_same<Derived, Scalar>::value &&
is_eigen_vector<Derived>::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 <typename Derived, typename Scalar>
struct is_eigen_nonvector_of
: std::integral_constant<
bool, is_eigen_scalar_same<Derived, Scalar>::value &&
!is_eigen_vector<Derived>::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<T>`
/// and provides access to it via `operator*` and `operator->`.
///
/// The motivation of this class is to follow <a
/// href="https://google.github.io/styleguide/cppguide.html#Reference_Arguments">GSG's
/// "output arguments should be pointers" rule</a> 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<Eigen::MatrixXd> 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<Eigen::MatrixXd> 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<T>`
/// 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<T>*`, 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
/// <a
/// href="https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#TopicPlainFunctionsFailing">Eigen's
/// documentation</a>.
template <typename PlainObjectType>
class EigenPtr {
public:
typedef Eigen::Ref<PlainObjectType> 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 <typename PlainObjectTypeIn>
// NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
EigenPtr(PlainObjectTypeIn* m) {
if (m) {
m_.set_value(m);
}
}
/// Constructs from another EigenPtr.
template <typename PlainObjectTypeIn>
// NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
EigenPtr(const EigenPtr<PlainObjectTypeIn>& 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 <typename PlainObjectTypeIn>
EigenPtr& operator=(const EigenPtr<PlainObjectTypeIn>& 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 <typename PlainObjectTypeIn>
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<RefType&>(storage_); }
bool has_value_{};
typename std::aligned_storage<sizeof(RefType), alignof(RefType)>::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 <typename PlainObjectTypeIn>
EigenPtr& assign(const EigenPtr<PlainObjectTypeIn>& 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

116
wpiutil/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h Normal file
View File

@@ -0,0 +1,116 @@
#pragma once
#include <algorithm>
#include <cmath>
#include <limits>
#include <Eigen/Core>
#include <gtest/gtest.h>
#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 <typename DerivedA, typename DerivedB>
DRAKE_NODISCARD ::testing::AssertionResult CompareMatrices(
const Eigen::MatrixBase<DerivedA>& m1,
const Eigen::MatrixBase<DerivedB>& m2, double tolerance = 0.0,
MatrixCompareType compare_type = MatrixCompareType::absolute) {
if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) {
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<double>::infinity() &&
m2(ii, jj) == std::numeric_limits<double>::infinity();
const auto both_negative_infinity =
m1(ii, jj) == -std::numeric_limits<double>::infinity() &&
m2(ii, jj) == -std::numeric_limits<double>::infinity();
using std::isnan;
const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj));
if (both_positive_infinity || both_negative_infinity || both_nan)
continue;
// Check for case where one value is NaN and the other is not
if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) ||
(!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) {
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

53
wpiutil/src/test/native/include/drake/common/unused.h Normal file
View File

@@ -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<T>& context, State<T>* state) const {
/// unused(context);
/// state->SetZero();
/// }
/// @endcode
///
template <typename ... Args>
void unused(const Args& ...) {}
} // namespace drake

332
wpiutil/src/test/native/include/drake/math/autodiff.h Normal file
View File

@@ -0,0 +1,332 @@
/// @file
/// Utilities for arithmetic on AutoDiffScalar.
// TODO(russt): rename methods to be GSG compliant.
#pragma once
#include <cmath>
#include <tuple>
#include <Eigen/Core>
#include "drake/common/autodiff.h"
#include "drake/common/unused.h"
namespace drake {
namespace math {
template <typename Derived>
struct AutoDiffToValueMatrix {
typedef typename Eigen::Matrix<typename Derived::Scalar::Scalar,
Derived::RowsAtCompileTime,
Derived::ColsAtCompileTime> type;
};
template <typename Derived>
typename AutoDiffToValueMatrix<Derived>::type autoDiffToValueMatrix(
const Eigen::MatrixBase<Derived>& auto_diff_matrix) {
typename AutoDiffToValueMatrix<Derived>::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<AutoDiffXd> A`, the
* comparable operation
* `B = A.cast<double>()`
* 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 Derived>
typename std::enable_if<
!std::is_same<typename Derived::Scalar, double>::value,
Eigen::Matrix<typename Derived::Scalar::Scalar, Derived::RowsAtCompileTime,
Derived::ColsAtCompileTime, 0, Derived::MaxRowsAtCompileTime,
Derived::MaxColsAtCompileTime>>::type
DiscardGradient(const Eigen::MatrixBase<Derived>& auto_diff_matrix) {
return autoDiffToValueMatrix(auto_diff_matrix);
}
/// @see DiscardGradient().
template <typename Derived>
typename std::enable_if<
std::is_same<typename Derived::Scalar, double>::value,
const Eigen::MatrixBase<Derived>&>::type
DiscardGradient(const Eigen::MatrixBase<Derived>& matrix) {
return matrix;
}
/// @see DiscardGradient().
template <typename _Scalar, int _Dim, int _Mode, int _Options>
typename std::enable_if<
!std::is_same<_Scalar, double>::value,
Eigen::Transform<typename _Scalar::Scalar, _Dim, _Mode, _Options>>::type
DiscardGradient(const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>&
auto_diff_transform) {
return Eigen::Transform<typename _Scalar::Scalar, _Dim, _Mode, _Options>(
autoDiffToValueMatrix(auto_diff_transform.matrix()));
}
/// @see DiscardGradient().
template <typename _Scalar, int _Dim, int _Mode, int _Options>
typename std::enable_if<std::is_same<_Scalar, double>::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 <typename Derived, typename DerivedAutoDiff>
void initializeAutoDiff(const Eigen::MatrixBase<Derived>& val,
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
Eigen::MatrixBase<DerivedAutoDiff>& auto_diff_matrix,
Eigen::DenseIndex num_derivatives = Eigen::Dynamic,
Eigen::DenseIndex deriv_num_start = 0) {
using ADScalar = typename DerivedAutoDiff::Scalar;
static_assert(static_cast<int>(Derived::RowsAtCompileTime) ==
static_cast<int>(DerivedAutoDiff::RowsAtCompileTime),
"auto diff matrix has wrong number of rows at compile time");
static_assert(static_cast<int>(Derived::ColsAtCompileTime) ==
static_cast<int>(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 <typename Derived, int Nq>
using AutoDiffMatrixType = Eigen::Matrix<
Eigen::AutoDiffScalar<Eigen::Matrix<typename Derived::Scalar, Nq, 1>>,
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 <int Nq = Eigen::Dynamic, typename Derived>
AutoDiffMatrixType<Derived, Nq> initializeAutoDiff(
const Eigen::MatrixBase<Derived>& mat,
Eigen::DenseIndex num_derivatives = -1,
Eigen::DenseIndex deriv_num_start = 0) {
if (num_derivatives == -1) num_derivatives = mat.size();
AutoDiffMatrixType<Derived, Nq> ret(mat.rows(), mat.cols());
initializeAutoDiff(mat, ret, num_derivatives, deriv_num_start);
return ret;
}
namespace internal {
template <typename Derived, typename Scalar>
struct ResizeDerivativesToMatchScalarImpl {
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
static void run(Eigen::MatrixBase<Derived>&, const Scalar&) {}
};
template <typename Derived, typename DerivType>
struct ResizeDerivativesToMatchScalarImpl<Derived,
Eigen::AutoDiffScalar<DerivType>> {
using Scalar = Eigen::AutoDiffScalar<DerivType>;
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
static void run(Eigen::MatrixBase<Derived>& 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 <typename Derived>
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
void resizeDerivativesToMatchScalar(Eigen::MatrixBase<Derived>& mat,
const typename Derived::Scalar& scalar) {
internal::ResizeDerivativesToMatchScalarImpl<
Derived, typename Derived::Scalar>::run(mat, scalar);
}
namespace internal {
/** \brief Helper for totalSizeAtCompileTime function (recursive)
*/
template <typename Head, typename... Tail>
struct TotalSizeAtCompileTime {
static constexpr int eval() {
return Head::SizeAtCompileTime == Eigen::Dynamic ||
TotalSizeAtCompileTime<Tail...>::eval() == Eigen::Dynamic
? Eigen::Dynamic
: Head::SizeAtCompileTime +
TotalSizeAtCompileTime<Tail...>::eval();
}
};
/** \brief Helper for totalSizeAtCompileTime function (base case)
*/
template <typename Head>
struct TotalSizeAtCompileTime<Head> {
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 <typename... Args>
constexpr int totalSizeAtCompileTime() {
return TotalSizeAtCompileTime<Args...>::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 <typename Head, typename... Tail>
Eigen::DenseIndex totalSizeAtRunTime(const Eigen::MatrixBase<Head>& head,
const Tail&... tail) {
return head.size() + totalSizeAtRunTime(tail...);
}
/** \brief Helper for initializeAutoDiffTuple function (recursive)
*/
template <size_t Index>
struct InitializeAutoDiffTupleHelper {
template <typename... ValueTypes, typename... AutoDiffTypes>
static void run(const std::tuple<ValueTypes...>& values,
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
std::tuple<AutoDiffTypes...>& auto_diffs,
Eigen::DenseIndex num_derivatives,
Eigen::DenseIndex deriv_num_start) {
constexpr size_t tuple_index = sizeof...(AutoDiffTypes)-Index;
const auto& value = std::get<tuple_index>(values);
auto& auto_diff = std::get<tuple_index>(auto_diffs);
auto_diff.resize(value.rows(), value.cols());
initializeAutoDiff(value, auto_diff, num_derivatives, deriv_num_start);
InitializeAutoDiffTupleHelper<Index - 1>::run(
values, auto_diffs, num_derivatives, deriv_num_start + value.size());
}
};
/** \brief Helper for initializeAutoDiffTuple function (base case)
*/
template <>
struct InitializeAutoDiffTupleHelper<0> {
template <typename... ValueTypes, typename... AutoDiffTypes>
static void run(const std::tuple<ValueTypes...>& values,
const std::tuple<AutoDiffTypes...>& 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 <typename... Deriveds>
std::tuple<AutoDiffMatrixType<
Deriveds, internal::totalSizeAtCompileTime<Deriveds...>()>...>
initializeAutoDiffTuple(const Eigen::MatrixBase<Deriveds>&... args) {
Eigen::DenseIndex dynamic_num_derivs = internal::totalSizeAtRunTime(args...);
std::tuple<AutoDiffMatrixType<
Deriveds, internal::totalSizeAtCompileTime<Deriveds...>()>...>
ret(AutoDiffMatrixType<Deriveds,
internal::totalSizeAtCompileTime<Deriveds...>()>(
args.rows(), args.cols())...);
auto values = std::forward_as_tuple(args...);
internal::InitializeAutoDiffTupleHelper<sizeof...(args)>::run(
values, ret, dynamic_num_derivs, 0);
return ret;
}
} // namespace math
} // namespace drake