cppad-algo-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/parsers/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); rnea_partial_dq.setZero();
61  Eigen::MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero();
62  Eigen::MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero();
63 
64  pinocchio::computeRNEADerivatives(model,data,q,v,a,
65  rnea_partial_dq,
66  rnea_partial_dv,
67  rnea_partial_da);
68 
69  rnea_partial_da.triangularView<Eigen::StrictlyLower>()
70  = rnea_partial_da.transpose().triangularView<Eigen::StrictlyLower>();
71 
72  typedef ADModel::ConfigVectorType ADConfigVectorType;
73  typedef ADModel::TangentVectorType ADTangentVectorType;
74 
75  ADConfigVectorType ad_q = q.cast<ADScalar>();
76  ADTangentVectorType ad_dq = ADTangentVectorType::Zero(model.nv);
77  ADTangentVectorType ad_v = v.cast<ADScalar>();
78  ADTangentVectorType ad_a = a.cast<ADScalar>();
79 
80  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> VectorXAD;
81  pinocchio::crba(model,data,q);
82  data.M.triangularView<Eigen::StrictlyLower>()
83  = data.M.transpose().triangularView<Eigen::StrictlyLower>();
84 
85  Data::TangentVectorType tau = pinocchio::rnea(model,data,q,v,a);
86 
87  // dtau_dq
88  {
89  CppAD::Independent(ad_dq);
90  ADConfigVectorType ad_q_plus = pinocchio::integrate(ad_model,ad_q,ad_dq);
91  pinocchio::rnea(ad_model,ad_data,ad_q_plus,ad_v,ad_a);
92 
93  VectorXAD Y(model.nv);
94  Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.tau;
95 
96  CppAD::ADFun<Scalar> ad_fun(ad_dq,Y);
97 
98  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
99  Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1).setZero();
100 
101  CPPAD_TESTVECTOR(Scalar) tau = ad_fun.Forward(0,x);
102  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(tau.data(),model.nv,1).isApprox(data.tau));
103 
104  CPPAD_TESTVECTOR(Scalar) dtau_dq = ad_fun.Jacobian(x);
105  Data::MatrixXs dtau_dq_mat = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(dtau_dq.data(),model.nv,model.nv);
106  BOOST_CHECK(dtau_dq_mat.isApprox(rnea_partial_dq));
107  }
108 
109  // dtau_dv
110  {
111  CppAD::Independent(ad_v);
112  pinocchio::rnea(ad_model,ad_data,ad_q,ad_v,ad_a);
113 
114  VectorXAD Y(model.nv);
115  Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.tau;
116 
117  CppAD::ADFun<Scalar> ad_fun(ad_v,Y);
118 
119  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
120  Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1) = v;
121 
122  CPPAD_TESTVECTOR(Scalar) tau = ad_fun.Forward(0,x);
123  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(tau.data(),model.nv,1).isApprox(data.tau));
124 
125  CPPAD_TESTVECTOR(Scalar) dtau_dv = ad_fun.Jacobian(x);
126  Data::MatrixXs dtau_dv_mat = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(dtau_dv.data(),model.nv,model.nv);
127  BOOST_CHECK(dtau_dv_mat.isApprox(rnea_partial_dv));
128  }
129 
130  // dtau_da
131  {
132  CppAD::Independent(ad_a);
133  pinocchio::rnea(ad_model,ad_data,ad_q,ad_v,ad_a);
134 
135  VectorXAD Y(model.nv);
136  Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.tau;
137 
138  CppAD::ADFun<Scalar> ad_fun(ad_a,Y);
139 
140  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
141  Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1) = a;
142 
143  CPPAD_TESTVECTOR(Scalar) tau = ad_fun.Forward(0,x);
144  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(tau.data(),model.nv,1).isApprox(data.tau));
145 
146  CPPAD_TESTVECTOR(Scalar) dtau_da = ad_fun.Jacobian(x);
147  Data::MatrixXs dtau_da_mat = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(dtau_da.data(),model.nv,model.nv);
148  BOOST_CHECK(dtau_da_mat.isApprox(rnea_partial_da));
149  BOOST_CHECK(dtau_da_mat.isApprox(data.M));
150  }
151 
152 }
153 
154 BOOST_AUTO_TEST_CASE(test_aba_derivatives)
155 {
156  using CppAD::AD;
157  using CppAD::NearEqual;
158 
159  typedef double Scalar;
160  typedef AD<Scalar> ADScalar;
161 
162  typedef pinocchio::ModelTpl<Scalar> Model;
163  typedef Model::Data Data;
164 
165  typedef pinocchio::ModelTpl<ADScalar> ADModel;
166  typedef ADModel::Data ADData;
167 
168  Model model;
169  pinocchio::buildModels::humanoidRandom(model);
170  model.lowerPositionLimit.head<3>().fill(-1.);
171  model.upperPositionLimit.head<3>().fill(1.);
172  Data data(model);
173 
174  ADModel ad_model = model.cast<ADScalar>();
175  ADData ad_data(ad_model);
176 
177  // Sample random configuration
178  typedef Model::ConfigVectorType ConfigVectorType;
179  typedef Model::TangentVectorType TangentVectorType;
180  ConfigVectorType q(model.nq);
181  q = pinocchio::randomConfiguration(model);
182 
183  TangentVectorType v(TangentVectorType::Random(model.nv));
184  TangentVectorType tau(TangentVectorType::Random(model.nv));
185 
186  Eigen::MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
187  Eigen::MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
188  Eigen::MatrixXd aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
189 
190  pinocchio::computeABADerivatives(model,data,q,v,tau,
191  aba_partial_dq,
192  aba_partial_dv,
193  aba_partial_dtau);
194 
195  aba_partial_dtau.triangularView<Eigen::StrictlyLower>()
196  = aba_partial_dtau.transpose().triangularView<Eigen::StrictlyLower>();
197 
198  typedef ADModel::ConfigVectorType ADConfigVectorType;
199  typedef ADModel::TangentVectorType ADTangentVectorType;
200 
201  ADConfigVectorType ad_q = q.cast<ADScalar>();
202  ADTangentVectorType ad_dq = ADTangentVectorType::Zero(model.nv);
203  ADTangentVectorType ad_v = v.cast<ADScalar>();
204  ADTangentVectorType ad_tau = tau.cast<ADScalar>();
205 
206  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> VectorXAD;
207  pinocchio::computeMinverse(model,data,q);
208  data.Minv.triangularView<Eigen::StrictlyLower>()
209  = data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
210 
211  Data::TangentVectorType ddq = pinocchio::aba(model,data,q,v,tau);
212 
213  // dddq_dq
214  {
215  CppAD::Independent(ad_dq);
216  ADConfigVectorType ad_q_plus = pinocchio::integrate(ad_model,ad_q,ad_dq);
217  pinocchio::aba(ad_model,ad_data,ad_q_plus,ad_v,ad_tau);
218 
219  VectorXAD Y(model.nv);
220  Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.ddq;
221 
222  CppAD::ADFun<Scalar> ad_fun(ad_dq,Y);
223 
224  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
225  Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1).setZero();
226 
227  CPPAD_TESTVECTOR(Scalar) ddq = ad_fun.Forward(0,x);
228  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(ddq.data(),model.nv,1).isApprox(data.ddq));
229 
230  CPPAD_TESTVECTOR(Scalar) ddq_dq = ad_fun.Jacobian(x);
231  Data::MatrixXs ddq_dq_mat = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(ddq_dq.data(),model.nv,model.nv);
232  BOOST_CHECK(ddq_dq_mat.isApprox(aba_partial_dq));
233  }
234 
235  // dddq_dv
236  {
237  CppAD::Independent(ad_v);
238  pinocchio::aba(ad_model,ad_data,ad_q,ad_v,ad_tau);
239 
240  VectorXAD Y(model.nv);
241  Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.ddq;
242 
243  CppAD::ADFun<Scalar> ad_fun(ad_v,Y);
244 
245  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
246  Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1) = v;
247 
248  CPPAD_TESTVECTOR(Scalar) ddq = ad_fun.Forward(0,x);
249  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(ddq.data(),model.nv,1).isApprox(data.ddq));
250 
251  CPPAD_TESTVECTOR(Scalar) ddq_dv = ad_fun.Jacobian(x);
252  Data::MatrixXs ddq_dv_mat = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(ddq_dv.data(),model.nv,model.nv);
253  BOOST_CHECK(ddq_dv_mat.isApprox(aba_partial_dv));
254  }
255 
256  // dddq_da
257  {
258  CppAD::Independent(ad_tau);
259  pinocchio::aba(ad_model,ad_data,ad_q,ad_v,ad_tau);
260 
261  VectorXAD Y(model.nv);
262  Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.ddq;
263 
264  CppAD::ADFun<Scalar> ad_fun(ad_tau,Y);
265 
266  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
267  Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1) = tau;
268 
269  CPPAD_TESTVECTOR(Scalar) ddq = ad_fun.Forward(0,x);
270  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(ddq.data(),model.nv,1).isApprox(data.ddq));
271 
272  CPPAD_TESTVECTOR(Scalar) ddq_dtau = ad_fun.Jacobian(x);
273  Data::MatrixXs ddq_dtau_mat = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(ddq_dtau.data(),model.nv,model.nv);
274  BOOST_CHECK(ddq_dtau_mat.isApprox(aba_partial_dtau));
275  BOOST_CHECK(ddq_dtau_mat.isApprox(data.Minv));
276  }
277 
278 }
279 
280 BOOST_AUTO_TEST_SUITE_END()
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)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
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)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
pinocchio::Model
ModelTpl< double > Model
Definition: src/multibody/fwd.hpp:35
q
q
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.
pinocchio::rnea
void rnea(const int num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &a, const Eigen::MatrixBase< TangentVectorPool3 > &tau)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
Definition: parallel/rnea.hpp:32
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:54
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:224
fill
fill
pinocchio::python::Scalar
SE3::Scalar Scalar
Definition: conversions.cpp:13
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...
isApprox
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
x
x
a
Vec3f a
pinocchio::Data
DataTpl< double > Data
Definition: src/multibody/fwd.hpp:36
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
Definition: cppad-algo-derivatives.cpp:28
setZero
void setZero(std::vector< MatType, Eigen::aligned_allocator< MatType > > &Ms)
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.
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
Author(s):
autogenerated on Fri Jun 23 2023 02:38:29