casadi/basic.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4 
6 
7 #include <boost/variant.hpp> // to avoid C99 warnings
8 
9 #include <iostream>
10 #include <boost/test/unit_test.hpp>
11 #include <boost/utility/binary.hpp>
12 
13 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
14 
16 {
17  casadi::SX a(1, 1);
18  casadi::SX b;
19 
20  casadi::MX c;
21 
22  b = a(0, 0);
23 }
24 
26 {
27  Eigen::Matrix<casadi::SX, 3, 3> A, B;
28  Eigen::Matrix<casadi::SX, 3, 1> a, b;
29  Eigen::Matrix<casadi::SX, 3, 1> c = A * a - B.transpose() * b;
30 }
31 
32 // A function working with Eigen::Matrix'es parameterized by the Scalar type
33 template<typename Scalar, typename T1, typename T2, typename T3, typename T4>
34 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> eigenFun(
35  Eigen::MatrixBase<T1> const & A,
36  Eigen::MatrixBase<T2> const & a,
37  Eigen::MatrixBase<T3> const & B,
38  Eigen::MatrixBase<T4> const & b)
39 {
40  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> c(4);
41  c.segment(1, 3) = A * a.segment(1, 3) - B.transpose() * b;
42  c[0] = 0.;
43 
44  return c;
45 }
46 
47 BOOST_AUTO_TEST_CASE(test_example)
48 {
49  // Declare casadi symbolic matrix arguments
50  casadi::SX cs_a = casadi::SX::sym("a", 4);
51  casadi::SX cs_b = casadi::SX::sym("b", 3);
52 
53  // Declare Eigen matrices
54  Eigen::Matrix<casadi::SX, 3, 3> A, B;
55  Eigen::Matrix<casadi::SX, -1, 1> a(4), c(4);
56  Eigen::Matrix<casadi::SX, 3, 1> b;
57 
58  // Let A, B be some numeric matrices
59  for (Eigen::Index i = 0; i < A.rows(); ++i)
60  {
61  for (Eigen::Index j = 0; j < A.cols(); ++j)
62  {
63  A(i, j) = 10. * static_cast<double>(i) + static_cast<double>(j);
64  B(i, j) = -10. * static_cast<double>(i) - static_cast<double>(j);
65  }
66  }
67 
68  // Let a, b be symbolic arguments of a function
71 
72  // Call the function taking Eigen matrices
73  c = eigenFun<casadi::SX>(A, a, B, b);
74 
75  // Copy the result from Eigen matrices to casadi matrix
76  casadi::SX cs_c = casadi::SX(casadi::Sparsity::dense(c.rows(), 1));
78 
79  // Display the resulting casadi matrix
80  std::cout << "c = " << cs_c << std::endl;
81 
82  // Do some AD
83  casadi::SX dc_da = jacobian(cs_c, cs_a);
84 
85  // Display the resulting jacobian
86  std::cout << "dc/da = " << dc_da << std::endl;
87 
88  // Create a function which takes a, b and returns c and dc_da
89  casadi::Function fun("fun", casadi::SXVector{cs_a, cs_b}, casadi::SXVector{cs_c, dc_da});
90  std::cout << "fun = " << fun << std::endl;
91 
92  // Evaluate the function
93  casadi::DMVector res =
94  fun(casadi::DMVector{std::vector<double>{1., 2., 3., 4.}, std::vector<double>{-1., -2., -3.}});
95  std::cout << "fun(a, b)=" << res << std::endl;
96 }
97 
98 BOOST_AUTO_TEST_CASE(test_jacobian)
99 {
100  casadi::SX cs_x = casadi::SX::sym("x", 3);
101 
102  casadi::SX cs_y = casadi::SX::sym("y", 1);
103  cs_y(0) = cs_x(0) + cs_x(1) + cs_x(2);
104 
105  // Display the resulting expression
106  std::cout << "y = " << cs_y << std::endl;
107 
108  // Do some AD
109  casadi::SX dy_dx = jacobian(cs_x, cs_x);
110 
111  // Display the resulting jacobian
112  std::cout << "dy/dx = " << dy_dx << std::endl;
113 }
114 
115 BOOST_AUTO_TEST_CASE(test_copy_casadi_to_eigen)
116 {
117  casadi::SX cs_mat = casadi::SX::sym("A", 3, 4);
118  Eigen::Matrix<casadi::SX, 3, 4> eig_mat;
119 
120  pinocchio::casadi::copy(cs_mat, eig_mat);
121  std::cout << eig_mat << std::endl;
122 }
123 
124 BOOST_AUTO_TEST_CASE(test_copy_eigen_to_casadi)
125 {
126  Eigen::Matrix<casadi::SX, 3, 4> eig_mat;
127  pinocchio::casadi::sym(eig_mat, "A");
128 
129  casadi::SX cs_mat;
130 
131  pinocchio::casadi::copy(eig_mat, cs_mat);
132  std::cout << cs_mat << std::endl;
133 }
134 
135 BOOST_AUTO_TEST_CASE(test_casadi_codegen)
136 {
137  casadi::SX x = casadi::SX::sym("x");
138  casadi::SX y = casadi::SX::sym("y");
139  casadi::Function fun("fun", casadi::SXVector{x, y}, casadi::SXVector{x + y});
140 
141  casadi::CodeGenerator gen("module");
142  gen.add(fun);
143 
144  std::cout << gen.dump();
145 }
146 
148 {
149  casadi::SX x = casadi::SX::sym("x");
150  casadi::SX y = casadi::SX::sym("y");
151 
152  casadi::SX max_x_y = pinocchio::math::max(x, y);
153  casadi::SX max_x_0 = pinocchio::math::max(x, 0.);
154  casadi::SX max_0_y = pinocchio::math::max(0., y);
155 }
156 
157 BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_basic)
Definition: casadi/basic.cpp:15
B
B
y
y
c
Vec3f c
pinocchio.casadi::sym
void sym(const Eigen::MatrixBase< MatrixDerived > &eig_mat, std::string const &name)
Definition: autodiff/casadi.hpp:230
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
b
Vec3f b
pinocchio::Index
PINOCCHIO_COMPILER_DIAGNOSTIC_POP typedef std::size_t Index
Definition: multibody/fwd.hpp:22
res
res
x
x
simulation-contact-dynamics.A
A
Definition: simulation-contact-dynamics.py:110
pinocchio::jacobian
void jacobian(const ConstraintModelTpl< Scalar, Options, ConstraintCollectionTpl > &cmodel, ConstraintDataTpl< Scalar, Options, ConstraintCollectionTpl > &cdata, const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< JacobianMatrix > &jacobian_matrix)
Definition: constraint-model-visitor.hpp:199
a
Vec3f a
pinocchio.casadi::copy
void copy(::casadi::Matrix< Scalar > const &src, Eigen::MatrixBase< MT > &dst)
Definition: autodiff/casadi.hpp:154
eigenFun
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > eigenFun(Eigen::MatrixBase< T1 > const &A, Eigen::MatrixBase< T2 > const &a, Eigen::MatrixBase< T3 > const &B, Eigen::MatrixBase< T4 > const &b)
Definition: casadi/basic.cpp:34
casadi.hpp
codegen-rnea.fun
fun
Definition: codegen-rnea.py:27
CppAD::max
AD< Scalar > max(const AD< Scalar > &x, const AD< Scalar > &y)
Definition: autodiff/cppad.hpp:181


pinocchio
Author(s):
autogenerated on Thu Dec 19 2024 03:41:25