Drake
autodiff.h
Go to the documentation of this file.
1 /// @file
2 /// Utilities for arithmetic on AutoDiffScalar.
3 
4 // TODO(russt): rename methods to be GSG compliant.
5 
6 #pragma once
7 
8 #include <cmath>
9 #include <tuple>
10 
11 #include <Eigen/Dense>
12 
13 #include "drake/common/autodiff.h"
14 #include "drake/common/unused.h"
15 
16 namespace drake {
17 namespace math {
18 
19 template <typename Derived>
21  typedef typename Eigen::Matrix<typename Derived::Scalar::Scalar,
22  Derived::RowsAtCompileTime,
23  Derived::ColsAtCompileTime> type;
24 };
25 
26 template <typename Derived>
28  const Eigen::MatrixBase<Derived>& auto_diff_matrix) {
29  typename AutoDiffToValueMatrix<Derived>::type ret(auto_diff_matrix.rows(),
30  auto_diff_matrix.cols());
31  for (int i = 0; i < auto_diff_matrix.rows(); i++) {
32  for (int j = 0; j < auto_diff_matrix.cols(); ++j) {
33  ret(i, j) = auto_diff_matrix(i, j).value();
34  }
35  }
36  return ret;
37 }
38 
39 /** `B = DiscardGradient(A)` enables casting from a matrix of AutoDiffScalars
40  * to AutoDiffScalar::Scalar type, explicitly throwing away any gradient
41  * information. For a matrix of type, e.g. `MatrixX<AutoDiffXd> A`, the
42  * comparable operation
43  * `B = A.cast<double>()`
44  * should (and does) fail to compile. Use `DiscardGradient(A)` if you want to
45  * force the cast (and explicitly declare that information is lost).
46  *
47  * This method is overloaded to permit the user to call it for double types and
48  * AutoDiffScalar types (to avoid the calling function having to handle the
49  * two cases differently).
50  *
51  * @see DiscardZeroGradient
52  */
53 template <typename Derived>
54 typename std::enable_if<
56  Eigen::Matrix<typename Derived::Scalar::Scalar, Derived::RowsAtCompileTime,
57  Derived::ColsAtCompileTime, 0, Derived::MaxRowsAtCompileTime,
58  Derived::MaxColsAtCompileTime>>::type
59 DiscardGradient(const Eigen::MatrixBase<Derived>& auto_diff_matrix) {
60  return autoDiffToValueMatrix(auto_diff_matrix);
61 }
62 
63 /// @see DiscardGradient().
64 template <typename Derived>
65 typename std::enable_if<
67  const Eigen::MatrixBase<Derived>&>::type
68 DiscardGradient(const Eigen::MatrixBase<Derived>& matrix) {
69  return matrix;
70 }
71 
72 /// @see DiscardGradient().
73 template <typename _Scalar, int _Dim, int _Mode, int _Options>
74 typename std::enable_if<
76  Eigen::Transform<typename _Scalar::Scalar, _Dim, _Mode, _Options>>::type
77 DiscardGradient(const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>&
78  auto_diff_transform) {
79  return Eigen::Transform<typename _Scalar::Scalar, _Dim, _Mode, _Options>(
80  autoDiffToValueMatrix(auto_diff_transform.matrix()));
81 }
82 
83 /// @see DiscardGradient().
84 template <typename _Scalar, int _Dim, int _Mode, int _Options>
86  const Eigen::Transform<_Scalar, _Dim, _Mode,
87  _Options>&>::type
89  const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& transform) {
90  return transform;
91 }
92 
93 
94 /** \brief Initialize a single autodiff matrix given the corresponding value
95  *matrix.
96  *
97  * Set the values of \p auto_diff_matrix to be equal to \p val, and for each
98  *element i of \p auto_diff_matrix,
99  * resize the derivatives vector to \p num_derivatives, and set derivative
100  *number \p deriv_num_start + i to one (all other elements of the derivative
101  *vector set to zero).
102  *
103  * \param[in] mat 'regular' matrix of values
104  * \param[out] ret AutoDiff matrix
105  * \param[in] num_derivatives the size of the derivatives vector @default the
106  *size of mat
107  * \param[in] deriv_num_start starting index into derivative vector (i.e.
108  *element deriv_num_start in derivative vector corresponds to mat(0, 0)).
109  *@default 0
110  */
111 template <typename Derived, typename DerivedAutoDiff>
112 void initializeAutoDiff(const Eigen::MatrixBase<Derived>& val,
113  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
114  Eigen::MatrixBase<DerivedAutoDiff>& auto_diff_matrix,
115  Eigen::DenseIndex num_derivatives = Eigen::Dynamic,
116  Eigen::DenseIndex deriv_num_start = 0) {
117  using ADScalar = typename DerivedAutoDiff::Scalar;
118  static_assert(static_cast<int>(Derived::RowsAtCompileTime) ==
119  static_cast<int>(DerivedAutoDiff::RowsAtCompileTime),
120  "auto diff matrix has wrong number of rows at compile time");
121  static_assert(static_cast<int>(Derived::ColsAtCompileTime) ==
122  static_cast<int>(DerivedAutoDiff::ColsAtCompileTime),
123  "auto diff matrix has wrong number of columns at compile time");
124 
125  if (num_derivatives == Eigen::Dynamic) num_derivatives = val.size();
126 
127  auto_diff_matrix.resize(val.rows(), val.cols());
128  Eigen::DenseIndex deriv_num = deriv_num_start;
129  for (Eigen::DenseIndex i = 0; i < val.size(); i++) {
130  auto_diff_matrix(i) = ADScalar(val(i), num_derivatives, deriv_num++);
131  }
132 }
133 
134 /** \brief The appropriate AutoDiffScalar gradient type given the value type and
135  * the number of derivatives at compile time
136  */
137 template <typename Derived, int Nq>
138 using AutoDiffMatrixType = Eigen::Matrix<
139  Eigen::AutoDiffScalar<Eigen::Matrix<typename Derived::Scalar, Nq, 1>>,
140  Derived::RowsAtCompileTime, Derived::ColsAtCompileTime, 0,
141  Derived::MaxRowsAtCompileTime, Derived::MaxColsAtCompileTime>;
142 
143 /** \brief Initialize a single autodiff matrix given the corresponding value
144  *matrix.
145  *
146  * Create autodiff matrix that matches \p mat in size with derivatives of
147  *compile time size \p Nq and runtime size \p num_derivatives.
148  * Set its values to be equal to \p val, and for each element i of \p
149  *auto_diff_matrix, set derivative number \p deriv_num_start + i to one (all
150  *other derivatives set to zero).
151  *
152  * \param[in] mat 'regular' matrix of values
153  * \param[in] num_derivatives the size of the derivatives vector @default the
154  *size of mat
155  * \param[in] deriv_num_start starting index into derivative vector (i.e.
156  *element deriv_num_start in derivative vector corresponds to mat(0, 0)).
157  *@default 0
158  * \return AutoDiff matrix
159  */
160 template <int Nq = Eigen::Dynamic, typename Derived>
162  const Eigen::MatrixBase<Derived>& mat,
163  Eigen::DenseIndex num_derivatives = -1,
164  Eigen::DenseIndex deriv_num_start = 0) {
165  if (num_derivatives == -1) num_derivatives = mat.size();
166 
167  AutoDiffMatrixType<Derived, Nq> ret(mat.rows(), mat.cols());
168  initializeAutoDiff(mat, ret, num_derivatives, deriv_num_start);
169  return ret;
170 }
171 
172 namespace internal {
173 template <typename Derived, typename Scalar>
175  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
176  static void run(Eigen::MatrixBase<Derived>&, const Scalar&) {}
177 };
178 
179 template <typename Derived, typename DerivType>
181  Eigen::AutoDiffScalar<DerivType>> {
182  using Scalar = Eigen::AutoDiffScalar<DerivType>;
183  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
184  static void run(Eigen::MatrixBase<Derived>& mat, const Scalar& scalar) {
185  for (int i = 0; i < mat.size(); i++) {
186  auto& derivs = mat(i).derivatives();
187  if (derivs.size() == 0) {
188  derivs.resize(scalar.derivatives().size());
189  derivs.setZero();
190  }
191  }
192  }
193 };
194 } // namespace internal
195 
196 /** Resize derivatives vector of each element of a matrix to to match the size
197  * of the derivatives vector of a given scalar.
198  * \brief If the mat and scalar inputs are AutoDiffScalars, resize the
199  * derivatives vector of each element of the matrix mat to match
200  * the number of derivatives of the scalar. This is useful in functions that
201  * return matrices that do not depend on an AutoDiffScalar
202  * argument (e.g. a function with a constant output), while it is desired that
203  * information about the number of derivatives is preserved.
204  * \param mat matrix, for which the derivative vectors of the elements will be
205  * resized
206  * \param scalar scalar to match the derivative size vector against.
207  */
208 template <typename Derived>
209 // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
210 void resizeDerivativesToMatchScalar(Eigen::MatrixBase<Derived>& mat,
211  const typename Derived::Scalar& scalar) {
213  Derived, typename Derived::Scalar>::run(mat, scalar);
214 }
215 
216 namespace internal {
217 /** \brief Helper for totalSizeAtCompileTime function (recursive)
218  */
219 template <typename Head, typename... Tail>
221  static constexpr int eval() {
222  return Head::SizeAtCompileTime == Eigen::Dynamic ||
223  TotalSizeAtCompileTime<Tail...>::eval() == Eigen::Dynamic
224  ? Eigen::Dynamic
225  : Head::SizeAtCompileTime +
227  }
228 };
229 
230 /** \brief Helper for totalSizeAtCompileTime function (base case)
231  */
232 template <typename Head>
234  static constexpr int eval() { return Head::SizeAtCompileTime; }
235 };
236 
237 /** \brief Determine the total size at compile time of a number of arguments
238  * based on their SizeAtCompileTime static members
239  */
240 template <typename... Args>
241 constexpr int totalSizeAtCompileTime() {
243 }
244 
245 /** \brief Determine the total size at runtime of a number of arguments using
246  * their size() methods (base case).
247  */
248 constexpr Eigen::DenseIndex totalSizeAtRunTime() { return 0; }
249 
250 /** \brief Determine the total size at runtime of a number of arguments using
251  * their size() methods (recursive)
252  */
253 template <typename Head, typename... Tail>
254 Eigen::DenseIndex totalSizeAtRunTime(const Eigen::MatrixBase<Head>& head,
255  const Tail&... tail) {
256  return head.size() + totalSizeAtRunTime(tail...);
257 }
258 
259 /** \brief Helper for initializeAutoDiffTuple function (recursive)
260  */
261 template <size_t Index>
263  template <typename... ValueTypes, typename... AutoDiffTypes>
264  static void run(const std::tuple<ValueTypes...>& values,
265  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
266  std::tuple<AutoDiffTypes...>& auto_diffs,
267  Eigen::DenseIndex num_derivatives,
268  Eigen::DenseIndex deriv_num_start) {
269  constexpr size_t tuple_index = sizeof...(AutoDiffTypes)-Index;
270  const auto& value = std::get<tuple_index>(values);
271  auto& auto_diff = std::get<tuple_index>(auto_diffs);
272  auto_diff.resize(value.rows(), value.cols());
273  initializeAutoDiff(value, auto_diff, num_derivatives, deriv_num_start);
275  values, auto_diffs, num_derivatives, deriv_num_start + value.size());
276  }
277 };
278 
279 /** \brief Helper for initializeAutoDiffTuple function (base case)
280  */
281 template <>
283  template <typename... ValueTypes, typename... AutoDiffTypes>
284  static void run(const std::tuple<ValueTypes...>& values,
285  const std::tuple<AutoDiffTypes...>& auto_diffs,
286  Eigen::DenseIndex num_derivatives,
287  Eigen::DenseIndex deriv_num_start) {
288  unused(values, auto_diffs, num_derivatives, deriv_num_start);
289  }
290 };
291 } // namespace internal
292 
293 /** \brief Given a series of Eigen matrices, create a tuple of corresponding
294  *AutoDiff matrices with values equal to the input matrices and properly
295  *initialized derivative vectors.
296  *
297  * The size of the derivative vector of each element of the matrices in the
298  *output tuple will be the same, and will equal the sum of the number of
299  *elements of the matrices in \p args.
300  * If all of the matrices in \p args have fixed size, then the derivative
301  *vectors will also have fixed size (being the sum of the sizes at compile time
302  *of all of the input arguments),
303  * otherwise the derivative vectors will have dynamic size.
304  * The 0th element of the derivative vectors will correspond to the derivative
305  *with respect to the 0th element of the first argument.
306  * Subsequent derivative vector elements correspond first to subsequent elements
307  *of the first input argument (traversed first by row, then by column), and so
308  *on for subsequent arguments.
309  *
310  * \param args a series of Eigen matrices
311  * \return a tuple of properly initialized AutoDiff matrices corresponding to \p
312  *args
313  *
314  */
315 template <typename... Deriveds>
316 std::tuple<AutoDiffMatrixType<
317  Deriveds, internal::totalSizeAtCompileTime<Deriveds...>()>...>
318 initializeAutoDiffTuple(const Eigen::MatrixBase<Deriveds>&... args) {
319  Eigen::DenseIndex dynamic_num_derivs = internal::totalSizeAtRunTime(args...);
320  std::tuple<AutoDiffMatrixType<
321  Deriveds, internal::totalSizeAtCompileTime<Deriveds...>()>...>
322  ret(AutoDiffMatrixType<Deriveds,
323  internal::totalSizeAtCompileTime<Deriveds...>()>(
324  args.rows(), args.cols())...);
325  auto values = std::forward_as_tuple(args...);
326  internal::InitializeAutoDiffTupleHelper<sizeof...(args)>::run(
327  values, ret, dynamic_num_derivs, 0);
328  return ret;
329 }
330 
331 } // namespace math
332 } // namespace drake
static constexpr int eval()
Definition: autodiff.h:221
AutoDiffToValueMatrix< Derived >::type autoDiffToValueMatrix(const Eigen::MatrixBase< Derived > &auto_diff_matrix)
Definition: autodiff.h:27
int i
Definition: reset_after_move_test.cc:51
MixedIntegerRotationConstraintGenerator::ReturnType ret
Definition: mixed_integer_rotation_constraint_test.cc:188
double value
Definition: wrap_test_util_py.cc:12
Definition: bullet_model.cc:22
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:284
Definition: autodiff.h:20
symbolic::Monomial transform(const symbolic::Monomial &monomial, const map< Variable::Id, Variable > &var_id_to_var)
Definition: mathematical_program_test.cc:2489
Definition: autodiff_overloads.h:34
static constexpr int eval()
Definition: autodiff.h:234
Eigen::Matrix< typename Derived::Scalar::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime > type
Definition: autodiff.h:23
constexpr Eigen::DenseIndex totalSizeAtRunTime()
Determine the total size at runtime of a number of arguments using their size() methods (base case)...
Definition: autodiff.h:248
constexpr int totalSizeAtCompileTime()
Determine the total size at compile time of a number of arguments based on their SizeAtCompileTime st...
Definition: autodiff.h:241
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:318
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:254
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:141
Helper for totalSizeAtCompileTime function (recursive)
Definition: autodiff.h:220
Helper for initializeAutoDiffTuple function (recursive)
Definition: autodiff.h:262
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)
B = DiscardGradient(A) enables casting from a matrix of AutoDiffScalars to AutoDiffScalar::Scalar typ...
Definition: autodiff.h:59
static void run(Eigen::MatrixBase< Derived > &, const Scalar &)
Definition: autodiff.h:176
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:210
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:264
static void run(Eigen::MatrixBase< Derived > &mat, const Scalar &scalar)
Definition: autodiff.h:184
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:112
void unused(const Args &...)
Documents the argument(s) as unused, placating GCC&#39;s -Wunused-parameter warning.
Definition: unused.h:51