cppadcg/algorithms.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2019 CNRS INRIA
3 //
4 
5 #include "pinocchio/codegen/cppadcg.hpp"
6 
7 #include "pinocchio/multibody/model.hpp"
8 #include "pinocchio/multibody/data.hpp"
9 
10 #include "pinocchio/algorithm/kinematics.hpp"
11 #include "pinocchio/algorithm/jacobian.hpp"
12 #include "pinocchio/algorithm/crba.hpp"
13 #include "pinocchio/algorithm/rnea.hpp"
14 #include "pinocchio/algorithm/aba.hpp"
15 #include "pinocchio/algorithm/joint-configuration.hpp"
16 
17 #include "pinocchio/multibody/sample-models.hpp"
18 
19 #include <iostream>
20 
21 #include <boost/test/unit_test.hpp>
22 #include <boost/utility/binary.hpp>
23 
24 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
25 
26 BOOST_AUTO_TEST_CASE(test_crba_code_generation)
27 {
28  typedef double Scalar;
29  typedef CppAD::cg::CG<Scalar> CGScalar;
30  typedef CppAD::AD<CGScalar> ADScalar;
31 
32  typedef Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ADVector;
33 
34  typedef pinocchio::ModelTpl<Scalar> Model;
35  typedef Model::Data Data;
36 
37  typedef pinocchio::ModelTpl<ADScalar> ADModel;
38  typedef ADModel::Data ADData;
39 
40  Model model;
41  pinocchio::buildModels::humanoidRandom(model);
42  model.lowerPositionLimit.head<3>().fill(-1.);
43  model.upperPositionLimit.head<3>().fill(1.);
44  Data data(model);
45 
46  ADModel ad_model = model.cast<ADScalar>();
47  ADData ad_data(ad_model);
48 
49  // Sample random configuration
50  typedef Model::ConfigVectorType ConfigVectorType;
51  typedef Model::TangentVectorType TangentVectorType;
52  ConfigVectorType q(model.nq);
53  q = pinocchio::randomConfiguration(model);
54 
55  TangentVectorType v(TangentVectorType::Random(model.nv));
56  TangentVectorType a(TangentVectorType::Random(model.nv));
57 
58  typedef ADModel::ConfigVectorType ADConfigVectorType;
59  typedef ADModel::TangentVectorType ADTangentVectorType;
60 
61  ADConfigVectorType ad_q = q.cast<ADScalar>();
62  ADTangentVectorType ad_v = v.cast<ADScalar>();
63  ADTangentVectorType ad_a = a.cast<ADScalar>();
64 
65  ADTangentVectorType & X = ad_a;
66  CppAD::Independent(X);
67 
68  pinocchio::rnea(ad_model, ad_data, ad_q, ad_v, ad_a);
69  ADVector Y(model.nv);
70  Y = ad_data.tau;
71 
72  CppAD::ADFun<CGScalar> fun(X, Y);
73 
74  // generates source code
75  CppAD::cg::ModelCSourceGen<Scalar> cgen(fun, "rnea");
76  cgen.setCreateJacobian(true);
77  cgen.setCreateForwardZero(true);
78  cgen.setCreateForwardOne(true);
79  cgen.setCreateReverseOne(true);
80  cgen.setCreateReverseTwo(true);
81  CppAD::cg::ModelLibraryCSourceGen<Scalar> libcgen(cgen);
82 
83  // compile source code
84  CppAD::cg::DynamicModelLibraryProcessor<Scalar> p(libcgen);
85 
86  CppAD::cg::GccCompiler<Scalar> compiler(PINOCCHIO_CXX_COMPILER);
87  std::unique_ptr<CppAD::cg::DynamicLib<Scalar>> dynamicLib = p.createDynamicLibrary(compiler);
88 
89  // save to files (not really required)
90  CppAD::cg::SaveFilesModelLibraryProcessor<Scalar> p2(libcgen);
91  p2.saveSources();
92 
93  // use the generated code
94  std::unique_ptr<CppAD::cg::GenericModel<Scalar>> rnea_generated = dynamicLib->model("rnea");
95 
96  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
97  Eigen::Map<TangentVectorType>(x.data(), model.nv, 1) = a;
98 
99  CPPAD_TESTVECTOR(Scalar) tau = rnea_generated->ForwardZero(x);
100 
101  Eigen::Map<TangentVectorType> tau_map(tau.data(), model.nv, 1);
102  Data::TangentVectorType tau_ref = pinocchio::rnea(model, data, q, v, a);
103  BOOST_CHECK(tau_map.isApprox(tau_ref));
104 
105  pinocchio::crba(model, data, q, pinocchio::Convention::WORLD);
106  data.M.triangularView<Eigen::StrictlyLower>() =
107  data.M.transpose().triangularView<Eigen::StrictlyLower>();
108 
109  CPPAD_TESTVECTOR(Scalar) dtau_da = rnea_generated->Jacobian(x);
110  Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)> M_map(
111  dtau_da.data(), model.nv, model.nv);
112  BOOST_CHECK(M_map.isApprox(data.M));
113 }
114 
115 BOOST_AUTO_TEST_CASE(test_crba_code_generation_pointer)
116 {
117  typedef double Scalar;
118  typedef CppAD::cg::CG<Scalar> CGScalar;
119  typedef CppAD::AD<CGScalar> ADScalar;
120  typedef CppAD::ADFun<CGScalar> ADCGFun;
121 
122  typedef Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ADVector;
123  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMatrixXs;
124 
125  typedef pinocchio::ModelTpl<Scalar> Model;
126  typedef Model::Data Data;
127 
128  typedef pinocchio::ModelTpl<ADScalar> ADModel;
129  typedef ADModel::Data ADData;
130 
131  Model model;
132  pinocchio::buildModels::humanoidRandom(model);
133  int nq = model.nq;
134  int nv = model.nv;
135  model.lowerPositionLimit.head<3>().fill(-1.);
136  model.upperPositionLimit.head<3>().fill(1.);
137  Data data(model);
138 
139  ADModel ad_model = model.cast<ADScalar>();
140  ADData ad_data(ad_model);
141 
142  // Sample random configuration
143  typedef Model::ConfigVectorType ConfigVectorType;
144  typedef Model::TangentVectorType TangentVectorType;
145  ConfigVectorType q(nq);
146  q = pinocchio::randomConfiguration(model);
147 
148  TangentVectorType v(TangentVectorType::Random(nv));
149  TangentVectorType a(TangentVectorType::Random(nv));
150 
151  typedef ADModel::ConfigVectorType ADConfigVectorType;
152  typedef ADModel::TangentVectorType ADTangentVectorType;
153 
154  ADConfigVectorType ad_q = q.cast<ADScalar>();
155  ADTangentVectorType ad_v = v.cast<ADScalar>();
156  ADTangentVectorType ad_a = a.cast<ADScalar>();
157 
158  ADConfigVectorType ad_X = ADConfigVectorType::Zero(nq + 2 * nv);
159  Eigen::DenseIndex i = 0;
160  ad_X.segment(i, nq) = ad_q;
161  i += nq;
162  ad_X.segment(i, nv) = ad_v;
163  i += nv;
164  ad_X.segment(i, nv) = ad_a;
165  i += nv;
166 
167  ADCGFun ad_fun;
168  std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar>> cgen_ptr;
169  std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar>> libcgen_ptr;
170  std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar>> dynamicLibManager_ptr;
171  std::unique_ptr<CppAD::cg::DynamicLib<Scalar>> dynamicLib_ptr;
172  std::unique_ptr<CppAD::cg::GenericModel<Scalar>> generatedFun_ptr;
173 
174  const std::string & function_name = "rnea";
175  const std::string & library_name = "cg_rnea_eval";
176  const std::string & compile_options = "-Ofast";
177 
178  CppAD::Independent(ad_X);
179  pinocchio::rnea(
180  ad_model, ad_data, ad_X.segment(0, nq), ad_X.segment(nq, nv), ad_X.segment(nq + nv, nv));
181  ADVector ad_Y = ad_data.tau;
182  ad_fun.Dependent(ad_X, ad_Y);
183  ad_fun.optimize("no_compare_op");
184  RowMatrixXs jac = RowMatrixXs::Zero(ad_Y.size(), ad_X.size());
185 
186  // generates source code
187  cgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar>>(
188  new CppAD::cg::ModelCSourceGen<Scalar>(ad_fun, function_name));
189  cgen_ptr->setCreateForwardZero(true);
190  cgen_ptr->setCreateJacobian(true);
191  libcgen_ptr = std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar>>(
192  new CppAD::cg::ModelLibraryCSourceGen<Scalar>(*cgen_ptr));
193 
194  dynamicLibManager_ptr = std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar>>(
195  new CppAD::cg::DynamicModelLibraryProcessor<Scalar>(*libcgen_ptr, library_name));
196 
197  CppAD::cg::GccCompiler<Scalar> compiler(PINOCCHIO_CXX_COMPILER);
198  std::vector<std::string> compile_flags = compiler.getCompileFlags();
199  compile_flags[0] = compile_options;
200  compiler.setCompileFlags(compile_flags);
201  dynamicLibManager_ptr->createDynamicLibrary(compiler, false);
202 
203  const auto it = dynamicLibManager_ptr->getOptions().find("dlOpenMode");
204  if (it == dynamicLibManager_ptr->getOptions().end())
205  {
206  dynamicLib_ptr.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(
207  dynamicLibManager_ptr->getLibraryName()
208  + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION));
209  }
210  else
211  {
212  int dlOpenMode = std::stoi(it->second);
213  dynamicLib_ptr.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(
214  dynamicLibManager_ptr->getLibraryName()
215  + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION,
216  dlOpenMode));
217  }
218 
219  generatedFun_ptr = dynamicLib_ptr->model(function_name.c_str());
220 
221  CppAD::cg::ArrayView<Scalar> jac_(jac.data(), (size_t)jac.size());
222  for (size_t it = 0; it < 3; ++it)
223  {
224  std::cout << "test " << it << std::endl;
225  ConfigVectorType q = pinocchio::randomConfiguration(model);
226  TangentVectorType v(TangentVectorType::Random(nv));
227  TangentVectorType a(TangentVectorType::Random(nv));
228 
229  ConfigVectorType x = ConfigVectorType::Zero(nq + 2 * nv);
230  i = 0;
231  x.segment(i, nq) = q;
232  i += nq;
233  x.segment(i, nv) = v;
234  i += nv;
235  x.segment(i, nv) = a;
236  i += nv;
237 
238  CppAD::cg::ArrayView<const Scalar> x_(x.data(), (size_t)x.size());
239  generatedFun_ptr->Jacobian(x_, jac_);
240 
241  pinocchio::crba(model, data, q, pinocchio::Convention::WORLD);
242  data.M.triangularView<Eigen::StrictlyLower>() =
243  data.M.transpose().triangularView<Eigen::StrictlyLower>();
244  BOOST_CHECK(jac.middleCols(nq + nv, nv).isApprox(data.M));
245  }
246 }
247 
248 BOOST_AUTO_TEST_SUITE_END()
cppadcg.hpp
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_crba_code_generation)
Definition: cppadcg/algorithms.cpp:26
codegen-rnea.jac
jac
Definition: codegen-rnea.py:41
pinocchio::Convention::WORLD
@ WORLD
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
kinematics.hpp
model.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Convention convention=Convention::LOCAL)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
pinocchio::python::context::Model
ModelTpl< Scalar, Options > Model
Definition: bindings/python/context/generic.hpp:63
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:315
rnea.hpp
pinocchio::context::RowMatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor|Options > RowMatrixXs
Definition: context/generic.hpp:51
aba.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
dpendulum.p
p
Definition: dpendulum.py:7
codegen-rnea.nq
nq
Definition: codegen-rnea.py:18
joint-configuration.hpp
pinocchio::python::context::Data
DataTpl< Scalar, Options > Data
Definition: bindings/python/context/generic.hpp:64
pinocchio::context::MatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
Definition: context/generic.hpp:49
cartpole.v
v
Definition: cartpole.py:153
data.hpp
x
x
q
q
inverse-kinematics-3d.it
int it
Definition: inverse-kinematics-3d.py:17
autodiff-rnea.dtau_da
dtau_da
Definition: autodiff-rnea.py:29
a
Vec3f a
fill
fill
pinocchio::rnea
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
Y
Y
codegen-rnea.nv
nv
Definition: codegen-rnea.py:19
sample-models.hpp
codegen-rnea.fun
fun
Definition: codegen-rnea.py:27
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
crba.hpp
X
PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE
#define PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(D)
Similar to macro PINOCCHIO_EIGEN_PLAIN_TYPE but with guaranty to provite a row major type.
Definition: eigen-macros.hpp:23

pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:06