unittest/casadi/aba.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4 
5 #include "pinocchio/autodiff/casadi.hpp"
6 #include "pinocchio/autodiff/casadi-algo.hpp"
7 
8 #include "pinocchio/algorithm/rnea.hpp"
9 #include "pinocchio/algorithm/rnea-derivatives.hpp"
10 #include "pinocchio/algorithm/aba.hpp"
11 #include "pinocchio/algorithm/aba-derivatives.hpp"
12 #include "pinocchio/algorithm/joint-configuration.hpp"
13 
14 #include "pinocchio/multibody/sample-models.hpp"
15 
16 #include <casadi/casadi.hpp>
17 
18 #include <iostream>
19 #include <boost/test/unit_test.hpp>
20 #include <boost/utility/binary.hpp>
21 
22 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
23 
24 BOOST_AUTO_TEST_CASE(test_aba)
25 {
26  typedef double Scalar;
27  typedef casadi::SX ADScalar;
28 
29  typedef pinocchio::ModelTpl<Scalar> Model;
30  typedef Model::Data Data;
31 
32  typedef pinocchio::ModelTpl<ADScalar> ADModel;
33  typedef ADModel::Data ADData;
34 
35  Model model;
36  pinocchio::buildModels::humanoidRandom(model);
37  model.lowerPositionLimit.head<3>().fill(-1.);
38  model.upperPositionLimit.head<3>().fill(1.);
39  Data data(model);
40 
41  typedef Model::ConfigVectorType ConfigVector;
42  typedef Model::TangentVectorType TangentVector;
43  ConfigVector q(model.nq);
44  q = pinocchio::randomConfiguration(model);
45  TangentVector v(TangentVector::Random(model.nv));
46  TangentVector tau(TangentVector::Random(model.nv));
47 
48  typedef ADModel::ConfigVectorType ConfigVectorAD;
49  typedef ADModel::TangentVectorType TangentVectorAD;
50  ADModel ad_model = model.cast<ADScalar>();
51  ADData ad_data(ad_model);
52 
53  pinocchio::aba(model, data, q, v, tau, pinocchio::Convention::WORLD);
54 
55  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
56  casadi::SX cs_v_int = casadi::SX::sym("v_inc", model.nv);
57  ConfigVectorAD q_ad(model.nq), v_int_ad(model.nv), q_int_ad(model.nq);
58  q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
59  v_int_ad =
60  Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_v_int).data(), model.nv, 1);
61 
62  pinocchio::integrate(ad_model, q_ad, v_int_ad, q_int_ad);
63  casadi::SX cs_q_int(model.nq, 1);
64  pinocchio::casadi::copy(q_int_ad, cs_q_int);
65  std::vector<double> q_vec((size_t)model.nq);
66  Eigen::Map<ConfigVector>(q_vec.data(), model.nq, 1) = q;
67 
68  std::vector<double> v_int_vec((size_t)model.nv);
69  Eigen::Map<TangentVector>(v_int_vec.data(), model.nv, 1).setZero();
70 
71  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
72  TangentVectorAD v_ad(model.nv);
73  v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
74 
75  casadi::SX cs_tau = casadi::SX::sym("tau", model.nv);
76  TangentVectorAD tau_ad(model.nv);
77  tau_ad =
78  Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_tau).data(), model.nv, 1);
79 
80  // ABA
81  pinocchio::aba(ad_model, ad_data, q_int_ad, v_ad, tau_ad, pinocchio::Convention::WORLD);
82  casadi::SX cs_ddq(model.nv, 1);
83  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
84  cs_ddq(k) = ad_data.ddq[k];
85  casadi::Function eval_aba(
86  "eval_aba", casadi::SXVector{cs_q, cs_v_int, cs_v, cs_tau}, casadi::SXVector{cs_ddq});
87 
88  std::vector<double> v_vec((size_t)model.nv);
89  Eigen::Map<TangentVector>(v_vec.data(), model.nv, 1) = v;
90 
91  std::vector<double> tau_vec((size_t)model.nv);
92  Eigen::Map<TangentVector>(tau_vec.data(), model.nv, 1) = tau;
93 
94  casadi::DM ddq_res = eval_aba(casadi::DMVector{q_vec, v_int_vec, v_vec, tau_vec})[0];
95  Data::TangentVectorType ddq_mat = Eigen::Map<Data::TangentVectorType>(
96  static_cast<std::vector<double>>(ddq_res).data(), model.nv, 1);
97 
98  BOOST_CHECK(ddq_mat.isApprox(data.ddq));
99 
100  // compute references
101  Data::MatrixXs ddq_dq_ref(model.nv, model.nv), ddq_dv_ref(model.nv, model.nv),
102  ddq_dtau_ref(model.nv, model.nv);
103  ddq_dq_ref.setZero();
104  ddq_dv_ref.setZero();
105  ddq_dtau_ref.setZero();
106 
107  pinocchio::computeABADerivatives(model, data, q, v, tau, ddq_dq_ref, ddq_dv_ref, ddq_dtau_ref);
108  ddq_dtau_ref.triangularView<Eigen::StrictlyLower>() =
109  ddq_dtau_ref.transpose().triangularView<Eigen::StrictlyLower>();
110 
111  // check with respect to q+dq
112  casadi::SX ddq_dq = jacobian(cs_ddq, cs_v_int);
113  casadi::Function eval_ddq_dq(
114  "eval_ddq_dq", casadi::SXVector{cs_q, cs_v_int, cs_v, cs_tau}, casadi::SXVector{ddq_dq});
115 
116  casadi::DM ddq_dq_res = eval_ddq_dq(casadi::DMVector{q_vec, v_int_vec, v_vec, tau_vec})[0];
117  std::vector<double> ddq_dq_vec(static_cast<std::vector<double>>(ddq_dq_res));
118  BOOST_CHECK(
119  Eigen::Map<Data::MatrixXs>(ddq_dq_vec.data(), model.nv, model.nv).isApprox(ddq_dq_ref));
120 
121  // check with respect to v+dv
122  casadi::SX ddq_dv = jacobian(cs_ddq, cs_v);
123  casadi::Function eval_ddq_dv(
124  "eval_ddq_dv", casadi::SXVector{cs_q, cs_v_int, cs_v, cs_tau}, casadi::SXVector{ddq_dv});
125 
126  casadi::DM ddq_dv_res = eval_ddq_dv(casadi::DMVector{q_vec, v_int_vec, v_vec, tau_vec})[0];
127  std::vector<double> ddq_dv_vec(static_cast<std::vector<double>>(ddq_dv_res));
128  BOOST_CHECK(
129  Eigen::Map<Data::MatrixXs>(ddq_dv_vec.data(), model.nv, model.nv).isApprox(ddq_dv_ref));
130 
131  // check with respect to a+da
132  casadi::SX ddq_dtau = jacobian(cs_ddq, cs_tau);
133  casadi::Function eval_ddq_da(
134  "eval_ddq_da", casadi::SXVector{cs_q, cs_v_int, cs_v, cs_tau}, casadi::SXVector{ddq_dtau});
135 
136  casadi::DM ddq_dtau_res = eval_ddq_da(casadi::DMVector{q_vec, v_int_vec, v_vec, tau_vec})[0];
137  std::vector<double> ddq_dtau_vec(static_cast<std::vector<double>>(ddq_dtau_res));
138  BOOST_CHECK(
139  Eigen::Map<Data::MatrixXs>(ddq_dtau_vec.data(), model.nv, model.nv).isApprox(ddq_dtau_ref));
140 
141  // call ABA derivatives in Casadi
142  casadi::SX cs_ddq_dq(model.nv, model.nv);
143  casadi::SX cs_ddq_dv(model.nv, model.nv);
144  casadi::SX cs_ddq_dtau(model.nv, model.nv);
145 
146  computeABADerivatives(ad_model, ad_data, q_ad, v_ad, tau_ad);
147  ad_data.Minv.triangularView<Eigen::StrictlyLower>() =
148  ad_data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
149 
150  pinocchio::casadi::copy(ad_data.ddq_dq, cs_ddq_dq);
151  pinocchio::casadi::copy(ad_data.ddq_dv, cs_ddq_dv);
152  pinocchio::casadi::copy(ad_data.Minv, cs_ddq_dtau);
153 
154  casadi::Function eval_aba_derivatives_dq(
155  "eval_aba_derivatives_dq", casadi::SXVector{cs_q, cs_v, cs_tau}, casadi::SXVector{cs_ddq_dq});
156 
157  casadi::DM ddq_dq_res_direct =
158  eval_aba_derivatives_dq(casadi::DMVector{q_vec, v_vec, tau_vec})[0];
159  Data::MatrixXs ddq_dq_res_direct_map = Eigen::Map<Data::MatrixXs>(
160  static_cast<std::vector<double>>(ddq_dq_res_direct).data(), model.nv, model.nv);
161  BOOST_CHECK(ddq_dq_ref.isApprox(ddq_dq_res_direct_map));
162 
163  casadi::Function eval_aba_derivatives_dv(
164  "eval_aba_derivatives_dv", casadi::SXVector{cs_q, cs_v, cs_tau}, casadi::SXVector{cs_ddq_dv});
165 
166  casadi::DM ddq_dv_res_direct =
167  eval_aba_derivatives_dv(casadi::DMVector{q_vec, v_vec, tau_vec})[0];
168  Data::MatrixXs ddq_dv_res_direct_map = Eigen::Map<Data::MatrixXs>(
169  static_cast<std::vector<double>>(ddq_dv_res_direct).data(), model.nv, model.nv);
170  BOOST_CHECK(ddq_dv_ref.isApprox(ddq_dv_res_direct_map));
171 
172  casadi::Function eval_aba_derivatives_dtau(
173  "eval_aba_derivatives_dtau", casadi::SXVector{cs_q, cs_v, cs_tau},
174  casadi::SXVector{cs_ddq_dtau});
175 
176  casadi::DM ddq_dtau_res_direct =
177  eval_aba_derivatives_dtau(casadi::DMVector{q_vec, v_vec, tau_vec})[0];
178  Data::MatrixXs ddq_dtau_res_direct_map = Eigen::Map<Data::MatrixXs>(
179  static_cast<std::vector<double>>(ddq_dtau_res_direct).data(), model.nv, model.nv);
180  BOOST_CHECK(ddq_dtau_ref.isApprox(ddq_dtau_res_direct_map));
181 }
182 
183 BOOST_AUTO_TEST_CASE(test_aba_casadi_algo)
184 {
185  typedef double Scalar;
186  typedef pinocchio::ModelTpl<Scalar> Model;
187  typedef pinocchio::DataTpl<Scalar> Data;
188  typedef typename Model::ConfigVectorType ConfigVector;
189  typedef typename Model::TangentVectorType TangentVector;
190 
191  Model model;
192  pinocchio::buildModels::humanoidRandom(model);
193  model.lowerPositionLimit.head<3>().fill(-1.);
194  model.upperPositionLimit.head<3>().fill(1.);
195  Data data(model);
196 
197  ConfigVector q(model.nq);
198  q = pinocchio::randomConfiguration(model);
199  TangentVector v(TangentVector::Random(model.nv));
200  TangentVector tau(TangentVector::Random(model.nv));
201 
202  pinocchio::aba(model, data, q, v, tau, pinocchio::Convention::WORLD);
203  pinocchio::computeABADerivatives(model, data, q, v, tau);
204  data.Minv.triangularView<Eigen::StrictlyLower>() =
205  data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
206 
207  pinocchio::casadi::AutoDiffABA<Scalar> ad_casadi(model);
208  ad_casadi.initLib();
209  std::cout << "Number of operations in the ABA function = " << ad_casadi.getFunOperationCount()
210  << "\n";
211  std::cout << "Number of operations in the ABA derivs function = "
212  << ad_casadi.getFunDerivsOperationCount() << "\n";
213  ad_casadi.loadLib();
214 
215  ad_casadi.evalFunction(q, v, tau);
216  ad_casadi.evalJacobian(q, v, tau);
217  BOOST_CHECK(ad_casadi.ddq.isApprox(data.ddq));
218  BOOST_CHECK(ad_casadi.ddq_dq.isApprox(data.ddq_dq));
219  BOOST_CHECK(ad_casadi.ddq_dv.isApprox(data.ddq_dv));
220  BOOST_CHECK(ad_casadi.ddq_dtau.isApprox(data.Minv));
221 }
222 
223 BOOST_AUTO_TEST_SUITE_END()
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio.casadi::sym
void sym(const Eigen::MatrixBase< MatrixDerived > &eig_mat, std::string const &name)
Definition: autodiff/casadi.hpp:230
pinocchio::Convention::WORLD
@ WORLD
rnea-derivatives.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::aba
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(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 > &tau, const Convention convention=Convention::LOCAL)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
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
aba-derivatives.hpp
casadi-algo.hpp
aba.hpp
pinocchio.casadi::AutoDiffABA
Definition: casadi-algo.hpp:154
anymal-simulation.model
model
Definition: anymal-simulation.py:8
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_aba)
Definition: unittest/casadi/aba.cpp:24
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
forward-dynamics-derivatives.ddq_dtau
ddq_dtau
Definition: forward-dynamics-derivatives.py:35
joint-configuration.hpp
pinocchio::integrate
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
Definition: joint-configuration.hpp:70
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
q
q
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
forward-dynamics-derivatives.ddq_dq
ddq_dq
Definition: forward-dynamics-derivatives.py:33
pinocchio::computeABADerivatives
void computeABADerivatives(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 > &tau, const Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
The derivatives of the Articulated-Body algorithm.
forward-dynamics-derivatives.ddq_dv
ddq_dv
Definition: forward-dynamics-derivatives.py:34
fill
fill
pinocchio.casadi::copy
void copy(::casadi::Matrix< Scalar > const &src, Eigen::MatrixBase< MT > &dst)
Definition: autodiff/casadi.hpp:154
casadi.hpp
pinocchio::python::computeABADerivatives
bp::tuple computeABADerivatives(const context::Model &model, context::Data &data, const context::VectorXs &q, const context::VectorXs &v, const context::VectorXs &tau)
Definition: expose-aba-derivatives.cpp:20
sample-models.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20

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