Drake
autodiffxd.h
Go to the documentation of this file.
1 #pragma once
2 
3 // This file is a modification of Eigen-3.3.3's AutoDiffScalar.h file which is
4 // available at
5 // https://bitbucket.org/eigen/eigen/raw/67e894c6cd8f5f1f604b27d37ed47fdf012674ff/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
6 //
7 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
8 // Copyright (C) 2017 Drake Authors
9 //
10 // This Source Code Form is subject to the terms of the Mozilla
11 // Public License v. 2.0. If a copy of the MPL was not distributed
12 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
13 
14 #ifndef DRAKE_COMMON_AUTODIFF_HEADER
15 // TODO(soonho-tri): Change to #error.
16 #warning Do not directly include this file. Include "drake/common/autodiff.h".
17 #endif
18 
19 #include <cmath>
20 #include <ostream>
21 
22 #include <Eigen/Dense>
23 
24 namespace Eigen {
25 
26 #if !defined(DRAKE_DOXYGEN_CXX)
27 // Explicit template specializations of Eigen::AutoDiffScalar for VectorXd.
28 //
29 // AutoDiffScalar tries to call internal::make_coherent to promote empty
30 // derivatives. However, it fails to do the promotion when an operand is an
31 // expression tree (i.e. CwiseBinaryOp). Our solution is to provide special
32 // overloading for VectorXd and change the return types of its operators. With
33 // this change, the operators evaluate terms immediately and return an
34 // AutoDiffScalar<VectorXd> instead of expression trees (such as CwiseBinaryOp).
35 // This ensures internal::make_coherent does the correct operation (resizing and
36 // setting-to-zero) when needed.
37 //
38 // We also provide overloading of math functions for AutoDiffScalar<VectorXd>
39 // which return AutoDiffScalar<VectorXd> instead of an expression tree.
40 //
41 // See https://github.com/RobotLocomotion/drake/issues/6944 for more
42 // information. See also drake/common/autodiff_overloads.h.
43 //
44 // TODO(soonho-tri): Next time when we upgrade Eigen, please check if we still
45 // need these specializations.
46 template <>
47 class AutoDiffScalar<VectorXd>
48  : public internal::auto_diff_special_op<VectorXd, false> {
49  public:
50  typedef internal::auto_diff_special_op<VectorXd, false> Base;
51  typedef typename internal::remove_all<VectorXd>::type DerType;
52  typedef typename internal::traits<DerType>::Scalar Scalar;
53  typedef typename NumTraits<Scalar>::Real Real;
54 
55  using Base::operator+;
56  using Base::operator*;
57 
58  AutoDiffScalar() {}
59 
60  AutoDiffScalar(const Scalar& value, int nbDer, int derNumber)
61  : m_value(value), m_derivatives(DerType::Zero(nbDer)) {
62  m_derivatives.coeffRef(derNumber) = Scalar(1);
63  }
64 
65  // NOLINTNEXTLINE(runtime/explicit): Code from Eigen.
66  AutoDiffScalar(const Real& value) : m_value(value) {
67  if (m_derivatives.size() > 0) m_derivatives.setZero();
68  }
69 
70  AutoDiffScalar(const Scalar& value, const DerType& der)
71  : m_value(value), m_derivatives(der) {}
72 
73  template <typename OtherDerType>
74  AutoDiffScalar(
75  const AutoDiffScalar<OtherDerType>& other
76 #ifndef EIGEN_PARSED_BY_DOXYGEN
77  ,
78  typename internal::enable_if<
79  internal::is_same<
80  Scalar, typename internal::traits<typename internal::remove_all<
81  OtherDerType>::type>::Scalar>::value,
82  void*>::type = 0
83 #endif
84  )
85  : m_value(other.value()), m_derivatives(other.derivatives()) {
86  }
87 
88  friend std::ostream& operator<<(std::ostream& s, const AutoDiffScalar& a) {
89  return s << a.value();
90  }
91 
92  AutoDiffScalar(const AutoDiffScalar& other)
93  : m_value(other.value()), m_derivatives(other.derivatives()) {}
94 
95  template <typename OtherDerType>
96  inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& other) {
97  m_value = other.value();
98  m_derivatives = other.derivatives();
99  return *this;
100  }
101 
102  inline AutoDiffScalar& operator=(const AutoDiffScalar& other) {
103  m_value = other.value();
104  m_derivatives = other.derivatives();
105  return *this;
106  }
107 
108  inline AutoDiffScalar& operator=(const Scalar& other) {
109  m_value = other;
110  if (m_derivatives.size() > 0) m_derivatives.setZero();
111  return *this;
112  }
113 
114  inline const Scalar& value() const { return m_value; }
115  inline Scalar& value() { return m_value; }
116 
117  inline const DerType& derivatives() const { return m_derivatives; }
118  inline DerType& derivatives() { return m_derivatives; }
119 
120  inline bool operator<(const Scalar& other) const { return m_value < other; }
121  inline bool operator<=(const Scalar& other) const { return m_value <= other; }
122  inline bool operator>(const Scalar& other) const { return m_value > other; }
123  inline bool operator>=(const Scalar& other) const { return m_value >= other; }
124  inline bool operator==(const Scalar& other) const { return m_value == other; }
125  inline bool operator!=(const Scalar& other) const { return m_value != other; }
126 
127  friend inline bool operator<(const Scalar& a, const AutoDiffScalar& b) {
128  return a < b.value();
129  }
130  friend inline bool operator<=(const Scalar& a, const AutoDiffScalar& b) {
131  return a <= b.value();
132  }
133  friend inline bool operator>(const Scalar& a, const AutoDiffScalar& b) {
134  return a > b.value();
135  }
136  friend inline bool operator>=(const Scalar& a, const AutoDiffScalar& b) {
137  return a >= b.value();
138  }
139  friend inline bool operator==(const Scalar& a, const AutoDiffScalar& b) {
140  return a == b.value();
141  }
142  friend inline bool operator!=(const Scalar& a, const AutoDiffScalar& b) {
143  return a != b.value();
144  }
145 
146  template <typename OtherDerType>
147  inline bool operator<(const AutoDiffScalar<OtherDerType>& b) const {
148  return m_value < b.value();
149  }
150  template <typename OtherDerType>
151  inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const {
152  return m_value <= b.value();
153  }
154  template <typename OtherDerType>
155  inline bool operator>(const AutoDiffScalar<OtherDerType>& b) const {
156  return m_value > b.value();
157  }
158  template <typename OtherDerType>
159  inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const {
160  return m_value >= b.value();
161  }
162  template <typename OtherDerType>
163  inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const {
164  return m_value == b.value();
165  }
166  template <typename OtherDerType>
167  inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const {
168  return m_value != b.value();
169  }
170 
171  inline const AutoDiffScalar<DerType> operator+(const Scalar& other) const {
172  return AutoDiffScalar<DerType>(m_value + other, m_derivatives);
173  }
174 
175  friend inline const AutoDiffScalar<DerType> operator+(
176  const Scalar& a, const AutoDiffScalar& b) {
177  return AutoDiffScalar<DerType>(a + b.value(), b.derivatives());
178  }
179 
180  inline AutoDiffScalar& operator+=(const Scalar& other) {
181  value() += other;
182  return *this;
183  }
184 
185  template <typename OtherDerType>
186  inline const AutoDiffScalar<DerType> operator+(
187  const AutoDiffScalar<OtherDerType>& other) const {
188  internal::make_coherent(m_derivatives, other.derivatives());
189  return AutoDiffScalar<DerType>(m_value + other.value(),
190  m_derivatives + other.derivatives());
191  }
192 
193  template <typename OtherDerType>
194  inline AutoDiffScalar& operator+=(const AutoDiffScalar<OtherDerType>& other) {
195  (*this) = (*this) + other;
196  return *this;
197  }
198 
199  inline const AutoDiffScalar<DerType> operator-(const Scalar& b) const {
200  return AutoDiffScalar<DerType>(m_value - b, m_derivatives);
201  }
202 
203  friend inline const AutoDiffScalar<DerType> operator-(
204  const Scalar& a, const AutoDiffScalar& b) {
205  return AutoDiffScalar<DerType>(a - b.value(), -b.derivatives());
206  }
207 
208  inline AutoDiffScalar& operator-=(const Scalar& other) {
209  value() -= other;
210  return *this;
211  }
212 
213  template <typename OtherDerType>
214  inline const AutoDiffScalar<DerType> operator-(
215  const AutoDiffScalar<OtherDerType>& other) const {
216  internal::make_coherent(m_derivatives, other.derivatives());
217  return AutoDiffScalar<DerType>(m_value - other.value(),
218  m_derivatives - other.derivatives());
219  }
220 
221  template <typename OtherDerType>
222  inline AutoDiffScalar& operator-=(const AutoDiffScalar<OtherDerType>& other) {
223  *this = *this - other;
224  return *this;
225  }
226 
227  inline const AutoDiffScalar<DerType> operator-() const {
228  return AutoDiffScalar<DerType>(-m_value, -m_derivatives);
229  }
230 
231  inline const AutoDiffScalar<DerType> operator*(const Scalar& other) const {
232  return MakeAutoDiffScalar(m_value * other, m_derivatives * other);
233  }
234 
235  friend inline const AutoDiffScalar<DerType> operator*(
236  const Scalar& other, const AutoDiffScalar& a) {
237  return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other);
238  }
239 
240  inline const AutoDiffScalar<DerType> operator/(const Scalar& other) const {
241  return MakeAutoDiffScalar(m_value / other,
242  (m_derivatives * (Scalar(1) / other)));
243  }
244 
245  friend inline const AutoDiffScalar<DerType> operator/(
246  const Scalar& other, const AutoDiffScalar& a) {
247  return MakeAutoDiffScalar(
248  other / a.value(),
249  a.derivatives() * (Scalar(-other) / (a.value() * a.value())));
250  }
251 
252  template <typename OtherDerType>
253  inline const AutoDiffScalar<DerType> operator/(
254  const AutoDiffScalar<OtherDerType>& other) const {
255  internal::make_coherent(m_derivatives, other.derivatives());
256  return MakeAutoDiffScalar(
257  m_value / other.value(),
258  ((m_derivatives * other.value()) - (other.derivatives() * m_value)) *
259  (Scalar(1) / (other.value() * other.value())));
260  }
261 
262  template <typename OtherDerType>
263  inline const AutoDiffScalar<DerType> operator*(
264  const AutoDiffScalar<OtherDerType>& other) const {
265  internal::make_coherent(m_derivatives, other.derivatives());
266  return MakeAutoDiffScalar(
267  m_value * other.value(),
268  (m_derivatives * other.value()) + (other.derivatives() * m_value));
269  }
270 
271  inline AutoDiffScalar& operator*=(const Scalar& other) {
272  *this = *this * other;
273  return *this;
274  }
275 
276  template <typename OtherDerType>
277  inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other) {
278  *this = *this * other;
279  return *this;
280  }
281 
282  inline AutoDiffScalar& operator/=(const Scalar& other) {
283  *this = *this / other;
284  return *this;
285  }
286 
287  template <typename OtherDerType>
288  inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& other) {
289  *this = *this / other;
290  return *this;
291  }
292 
293  protected:
294  Scalar m_value;
295  DerType m_derivatives;
296 };
297 
298 #define DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(FUNC, CODE) \
299  inline const AutoDiffScalar<VectorXd> FUNC( \
300  const AutoDiffScalar<VectorXd>& x) { \
301  EIGEN_UNUSED typedef double Scalar; \
302  CODE; \
303  }
304 
305 DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
306  abs, using std::abs; return Eigen::MakeAutoDiffScalar(
307  abs(x.value()), x.derivatives() * (x.value() < 0 ? -1 : 1));)
308 
309 DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
310  abs2, using numext::abs2; return Eigen::MakeAutoDiffScalar(
311  abs2(x.value()), x.derivatives() * (Scalar(2) * x.value()));)
312 
313 DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
314  sqrt, using std::sqrt; Scalar sqrtx = sqrt(x.value());
315  return Eigen::MakeAutoDiffScalar(sqrtx,
316  x.derivatives() * (Scalar(0.5) / sqrtx));)
317 
318 DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
319  cos, using std::cos; using std::sin;
320  return Eigen::MakeAutoDiffScalar(cos(x.value()),
321  x.derivatives() * (-sin(x.value())));)
322 
323 DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
324  sin, using std::sin; using std::cos;
325  return Eigen::MakeAutoDiffScalar(sin(x.value()),
326  x.derivatives() * cos(x.value()));)
327 
328 DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
329  exp, using std::exp; Scalar expx = exp(x.value());
330  return Eigen::MakeAutoDiffScalar(expx, x.derivatives() * expx);)
331 
332 DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
333  log, using std::log; return Eigen::MakeAutoDiffScalar(
334  log(x.value()), x.derivatives() * (Scalar(1) / x.value()));)
335 
336 DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
337  tan, using std::tan; using std::cos; return Eigen::MakeAutoDiffScalar(
338  tan(x.value()),
339  x.derivatives() * (Scalar(1) / numext::abs2(cos(x.value()))));)
340 
341 DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
342  asin, using std::sqrt; using std::asin; return Eigen::MakeAutoDiffScalar(
343  asin(x.value()),
344  x.derivatives() * (Scalar(1) / sqrt(1 - numext::abs2(x.value()))));)
345 
346 DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
347  acos, using std::sqrt; using std::acos; return Eigen::MakeAutoDiffScalar(
348  acos(x.value()),
349  x.derivatives() * (Scalar(-1) / sqrt(1 - numext::abs2(x.value()))));)
350 
351 DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
352  tanh, using std::cosh; using std::tanh; return Eigen::MakeAutoDiffScalar(
353  tanh(x.value()),
354  x.derivatives() * (Scalar(1) / numext::abs2(cosh(x.value()))));)
355 
356 DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
357  sinh, using std::sinh; using std::cosh;
358  return Eigen::MakeAutoDiffScalar(sinh(x.value()),
359  x.derivatives() * cosh(x.value()));)
360 
361 DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
362  cosh, using std::sinh; using std::cosh;
363  return Eigen::MakeAutoDiffScalar(cosh(x.value()),
364  x.derivatives() * sinh(x.value()));)
365 
366 #undef DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY
367 
368 // We have this specialization here because the Eigen-3.3.3's atan2
369 // implementation for AutoDiffScalar does not call `make_coherent` function.
370 inline const AutoDiffScalar<VectorXd> atan2(const AutoDiffScalar<VectorXd>& a,
371  const AutoDiffScalar<VectorXd>& b) {
372  using std::atan2;
373  typedef double Scalar;
374  typedef AutoDiffScalar<Matrix<Scalar, Dynamic, 1>> PlainADS;
375 
376  internal::make_coherent(a.derivatives(), b.derivatives());
377 
378  PlainADS ret;
379  ret.value() = atan2(a.value(), b.value());
380 
381  Scalar squared_hypot = a.value() * a.value() + b.value() * b.value();
382 
383  // if (squared_hypot==0) the derivation is undefined and the following results
384  // in a NaN:
385  ret.derivatives() =
386  (a.derivatives() * b.value() - a.value() * b.derivatives()) /
387  squared_hypot;
388 
389  return ret;
390 }
391 
392 // We have this specialization here because the Eigen-3.3.3's implementation of
393 // min for AutoDiffScalar does not call `make_coherent` function.
394 //
395 // For example, `min(x, y) + x` gives a runtime error if x.value() < y.value()
396 // but x's derivatives are not properly initialized while y's ones are.
397 inline AutoDiffScalar<VectorXd> min(const AutoDiffScalar<VectorXd>& x,
398  const AutoDiffScalar<VectorXd>& y) {
399  internal::make_coherent(x.derivatives(), y.derivatives());
400  return (x.value() < y.value() ? x : y);
401 }
402 
403 // We have this specialization here because the Eigen-3.3.3's implementation of
404 // max for AutoDiffScalar does not call `make_coherent` function.
405 //
406 // For example, `max(x, y) + x` gives a runtime error if x.value() > y.value()
407 // but x's derivatives are not properly initialized while y's ones are.
408 // NOLINTNEXTLINE(build/include_what_you_use): Suppress false alarm.
409 inline AutoDiffScalar<VectorXd> max(const AutoDiffScalar<VectorXd>& x,
410  const AutoDiffScalar<VectorXd>& y) {
411  internal::make_coherent(x.derivatives(), y.derivatives());
412  return (x.value() >= y.value() ? x : y);
413 }
414 
415 inline const AutoDiffScalar<VectorXd> pow(const AutoDiffScalar<VectorXd>& a,
416  double b) {
417  using std::pow;
418  return MakeAutoDiffScalar(pow(a.value(), b),
419  a.derivatives() * (b * pow(a.value(), b - 1)));
420 }
421 
422 #endif
423 
424 } // namespace Eigen
Expression operator*(Expression lhs, const Expression &rhs)
Definition: symbolic_expression.cc:321
int y
Definition: rgbd_renderer_test.cc:33
bool operator!=(const GeoPositionT< T > &lhs, const GeoPositionT< T > &rhs)
GeoPosition overload for the inequality operator.
Definition: lane_data.h:193
Expression acos(const Expression &e)
Definition: symbolic_expression.cc:634
std::vector< Number > x
Definition: ipopt_solver.cc:150
bool operator<(ExpressionKind k1, ExpressionKind k2)
Total ordering between ExpressionKinds.
Definition: symbolic_expression.cc:37
Expression sin(const Expression &e)
Definition: symbolic_expression.cc:599
bool operator==(const GeoPositionT< T > &lhs, const GeoPositionT< T > &rhs)
GeoPosition overload for the equality operator.
Definition: lane_data.h:187
Expression abs(const Expression &e)
Definition: symbolic_expression.cc:537
Expression & operator+=(Expression &lhs, const Expression &rhs)
Definition: symbolic_expression.cc:212
Expression asin(const Expression &e)
Definition: symbolic_expression.cc:624
Expression cosh(const Expression &e)
Definition: symbolic_expression.cc:669
Expression atan2(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:652
Expression operator/(Expression lhs, const Expression &rhs)
Definition: symbolic_expression.cc:468
Expression operator+(Expression lhs, const Expression &rhs)
Definition: symbolic_expression.cc:206
Definition: autodiff_overloads.h:34
int b
Definition: rgbd_camera.cc:91
Expression sinh(const Expression &e)
Definition: symbolic_expression.cc:661
int value
Definition: copyable_unique_ptr_test.cc:61
Expression exp(const Expression &e)
Definition: symbolic_expression.cc:545
Formula operator>(const Expression &e1, const Expression &e2)
Definition: symbolic_formula.cc:266
Expression max(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:697
std::ostream & operator<<(std::ostream &out, const LaneEnd::Which &which_end)
Streams a string representation of which_end into out.
Definition: lane_data.cc:11
Expression min(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:685
Expression tanh(const Expression &e)
Definition: symbolic_expression.cc:677
Expression & operator-=(Expression &lhs, const Expression &rhs)
Definition: symbolic_expression.cc:270
Expression tan(const Expression &e)
Definition: symbolic_expression.cc:616
Formula operator>=(const Expression &e1, const Expression &e2)
Definition: symbolic_formula.cc:275
Expression cos(const Expression &e)
Definition: symbolic_expression.cc:607
Expression sqrt(const Expression &e)
Definition: symbolic_expression.cc:553
Expression operator-(Expression lhs, const Expression &rhs)
Definition: symbolic_expression.cc:264
Polynomial< CoefficientType > pow(const Polynomial< CoefficientType > &base, typename Polynomial< CoefficientType >::PowerType exponent)
Provides power function for Polynomial.
Definition: polynomial.h:449
Eigen::AutoDiffScalar< typename internal::remove_all< DerTypeA >::type::PlainObject > pow(const Eigen::AutoDiffScalar< DerTypeA > &base, const Eigen::AutoDiffScalar< DerTypeB > &exponent)
Overloads pow for an AutoDiffScalar base and exponent, implementing the chain rule.
Definition: autodiff_overloads.h:99
Formula operator<=(const Expression &e1, const Expression &e2)
Definition: symbolic_formula.cc:257
Expression & operator*=(Expression &lhs, const Expression &rhs)
Definition: symbolic_expression.cc:327
Expression log(const Expression &e)
Definition: symbolic_expression.cc:527
Expression & operator/=(Expression &lhs, const Expression &rhs)
Definition: symbolic_expression.cc:474