mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
[wpimath] Move math functionality into new wpimath library (#2629)
The wpimath library is a new library designed to separate the reusable math functionality from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j). Package names / include file names were NOT changed to minimize breakage. In a future year it would be good to revamp these for a more uniform user experience and to reduce the risk of accidental naming conflicts. While theoretically all of this functionality could be placed into wpiutil, several pieces of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this expense for users who only want cscore or ntcore. It also allows for easy future separation of build tasks vs number of workers on memory-constrained machines. This moves the following functionality from wpiutil into wpimath: - Eigen - ejml - Drake - DARE - wpiutil.math package (Matrix etc) - units And the following functionality from wpilibc/j into wpimath: - Geometry - Kinematics - Spline - Trajectory - LinearFilter - MedianFilter - Feed-forward controllers
This commit is contained in:
Peter Johnson
34
wpimath/src/test/native/include/drake/common/autodiff.h
Normal file
34
wpimath/src/test/native/include/drake/common/autodiff.h
Normal 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
|
||||
@@ -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
wpimath/src/test/native/include/drake/common/autodiffxd.h
Normal file
423
wpimath/src/test/native/include/drake/common/autodiffxd.h
Normal 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
wpimath/src/test/native/include/drake/common/cond.h
Normal file
44
wpimath/src/test/native/include/drake/common/cond.h
Normal 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
wpimath/src/test/native/include/drake/common/constants.h
Normal file
19
wpimath/src/test/native/include/drake/common/constants.h
Normal 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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
wpimath/src/test/native/include/drake/common/dummy_value.h
Normal file
35
wpimath/src/test/native/include/drake/common/dummy_value.h
Normal 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
|
||||
@@ -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
|
||||
@@ -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
wpimath/src/test/native/include/drake/common/eigen_types.h
Normal file
461
wpimath/src/test/native/include/drake/common/eigen_types.h
Normal 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
|
||||
@@ -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
wpimath/src/test/native/include/drake/common/unused.h
Normal file
53
wpimath/src/test/native/include/drake/common/unused.h
Normal 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
wpimath/src/test/native/include/drake/math/autodiff.h
Normal file
332
wpimath/src/test/native/include/drake/math/autodiff.h
Normal 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
|
||||
40
wpimath/src/test/native/include/trajectory/TestTrajectory.h
Normal file
40
wpimath/src/test/native/include/trajectory/TestTrajectory.h
Normal file
@@ -0,0 +1,40 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "frc/trajectory/Trajectory.h"
|
||||
#include "frc/trajectory/TrajectoryGenerator.h"
|
||||
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
|
||||
|
||||
namespace frc {
|
||||
class TestTrajectory {
|
||||
public:
|
||||
static Trajectory GetTrajectory(TrajectoryConfig& config) {
|
||||
// 2018 cross scale auto waypoints
|
||||
const Pose2d sideStart{1.54_ft, 23.23_ft, Rotation2d(180_deg)};
|
||||
const Pose2d crossScale{23.7_ft, 6.8_ft, Rotation2d(-160_deg)};
|
||||
|
||||
config.SetReversed(true);
|
||||
|
||||
auto vector = std::vector<Translation2d>{
|
||||
(sideStart + Transform2d{Translation2d(-13_ft, 0_ft), Rotation2d()})
|
||||
.Translation(),
|
||||
(sideStart +
|
||||
Transform2d{Translation2d(-19.5_ft, 5.0_ft), Rotation2d(-90_deg)})
|
||||
.Translation()};
|
||||
|
||||
return TrajectoryGenerator::GenerateTrajectory(sideStart, vector,
|
||||
crossScale, config);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
Reference in New Issue
Block a user