unittest/casadi/rnea-derivatives.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_rnea_derivatives)
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 a(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::rnea(model, data, q, v, a);
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 
58  ConfigVectorAD q_ad(model.nq), v_int_ad(model.nv), q_int_ad(model.nq);
59  q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
60  v_int_ad =
61  Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_v_int).data(), model.nv, 1);
62 
63  pinocchio::integrate(ad_model, q_ad, v_int_ad, q_int_ad);
64  casadi::SX cs_q_int(model.nq, 1);
65  pinocchio::casadi::copy(q_int_ad, cs_q_int);
66  std::vector<double> q_vec((size_t)model.nq);
67  Eigen::Map<ConfigVector>(q_vec.data(), model.nq, 1) = q;
68 
69  std::vector<double> v_int_vec((size_t)model.nv);
70  Eigen::Map<TangentVector>(v_int_vec.data(), model.nv, 1).setZero();
71 
72  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
73  TangentVectorAD v_ad(model.nv);
74  v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
75 
76  casadi::SX cs_a = casadi::SX::sym("a", model.nv);
77  TangentVectorAD a_ad(model.nv);
78  a_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_a).data(), model.nv, 1);
79 
80  rnea(ad_model, ad_data, q_int_ad, v_ad, a_ad);
81  casadi::SX cs_tau(model.nv, 1);
82  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
83  {
84  cs_tau(k) = ad_data.tau[k];
85  }
86  casadi::Function eval_rnea(
87  "eval_rnea", casadi::SXVector{cs_q, cs_v_int, cs_v, cs_a}, casadi::SXVector{cs_tau});
88 
89  std::vector<double> v_vec((size_t)model.nv);
90  Eigen::Map<TangentVector>(v_vec.data(), model.nv, 1) = v;
91 
92  std::vector<double> a_vec((size_t)model.nv);
93  Eigen::Map<TangentVector>(a_vec.data(), model.nv, 1) = a;
94 
95  // check return value
96  casadi::DM tau_res = eval_rnea(casadi::DMVector{q_vec, v_int_vec, v_vec, a_vec})[0];
97  Data::TangentVectorType tau_vec = Eigen::Map<Data::TangentVectorType>(
98  static_cast<std::vector<double>>(tau_res).data(), model.nv, 1);
99 
100  BOOST_CHECK(data.tau.isApprox(tau_vec));
101 
102  // compute references
103  Data::MatrixXs dtau_dq_ref(model.nv, model.nv), dtau_dv_ref(model.nv, model.nv),
104  dtau_da_ref(model.nv, model.nv);
105  dtau_dq_ref.setZero();
106  dtau_dv_ref.setZero();
107  dtau_da_ref.setZero();
108 
109  pinocchio::computeRNEADerivatives(model, data, q, v, a, dtau_dq_ref, dtau_dv_ref, dtau_da_ref);
110  dtau_da_ref.triangularView<Eigen::StrictlyLower>() =
111  dtau_da_ref.transpose().triangularView<Eigen::StrictlyLower>();
112 
113  // check with respect to q+dq
114  casadi::SX dtau_dq = jacobian(cs_tau, cs_v_int);
115  casadi::Function eval_dtau_dq(
116  "eval_dtau_dq", casadi::SXVector{cs_q, cs_v_int, cs_v, cs_a}, casadi::SXVector{dtau_dq});
117 
118  casadi::DM dtau_dq_res = eval_dtau_dq(casadi::DMVector{q_vec, v_int_vec, v_vec, a_vec})[0];
119  std::vector<double> dtau_dq_vec(static_cast<std::vector<double>>(dtau_dq_res));
120  BOOST_CHECK(
121  Eigen::Map<Data::MatrixXs>(dtau_dq_vec.data(), model.nv, model.nv).isApprox(dtau_dq_ref));
122 
123  // check with respect to v+dv
124  casadi::SX dtau_dv = jacobian(cs_tau, cs_v);
125  casadi::Function eval_dtau_dv(
126  "eval_dtau_dv", casadi::SXVector{cs_q, cs_v_int, cs_v, cs_a}, casadi::SXVector{dtau_dv});
127 
128  casadi::DM dtau_dv_res = eval_dtau_dv(casadi::DMVector{q_vec, v_int_vec, v_vec, a_vec})[0];
129  std::vector<double> dtau_dv_vec(static_cast<std::vector<double>>(dtau_dv_res));
130  BOOST_CHECK(
131  Eigen::Map<Data::MatrixXs>(dtau_dv_vec.data(), model.nv, model.nv).isApprox(dtau_dv_ref));
132 
133  // check with respect to a+da
134  casadi::SX dtau_da = jacobian(cs_tau, cs_a);
135  casadi::Function eval_dtau_da(
136  "eval_dtau_da", casadi::SXVector{cs_q, cs_v_int, cs_v, cs_a}, casadi::SXVector{dtau_da});
137 
138  casadi::DM dtau_da_res = eval_dtau_da(casadi::DMVector{q_vec, v_int_vec, v_vec, a_vec})[0];
139  std::vector<double> dtau_da_vec(static_cast<std::vector<double>>(dtau_da_res));
140  BOOST_CHECK(
141  Eigen::Map<Data::MatrixXs>(dtau_da_vec.data(), model.nv, model.nv).isApprox(dtau_da_ref));
142 
143  // call RNEA derivatives in Casadi
144  casadi::SX cs_dtau_dq(model.nv, model.nv);
145  casadi::SX cs_dtau_dv(model.nv, model.nv);
146  casadi::SX cs_dtau_da(model.nv, model.nv);
147 
148  computeRNEADerivatives(ad_model, ad_data, q_ad, v_ad, a_ad);
149  ad_data.M.triangularView<Eigen::StrictlyLower>() =
150  ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();
151 
152  pinocchio::casadi::copy(ad_data.dtau_dq, cs_dtau_dq);
153  pinocchio::casadi::copy(ad_data.dtau_dv, cs_dtau_dv);
154  pinocchio::casadi::copy(ad_data.M, cs_dtau_da);
155 
156  casadi::Function eval_rnea_derivatives_dq(
157  "eval_rnea_derivatives_dq", casadi::SXVector{cs_q, cs_v, cs_a}, casadi::SXVector{cs_dtau_dq});
158 
159  casadi::DM dtau_dq_res_direct =
160  eval_rnea_derivatives_dq(casadi::DMVector{q_vec, v_vec, a_vec})[0];
161  Data::MatrixXs dtau_dq_res_direct_map = Eigen::Map<Data::MatrixXs>(
162  static_cast<std::vector<double>>(dtau_dq_res_direct).data(), model.nv, model.nv);
163  BOOST_CHECK(dtau_dq_ref.isApprox(dtau_dq_res_direct_map));
164 
165  casadi::Function eval_rnea_derivatives_dv(
166  "eval_rnea_derivatives_dv", casadi::SXVector{cs_q, cs_v, cs_a}, casadi::SXVector{cs_dtau_dv});
167 
168  casadi::DM dtau_dv_res_direct =
169  eval_rnea_derivatives_dv(casadi::DMVector{q_vec, v_vec, a_vec})[0];
170  Data::MatrixXs dtau_dv_res_direct_map = Eigen::Map<Data::MatrixXs>(
171  static_cast<std::vector<double>>(dtau_dv_res_direct).data(), model.nv, model.nv);
172  BOOST_CHECK(dtau_dv_ref.isApprox(dtau_dv_res_direct_map));
173 
174  casadi::Function eval_rnea_derivatives_da(
175  "eval_rnea_derivatives_da", casadi::SXVector{cs_q, cs_v, cs_a}, casadi::SXVector{cs_dtau_da});
176 
177  casadi::DM dtau_da_res_direct =
178  eval_rnea_derivatives_da(casadi::DMVector{q_vec, v_vec, a_vec})[0];
179  Data::MatrixXs dtau_da_res_direct_map = Eigen::Map<Data::MatrixXs>(
180  static_cast<std::vector<double>>(dtau_da_res_direct).data(), model.nv, model.nv);
181  BOOST_CHECK(dtau_da_ref.isApprox(dtau_da_res_direct_map));
182 }
183 
184 BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
Definition: unittest/casadi/rnea-derivatives.cpp:24
pinocchio::computeRNEADerivatives
void computeRNEADerivatives(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, const Eigen::MatrixBase< MatrixType1 > &rnea_partial_dq, const Eigen::MatrixBase< MatrixType2 > &rnea_partial_dv, const Eigen::MatrixBase< MatrixType3 > &rnea_partial_da)
Computes the partial derivatives of the Recursive Newton Euler Algorithms with respect to the joint c...
pinocchio.casadi::sym
void sym(const Eigen::MatrixBase< MatrixDerived > &eig_mat, std::string const &name)
Definition: autodiff/casadi.hpp:230
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::python::computeRNEADerivatives
bp::tuple computeRNEADerivatives(const context::Model &model, context::Data &data, const context::VectorXs &q, const context::VectorXs &v, const context::VectorXs &a)
Definition: expose-rnea-derivatives.cpp:38
pinocchio::python::context::Model
ModelTpl< Scalar, Options > Model
Definition: bindings/python/context/generic.hpp:61
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:325
rnea.hpp
aba-derivatives.hpp
casadi-algo.hpp
aba.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:12
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:72
pinocchio::python::context::Data
DataTpl< Scalar, Options > Data
Definition: bindings/python/context/generic.hpp:62
autodiff-rnea.v
v
Definition: autodiff-rnea.py:15
pinocchio::context::MatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
Definition: context/generic.hpp:49
autodiff-rnea.dtau_dv
dtau_dv
Definition: autodiff-rnea.py:29
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:208
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...
pinocchio.casadi::copy
void copy(::casadi::Matrix< Scalar > const &src, Eigen::MatrixBase< MT > &dst)
Definition: autodiff/casadi.hpp:154
autodiff-rnea.dtau_dq
dtau_dq
Definition: autodiff-rnea.py:29
casadi.hpp
sample-models.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
BOOST_CHECK
#define BOOST_CHECK(check)
Definition: overview-urdf.cpp:34

pinocchio
Author(s):
autogenerated on Tue Jun 25 2024 02:42:40