cppad-basic.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2019 CNRS INRIA
3 //
4 
5 #include "pinocchio/autodiff/cppad.hpp"
6 #include <cppad/speed/det_by_minor.hpp>
7 
8 #include <boost/variant.hpp> // to avoid C99 warnings
9 
10 #include <iostream>
11 
12 #include <boost/test/unit_test.hpp>
13 #include <boost/utility/binary.hpp>
14 
15 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
16 
17  BOOST_AUTO_TEST_CASE(test_example1_cppad)
18  {
19  using CppAD::AD;
20  using CppAD::NearEqual;
21  using Eigen::Matrix;
22  using Eigen::Dynamic;
23  //
24  typedef Matrix< AD<double> , Dynamic, 1 > eigen_vector;
25  //
26  // some temporary indices
27  size_t i, j;
28 
29  // domain and range space vectors
30  size_t n = 10, m = n;
31  eigen_vector a_x(n), a_y(m);
32 
33  // set and declare independent variables and start tape recording
34  for(j = 0; j < n; j++)
35  {
36  a_x[(Eigen::DenseIndex)j] = double(1 + j);
37  }
38  CppAD::Independent(a_x);
39 
40  // evaluate a component wise function
41  a_y = a_x.array() + a_x.array().sin();
42 
43  // create f: x -> y and stop tape recording
44  CppAD::ADFun<double> f(a_x, a_y);
45 
46  // compute the derivative of y w.r.t x using CppAD
47  CPPAD_TESTVECTOR(double) x(n);
48  for(j = 0; j < n; j++)
49  {
50  x[j] = double(j) + 1.0 / double(j+1);
51  }
52  CPPAD_TESTVECTOR(double) jac = f.Jacobian(x);
53 
54  // check Jacobian
55  double eps = 100. * CppAD::numeric_limits<double>::epsilon();
56  for(i = 0; i < m; i++)
57  {
58  for(j = 0; j < n; j++)
59  {
60  double check = 1.0 + cos(x[i]);
61  if( i != j ) check = 0.0;
62  BOOST_CHECK(NearEqual(jac[i * n + j], check, eps, eps));
63  }
64  }
65  }
66 
67 
68  BOOST_AUTO_TEST_CASE(test_example2_cppad)
69  {
70  using CppAD::AD;
71  using CppAD::NearEqual;
72  using Eigen::Matrix;
73  using Eigen::Dynamic;
74  //
75  typedef Matrix< double , Dynamic, Dynamic > eigen_matrix;
76  typedef Matrix< AD<double> , Dynamic, Dynamic > eigen_ad_matrix;
77  //
78  typedef Matrix< double , Dynamic , 1> eigen_vector;
79  typedef Matrix< AD<double> , Dynamic , 1> eigen_ad_vector;
80  // some temporary indices
81  size_t i, j;
82 
83  // domain and range space vectors
84  size_t size = 3, n = size * size, m = 1;
85  eigen_ad_vector a_x(n), a_y(m);
86  eigen_vector x(n);
87 
88  // set and declare independent variables and start tape recording
89  for(i = 0; i < size; i++)
90  {
91  for(j = 0; j < size; j++)
92  { // lower triangular matrix
93  a_x[(Eigen::DenseIndex)(i * size + j)] = x[(Eigen::DenseIndex)(i * size + j)] = 0.0;
94  if( j <= i )
95  a_x[(Eigen::DenseIndex)(i * size + j)] = x[(Eigen::DenseIndex)(i * size + j)] = double(1 + i + j);
96  }
97  }
98  CppAD::Independent(a_x);
99 
100  // copy independent variable vector to a matrix
101  eigen_ad_matrix a_X(size, size);
102  eigen_matrix X(size, size);
103  for(i = 0; i < size; i++)
104  {
105  for(j = 0; j < size; j++)
106  {
107  X((Eigen::DenseIndex)i, (Eigen::DenseIndex)j) = x[(Eigen::DenseIndex)(i * size + j)];
108  // If we used a_X(i, j) = X(i, j), a_X would not depend on a_x.
109  a_X((Eigen::DenseIndex)i, (Eigen::DenseIndex)j) = a_x[(Eigen::DenseIndex)(i * size + j)];
110  }
111  }
112 
113  // Compute the log of determinant of X
114  a_y[0] = log( a_X.determinant() );
115 
116  // create f: x -> y and stop tape recording
117  CppAD::ADFun<double> f(a_x, a_y);
118 
119  // check function value
120  double eps = 100. * CppAD::numeric_limits<double>::epsilon();
121  CppAD::det_by_minor<double> det(size);
122  BOOST_CHECK(NearEqual(Value(a_y[0]) , log(det(x)), eps, eps));
123 
124  // compute the derivative of y w.r.t x using CppAD
125  eigen_vector jac = f.Jacobian(x);
126 
127  // check the derivative using the formula
128  // d/dX log(det(X)) = transpose( inv(X) )
129  eigen_matrix inv_X = X.inverse();
130  for(i = 0; i < size; i++)
131  {
132  for(j = 0; j < size; j++)
133  BOOST_CHECK(NearEqual(jac[(Eigen::DenseIndex)(i * size + j)],
134  inv_X((Eigen::DenseIndex)j, (Eigen::DenseIndex)i),
135  eps,
136  eps));
137  }
138  }
139 
140  BOOST_AUTO_TEST_CASE(test_sincos)
141  {
142  using CppAD::AD;
143  using CppAD::NearEqual;
144  double eps99 = 99.0 * std::numeric_limits<double>::epsilon();
145 
146  typedef AD<double> AD_double;
147 
148  double x0 = 1.;
149  CPPAD_TESTVECTOR(AD_double) x(1), y(1), z(1);
150  x[0] = x0;
151  CppAD::Independent(x);
152 
153  y[0] = CppAD::cos(x[0]);
154  BOOST_CHECK(NearEqual(y[0],std::cos(x0),eps99,eps99));
155  CppAD::ADFun<double> fcos(x, y);
156 
157  CPPAD_TESTVECTOR(double) x_eval(1);
158  x_eval[0] = x0;
159  CPPAD_TESTVECTOR(double) dy(1);
160  dy = fcos.Jacobian(x_eval);
161  BOOST_CHECK(NearEqual(dy[0],-std::sin(x0),eps99,eps99));
162 
163  CppAD::Independent(x);
164  z[0] = CppAD::sin(x[0]);
165  BOOST_CHECK(NearEqual(z[0],std::sin(x0),eps99,eps99));
166 
167  CppAD::ADFun<double> fsin(x, z);
168 
169  CPPAD_TESTVECTOR(double) dz(1);
170  dz = fsin.Jacobian(x_eval);
171  BOOST_CHECK(NearEqual(dz[0],std::cos(x0),eps99,eps99));
172  }
173 
174  BOOST_AUTO_TEST_CASE(test_eigen_min)
175  {
176  using CppAD::AD;
177 
178  typedef double Scalar;
179  typedef AD<double> ADScalar;
180  Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ad_X;
181  Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ad_Y;
182  ad_X.resize(2);
183  ad_Y.resize(2);
184 
185  Eigen::Vector2d x_test(-1,1);
186  Eigen::Vector2d y_test = x_test.array().min(Scalar(0.));
187 
188  CppAD::Independent(ad_X);
189  //Function
190  ad_Y = ad_X.array().min(Scalar(0.));
191  CppAD::ADFun<Scalar> ad_fun(ad_X,ad_Y);
192 
193  CPPAD_TESTVECTOR(Scalar) x((size_t)2);
194  Eigen::Map<Eigen::Vector2d>(x.data(),2,1) = x_test;
195 
196  CPPAD_TESTVECTOR(Scalar) y = ad_fun.Forward(0,x);
197 
198  BOOST_CHECK(Eigen::Map<Eigen::Vector2d>(y.data(),2,1).isApprox(y_test));
199  }
200 
201  BOOST_AUTO_TEST_CASE(test_eigen_max)
202  {
203  using CppAD::AD;
204 
205  typedef double Scalar;
206  typedef AD<double> ADScalar;
207  Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ad_X;
208  Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ad_Y;
209  ad_X.resize(2);
210  ad_Y.resize(2);
211 
212  Eigen::Vector2d x_test(-1,1);
213  Eigen::Vector2d y_test = x_test.array().max(Scalar(0.));
214 
215  CppAD::Independent(ad_X);
216  //Function
217  ad_Y = ad_X.array().max(Scalar(0.));
218  CppAD::ADFun<Scalar> ad_fun(ad_X,ad_Y);
219 
220  CPPAD_TESTVECTOR(Scalar) x((size_t)2);
221  Eigen::Map<Eigen::Vector2d>(x.data(),2,1) = x_test;
222 
223  CPPAD_TESTVECTOR(Scalar) y = ad_fun.Forward(0,x);
224 
225  BOOST_CHECK(Eigen::Map<Eigen::Vector2d>(y.data(),2,1).isApprox(y_test));
226  }
227 
228 
229  BOOST_AUTO_TEST_CASE(test_eigen_support)
230  {
231  using namespace CppAD;
232 
233  // use a special object for source code generation
234  typedef AD<double> ADScalar;
235 
236  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> ADVector;
237 
238  ADVector vec_zero(ADVector::Zero(100));
239  BOOST_CHECK(vec_zero.isZero());
240 
241  ADVector vec_ones(100);
242  vec_ones.fill(1);
243  BOOST_CHECK(vec_ones.isOnes());
244 
245  }
246 
247  BOOST_AUTO_TEST_CASE(test_abs)
248  {
249  CppAD::AD<double> ad_value;
250  ad_value = -1.;
251  abs(ad_value);
252  }
253 
254 BOOST_AUTO_TEST_CASE(test_atan2)
255 {
256  CppAD::AD<double> theta,x,y;
257  x = pinocchio::math::cos(theta); y = pinocchio::math::sin(theta);
258 
259  pinocchio::math::atan2(y,x);
260 
261 }
262 
263 BOOST_AUTO_TEST_SUITE_END()
n
Vec3f n
y
y
check
bool check(const T &value, const T &other)
dcrba.eps
int eps
Definition: dcrba.py:384
pinocchio::python::Scalar
SE3::Scalar Scalar
Definition: conversions.cpp:13
isApprox
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
x
x
m
float m
size
FCL_REAL size() const
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_example1_cppad)
Definition: cppad-basic.cpp:17
ocp.x0
x0
Definition: ocp.py:16

pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:29