Drake
autodiff.h
Go to the documentation of this file.
1 
4 #pragma once
5 
6 #include <cmath>
7 #include <tuple>
8 
9 #include <Eigen/Dense>
10 
12 #include "drake/common/unused.h"
13 
14 namespace drake {
15 namespace math {
16 
17 template <typename Derived>
19  typedef typename Eigen::Matrix<typename Derived::Scalar::Scalar,
20  Derived::RowsAtCompileTime,
21  Derived::ColsAtCompileTime>
23 };
24 
25 template <typename Derived>
27  const Eigen::MatrixBase<Derived>& auto_diff_matrix) {
28  typename AutoDiffToValueMatrix<Derived>::type ret(auto_diff_matrix.rows(),
29  auto_diff_matrix.cols());
30  for (int i = 0; i < auto_diff_matrix.rows(); i++) {
31  for (int j = 0; j < auto_diff_matrix.cols(); ++j) {
32  ret(i, j) = auto_diff_matrix(i, j).value();
33  }
34  }
35  return ret;
36 }
37 
55 template <typename Derived, typename DerivedAutoDiff>
56 void initializeAutoDiff(const Eigen::MatrixBase<Derived>& val,
57  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
58  Eigen::MatrixBase<DerivedAutoDiff>& auto_diff_matrix,
59  Eigen::DenseIndex num_derivatives = Eigen::Dynamic,
60  Eigen::DenseIndex deriv_num_start = 0) {
61  using ADScalar = typename DerivedAutoDiff::Scalar;
62  static_assert(static_cast<int>(Derived::RowsAtCompileTime) ==
63  static_cast<int>(DerivedAutoDiff::RowsAtCompileTime),
64  "auto diff matrix has wrong number of rows at compile time");
65  static_assert(static_cast<int>(Derived::ColsAtCompileTime) ==
66  static_cast<int>(DerivedAutoDiff::ColsAtCompileTime),
67  "auto diff matrix has wrong number of columns at compile time");
68 
69  if (num_derivatives == Eigen::Dynamic) num_derivatives = val.size();
70 
71  auto_diff_matrix.resize(val.rows(), val.cols());
72  Eigen::DenseIndex deriv_num = deriv_num_start;
73  for (Eigen::DenseIndex i = 0; i < val.size(); i++) {
74  auto_diff_matrix(i) = ADScalar(val(i), num_derivatives, deriv_num++);
75  }
76 }
77 
81 template <typename Derived, int Nq>
82 using AutoDiffMatrixType = Eigen::Matrix<
83  Eigen::AutoDiffScalar<Eigen::Matrix<typename Derived::Scalar, Nq, 1> >,
84  Derived::RowsAtCompileTime, Derived::ColsAtCompileTime, 0,
85  Derived::MaxRowsAtCompileTime, Derived::MaxColsAtCompileTime>;
86 
104 template <int Nq = Eigen::Dynamic, typename Derived>
106  const Eigen::MatrixBase<Derived>& mat,
107  Eigen::DenseIndex num_derivatives = -1,
108  Eigen::DenseIndex deriv_num_start = 0) {
109  if (num_derivatives == -1) num_derivatives = mat.size();
110 
111  AutoDiffMatrixType<Derived, Nq> ret(mat.rows(), mat.cols());
112  initializeAutoDiff(mat, ret, num_derivatives, deriv_num_start);
113  return ret;
114 }
115 
116 namespace internal {
117 template <typename Derived, typename Scalar>
119  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
120  static void run(Eigen::MatrixBase<Derived>&, const Scalar&) {}
121 };
122 
123 template <typename Derived, typename DerivType>
125  Eigen::AutoDiffScalar<DerivType> > {
126  using Scalar = Eigen::AutoDiffScalar<DerivType>;
127  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
128  static void run(Eigen::MatrixBase<Derived>& mat, const Scalar& scalar) {
129  for (int i = 0; i < mat.size(); i++) {
130  auto& derivs = mat(i).derivatives();
131  if (derivs.size() == 0) {
132  derivs.resize(scalar.derivatives().size());
133  derivs.setZero();
134  }
135  }
136  }
137 };
138 } // namespace internal
139 
152 template <typename Derived>
153 // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
154 void resizeDerivativesToMatchScalar(Eigen::MatrixBase<Derived>& mat,
155  const typename Derived::Scalar& scalar) {
157  Derived, typename Derived::Scalar>::run(mat, scalar);
158 }
159 
160 namespace internal {
163 template <typename Head, typename... Tail>
165  static constexpr int eval() {
166  return Head::SizeAtCompileTime == Eigen::Dynamic ||
167  TotalSizeAtCompileTime<Tail...>::eval() == Eigen::Dynamic
168  ? Eigen::Dynamic
169  : Head::SizeAtCompileTime +
171  }
172 };
173 
176 template <typename Head>
178  static constexpr int eval() { return Head::SizeAtCompileTime; }
179 };
180 
184 template <typename... Args>
185 constexpr int totalSizeAtCompileTime() {
187 }
188 
192 constexpr Eigen::DenseIndex totalSizeAtRunTime() { return 0; }
193 
197 template <typename Head, typename... Tail>
198 Eigen::DenseIndex totalSizeAtRunTime(const Eigen::MatrixBase<Head>& head,
199  const Tail&... tail) {
200  return head.size() + totalSizeAtRunTime(tail...);
201 }
202 
205 template <size_t Index>
207  template <typename... ValueTypes, typename... AutoDiffTypes>
208  static void run(const std::tuple<ValueTypes...>& values,
209  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
210  std::tuple<AutoDiffTypes...>& auto_diffs,
211  Eigen::DenseIndex num_derivatives,
212  Eigen::DenseIndex deriv_num_start) {
213  constexpr size_t tuple_index = sizeof...(AutoDiffTypes)-Index;
214  const auto& value = std::get<tuple_index>(values);
215  auto& auto_diff = std::get<tuple_index>(auto_diffs);
216  auto_diff.resize(value.rows(), value.cols());
217  initializeAutoDiff(value, auto_diff, num_derivatives, deriv_num_start);
219  values, auto_diffs, num_derivatives, deriv_num_start + value.size());
220  }
221 };
222 
225 template <>
227  template <typename... ValueTypes, typename... AutoDiffTypes>
228  static void run(const std::tuple<ValueTypes...>& values,
229  const std::tuple<AutoDiffTypes...>& auto_diffs,
230  Eigen::DenseIndex num_derivatives,
231  Eigen::DenseIndex deriv_num_start) {
232  unused(values, auto_diffs, num_derivatives, deriv_num_start);
233  }
234 };
235 } // namespace internal
236 
259 template <typename... Deriveds>
260 std::tuple<AutoDiffMatrixType<
261  Deriveds, internal::totalSizeAtCompileTime<Deriveds...>()>...>
262 initializeAutoDiffTuple(const Eigen::MatrixBase<Deriveds>&... args) {
263  Eigen::DenseIndex dynamic_num_derivs = internal::totalSizeAtRunTime(args...);
264  std::tuple<AutoDiffMatrixType<
265  Deriveds, internal::totalSizeAtCompileTime<Deriveds...>()>...>
266  ret(AutoDiffMatrixType<Deriveds,
267  internal::totalSizeAtCompileTime<Deriveds...>()>(
268  args.rows(), args.cols())...);
269  auto values = std::forward_as_tuple(args...);
270  internal::InitializeAutoDiffTupleHelper<sizeof...(args)>::run(
271  values, ret, dynamic_num_derivs, 0);
272  return ret;
273 }
274 
275 } // namespace math
276 } // namespace drake
static constexpr int eval()
Definition: autodiff.h:165
AutoDiffToValueMatrix< Derived >::type autoDiffToValueMatrix(const Eigen::MatrixBase< Derived > &auto_diff_matrix)
Definition: autodiff.h:26
Definition: automotive_demo.cc:88
static void run(const std::tuple< ValueTypes... > &values, const std::tuple< AutoDiffTypes... > &auto_diffs, Eigen::DenseIndex num_derivatives, Eigen::DenseIndex deriv_num_start)
Definition: autodiff.h:228
Definition: autodiff.h:18
AutoDiffXd eval(const Eigen::AutoDiffScalar< Derived > &x)
Force Eigen to evaluate an autodiff expression.
Definition: pydrake_autodiffutils.cc:22
Definition: autodiff_overloads.h:30
static constexpr int eval()
Definition: autodiff.h:178
Eigen::Matrix< Eigen::AutoDiffScalar< Eigen::Matrix< typename Derived::Scalar, Nq, 1 > >, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime, 0, Derived::MaxRowsAtCompileTime, Derived::MaxColsAtCompileTime > AutoDiffMatrixType
The appropriate AutoDiffScalar gradient type given the value type and the number of derivatives at co...
Definition: autodiff.h:85
Overloads for STL mathematical operations on AutoDiffScalar.
Eigen::Matrix< typename Derived::Scalar::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime > type
Definition: autodiff.h:22
constexpr Eigen::DenseIndex totalSizeAtRunTime()
Determine the total size at runtime of a number of arguments using their size() methods (base case)...
Definition: autodiff.h:192
constexpr int totalSizeAtCompileTime()
Determine the total size at compile time of a number of arguments based on their SizeAtCompileTime st...
Definition: autodiff.h:185
int value
Definition: copyable_unique_ptr_test.cc:61
std::tuple< AutoDiffMatrixType< Deriveds, internal::totalSizeAtCompileTime< Deriveds... >)>... > initializeAutoDiffTuple(const Eigen::MatrixBase< Deriveds > &...args)
Given a series of Eigen matrices, create a tuple of corresponding AutoDiff matrices with values equal...
Definition: autodiff.h:262
Eigen::DenseIndex totalSizeAtRunTime(const Eigen::MatrixBase< Head > &head, const Tail &...tail)
Determine the total size at runtime of a number of arguments using their size() methods (recursive) ...
Definition: autodiff.h:198
Helper for totalSizeAtCompileTime function (recursive)
Definition: autodiff.h:164
Helper for initializeAutoDiffTuple function (recursive)
Definition: autodiff.h:206
static void run(Eigen::MatrixBase< Derived > &, const Scalar &)
Definition: autodiff.h:120
void resizeDerivativesToMatchScalar(Eigen::MatrixBase< Derived > &mat, const typename Derived::Scalar &scalar)
Resize derivatives vector of each element of a matrix to to match the size of the derivatives vector ...
Definition: autodiff.h:154
static void run(const std::tuple< ValueTypes... > &values, std::tuple< AutoDiffTypes... > &auto_diffs, Eigen::DenseIndex num_derivatives, Eigen::DenseIndex deriv_num_start)
Definition: autodiff.h:208
static void run(Eigen::MatrixBase< Derived > &mat, const Scalar &scalar)
Definition: autodiff.h:128
void initializeAutoDiff(const Eigen::MatrixBase< Derived > &val, Eigen::MatrixBase< DerivedAutoDiff > &auto_diff_matrix, Eigen::DenseIndex num_derivatives=Eigen::Dynamic, Eigen::DenseIndex deriv_num_start=0)
Initialize a single autodiff matrix given the corresponding value matrix.
Definition: autodiff.h:56
void unused(const Args &...)
Documents the argument(s) as unused, placating GCC&#39;s -Wunused-parameter warning.
Definition: unused.h:53