derivatives.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 
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/rnea-derivatives.hpp"
15 #include "pinocchio/algorithm/aba.hpp"
16 #include "pinocchio/algorithm/aba-derivatives.hpp"
17 #include "pinocchio/algorithm/joint-configuration.hpp"
18 
19 #include "pinocchio/multibody/sample-models.hpp"
20 
21 #include <iostream>
22 
23 #include <boost/test/unit_test.hpp>
24 #include <boost/utility/binary.hpp>
25 
26 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
27 
28 BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
29 {
30  using CppAD::AD;
31  using CppAD::NearEqual;
32 
33  typedef double Scalar;
34  typedef AD<Scalar> ADScalar;
35 
36  typedef pinocchio::ModelTpl<Scalar> Model;
37  typedef Model::Data Data;
38 
39  typedef pinocchio::ModelTpl<ADScalar> ADModel;
40  typedef ADModel::Data ADData;
41 
42  Model model;
43  pinocchio::buildModels::humanoidRandom(model);
44  model.lowerPositionLimit.head<3>().fill(-1.);
45  model.upperPositionLimit.head<3>().fill(1.);
46  Data data(model);
47 
48  ADModel ad_model = model.cast<ADScalar>();
49  ADData ad_data(ad_model);
50 
51  // Sample random configuration
52  typedef Model::ConfigVectorType ConfigVectorType;
53  typedef Model::TangentVectorType TangentVectorType;
54  ConfigVectorType q(model.nq);
55  q = pinocchio::randomConfiguration(model);
56 
57  TangentVectorType v(TangentVectorType::Random(model.nv));
58  TangentVectorType a(TangentVectorType::Random(model.nv));
59 
60  Eigen::MatrixXd rnea_partial_dq(model.nv, model.nv);
61  rnea_partial_dq.setZero();
62  Eigen::MatrixXd rnea_partial_dv(model.nv, model.nv);
63  rnea_partial_dv.setZero();
64  Eigen::MatrixXd rnea_partial_da(model.nv, model.nv);
65  rnea_partial_da.setZero();
66 
67  pinocchio::computeRNEADerivatives(
68  model, data, q, v, a, rnea_partial_dq, rnea_partial_dv, rnea_partial_da);
69 
70  rnea_partial_da.triangularView<Eigen::StrictlyLower>() =
71  rnea_partial_da.transpose().triangularView<Eigen::StrictlyLower>();
72 
73  typedef ADModel::ConfigVectorType ADConfigVectorType;
74  typedef ADModel::TangentVectorType ADTangentVectorType;
75 
76  ADConfigVectorType ad_q = q.cast<ADScalar>();
77  ADTangentVectorType ad_dq = ADTangentVectorType::Zero(model.nv);
78  ADTangentVectorType ad_v = v.cast<ADScalar>();
79  ADTangentVectorType ad_a = a.cast<ADScalar>();
80 
81  typedef Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> VectorXAD;
82  pinocchio::crba(model, data, q, pinocchio::Convention::WORLD);
83  data.M.triangularView<Eigen::StrictlyLower>() =
84  data.M.transpose().triangularView<Eigen::StrictlyLower>();
85 
86  Data::TangentVectorType tau = pinocchio::rnea(model, data, q, v, a);
87 
88  // dtau_dq
89  {
90  CppAD::Independent(ad_dq);
91  ADConfigVectorType ad_q_plus = pinocchio::integrate(ad_model, ad_q, ad_dq);
92  pinocchio::rnea(ad_model, ad_data, ad_q_plus, ad_v, ad_a);
93 
94  VectorXAD Y(model.nv);
95  Eigen::Map<ADData::TangentVectorType>(Y.data(), model.nv, 1) = ad_data.tau;
96 
97  CppAD::ADFun<Scalar> ad_fun(ad_dq, Y);
98 
99  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
100  Eigen::Map<Data::TangentVectorType>(x.data(), model.nv, 1).setZero();
101 
102  CPPAD_TESTVECTOR(Scalar) tau = ad_fun.Forward(0, x);
103  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(tau.data(), model.nv, 1).isApprox(data.tau));
104 
105  CPPAD_TESTVECTOR(Scalar) dtau_dq = ad_fun.Jacobian(x);
106  Data::MatrixXs dtau_dq_mat = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(
107  dtau_dq.data(), model.nv, model.nv);
108  BOOST_CHECK(dtau_dq_mat.isApprox(rnea_partial_dq));
109  }
110 
111  // dtau_dv
112  {
113  CppAD::Independent(ad_v);
114  pinocchio::rnea(ad_model, ad_data, ad_q, ad_v, ad_a);
115 
116  VectorXAD Y(model.nv);
117  Eigen::Map<ADData::TangentVectorType>(Y.data(), model.nv, 1) = ad_data.tau;
118 
119  CppAD::ADFun<Scalar> ad_fun(ad_v, Y);
120 
121  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
122  Eigen::Map<Data::TangentVectorType>(x.data(), model.nv, 1) = v;
123 
124  CPPAD_TESTVECTOR(Scalar) tau = ad_fun.Forward(0, x);
125  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(tau.data(), model.nv, 1).isApprox(data.tau));
126 
127  CPPAD_TESTVECTOR(Scalar) dtau_dv = ad_fun.Jacobian(x);
128  Data::MatrixXs dtau_dv_mat = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(
129  dtau_dv.data(), model.nv, model.nv);
130  BOOST_CHECK(dtau_dv_mat.isApprox(rnea_partial_dv));
131  }
132 
133  // dtau_da
134  {
135  CppAD::Independent(ad_a);
136  pinocchio::rnea(ad_model, ad_data, ad_q, ad_v, ad_a);
137 
138  VectorXAD Y(model.nv);
139  Eigen::Map<ADData::TangentVectorType>(Y.data(), model.nv, 1) = ad_data.tau;
140 
141  CppAD::ADFun<Scalar> ad_fun(ad_a, Y);
142 
143  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
144  Eigen::Map<Data::TangentVectorType>(x.data(), model.nv, 1) = a;
145 
146  CPPAD_TESTVECTOR(Scalar) tau = ad_fun.Forward(0, x);
147  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(tau.data(), model.nv, 1).isApprox(data.tau));
148 
149  CPPAD_TESTVECTOR(Scalar) dtau_da = ad_fun.Jacobian(x);
150  Data::MatrixXs dtau_da_mat = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(
151  dtau_da.data(), model.nv, model.nv);
152  BOOST_CHECK(dtau_da_mat.isApprox(rnea_partial_da));
153  BOOST_CHECK(dtau_da_mat.isApprox(data.M));
154  }
155 }
156 
157 BOOST_AUTO_TEST_CASE(test_aba_derivatives)
158 {
159  using CppAD::AD;
160  using CppAD::NearEqual;
161 
162  typedef double Scalar;
163  typedef AD<Scalar> ADScalar;
164 
165  typedef pinocchio::ModelTpl<Scalar> Model;
166  typedef Model::Data Data;
167 
168  typedef pinocchio::ModelTpl<ADScalar> ADModel;
169  typedef ADModel::Data ADData;
170 
171  Model model;
172  pinocchio::buildModels::humanoidRandom(model);
173  model.lowerPositionLimit.head<3>().fill(-1.);
174  model.upperPositionLimit.head<3>().fill(1.);
175  Data data(model);
176 
177  ADModel ad_model = model.cast<ADScalar>();
178  ADData ad_data(ad_model);
179 
180  // Sample random configuration
181  typedef Model::ConfigVectorType ConfigVectorType;
182  typedef Model::TangentVectorType TangentVectorType;
183  ConfigVectorType q(model.nq);
184  q = pinocchio::randomConfiguration(model);
185 
186  TangentVectorType v(TangentVectorType::Random(model.nv));
187  TangentVectorType tau(TangentVectorType::Random(model.nv));
188 
189  Eigen::MatrixXd aba_partial_dq(model.nv, model.nv);
190  aba_partial_dq.setZero();
191  Eigen::MatrixXd aba_partial_dv(model.nv, model.nv);
192  aba_partial_dv.setZero();
193  Eigen::MatrixXd aba_partial_dtau(model.nv, model.nv);
194  aba_partial_dtau.setZero();
195 
196  pinocchio::computeABADerivatives(
197  model, data, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
198 
199  aba_partial_dtau.triangularView<Eigen::StrictlyLower>() =
200  aba_partial_dtau.transpose().triangularView<Eigen::StrictlyLower>();
201 
202  typedef ADModel::ConfigVectorType ADConfigVectorType;
203  typedef ADModel::TangentVectorType ADTangentVectorType;
204 
205  ADConfigVectorType ad_q = q.cast<ADScalar>();
206  ADTangentVectorType ad_dq = ADTangentVectorType::Zero(model.nv);
207  ADTangentVectorType ad_v = v.cast<ADScalar>();
208  ADTangentVectorType ad_tau = tau.cast<ADScalar>();
209 
210  typedef Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> VectorXAD;
211  pinocchio::computeMinverse(model, data, q);
212  data.Minv.triangularView<Eigen::StrictlyLower>() =
213  data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
214 
215  Data::TangentVectorType ddq =
216  pinocchio::aba(model, data, q, v, tau, pinocchio::Convention::WORLD);
217 
218  // dddq_dq
219  {
220  CppAD::Independent(ad_dq);
221  ADConfigVectorType ad_q_plus = pinocchio::integrate(ad_model, ad_q, ad_dq);
222  pinocchio::aba(ad_model, ad_data, ad_q_plus, ad_v, ad_tau, pinocchio::Convention::WORLD);
223 
224  VectorXAD Y(model.nv);
225  Eigen::Map<ADData::TangentVectorType>(Y.data(), model.nv, 1) = ad_data.ddq;
226 
227  CppAD::ADFun<Scalar> ad_fun(ad_dq, Y);
228 
229  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
230  Eigen::Map<Data::TangentVectorType>(x.data(), model.nv, 1).setZero();
231 
232  CPPAD_TESTVECTOR(Scalar) ddq = ad_fun.Forward(0, x);
233  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(ddq.data(), model.nv, 1).isApprox(data.ddq));
234 
235  CPPAD_TESTVECTOR(Scalar) ddq_dq = ad_fun.Jacobian(x);
236  Data::MatrixXs ddq_dq_mat = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(
237  ddq_dq.data(), model.nv, model.nv);
238  BOOST_CHECK(ddq_dq_mat.isApprox(aba_partial_dq));
239  }
240 
241  // dddq_dv
242  {
243  CppAD::Independent(ad_v);
244  pinocchio::aba(ad_model, ad_data, ad_q, ad_v, ad_tau, pinocchio::Convention::WORLD);
245 
246  VectorXAD Y(model.nv);
247  Eigen::Map<ADData::TangentVectorType>(Y.data(), model.nv, 1) = ad_data.ddq;
248 
249  CppAD::ADFun<Scalar> ad_fun(ad_v, Y);
250 
251  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
252  Eigen::Map<Data::TangentVectorType>(x.data(), model.nv, 1) = v;
253 
254  CPPAD_TESTVECTOR(Scalar) ddq = ad_fun.Forward(0, x);
255  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(ddq.data(), model.nv, 1).isApprox(data.ddq));
256 
257  CPPAD_TESTVECTOR(Scalar) ddq_dv = ad_fun.Jacobian(x);
258  Data::MatrixXs ddq_dv_mat = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(
259  ddq_dv.data(), model.nv, model.nv);
260  BOOST_CHECK(ddq_dv_mat.isApprox(aba_partial_dv));
261  }
262 
263  // dddq_da
264  {
265  CppAD::Independent(ad_tau);
266  pinocchio::aba(ad_model, ad_data, ad_q, ad_v, ad_tau, pinocchio::Convention::WORLD);
267 
268  VectorXAD Y(model.nv);
269  Eigen::Map<ADData::TangentVectorType>(Y.data(), model.nv, 1) = ad_data.ddq;
270 
271  CppAD::ADFun<Scalar> ad_fun(ad_tau, Y);
272 
273  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
274  Eigen::Map<Data::TangentVectorType>(x.data(), model.nv, 1) = tau;
275 
276  CPPAD_TESTVECTOR(Scalar) ddq = ad_fun.Forward(0, x);
277  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(ddq.data(), model.nv, 1).isApprox(data.ddq));
278 
279  CPPAD_TESTVECTOR(Scalar) ddq_dtau = ad_fun.Jacobian(x);
280  Data::MatrixXs ddq_dtau_mat = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(
281  ddq_dtau.data(), model.nv, model.nv);
282  BOOST_CHECK(ddq_dtau_mat.isApprox(aba_partial_dtau));
283  BOOST_CHECK(ddq_dtau_mat.isApprox(data.Minv));
284  }
285 }
286 
287 BOOST_AUTO_TEST_SUITE_END()
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::Convention::WORLD
@ WORLD
kinematics.hpp
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
Definition: derivatives.cpp:28
rnea-derivatives.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::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
pinocchio::computeMinverse
const DataTpl< Scalar, Options, JointCollectionTpl >::RowMatrixXs & computeMinverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the inverse of the joint space inertia matrix using Articulated Body formulation.
aba.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
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
autodiff-rnea.dtau_dv
dtau_dv
Definition: autodiff-rnea.py:29
cartpole.v
v
Definition: cartpole.py:153
data.hpp
x
x
q
q
cppad.hpp
forward-dynamics-derivatives.ddq_dq
ddq_dq
Definition: forward-dynamics-derivatives.py:33
autodiff-rnea.dtau_da
dtau_da
Definition: autodiff-rnea.py:29
a
Vec3f a
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::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...
autodiff-rnea.dtau_dq
dtau_dq
Definition: autodiff-rnea.py:29
Y
Y
sample-models.hpp
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
crba.hpp

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