cppad-algo.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/aba.hpp"
15 #include "pinocchio/algorithm/joint-configuration.hpp"
16 
17 #include "pinocchio/parsers/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_mass_matrix)
27 {
28  using CppAD::AD;
29  using CppAD::NearEqual;
30 
31  typedef double Scalar;
32  typedef AD<Scalar> ADScalar;
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  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> VectorXAD;
66  pinocchio::crba(model,data,q);
67  data.M.triangularView<Eigen::StrictlyLower>()
68  = data.M.transpose().triangularView<Eigen::StrictlyLower>();
69 
70  Data::TangentVectorType tau = pinocchio::rnea(model,data,q,v,a);
71 
72  {
73  CppAD::Independent(ad_a);
74  pinocchio::rnea(ad_model,ad_data,ad_q,ad_v,ad_a);
75 
76  VectorXAD Y(model.nv);
77  Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.tau;
78 
79  CppAD::ADFun<Scalar> ad_fun(ad_a,Y);
80 
81  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
82  Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1) = a;
83 
84  CPPAD_TESTVECTOR(Scalar) tau = ad_fun.Forward(0,x);
85  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(tau.data(),model.nv,1).isApprox(data.tau));
86 
87  CPPAD_TESTVECTOR(Scalar) dtau_da = ad_fun.Jacobian(x);
88  Data::MatrixXs M = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(dtau_da.data(),model.nv,model.nv);
89  BOOST_CHECK(M.isApprox(data.M));
90 
91  }
92 
93  ADTangentVectorType ad_tau = tau.cast<ADScalar>();
94 
95  pinocchio::computeMinverse(model,data,q);
96  data.Minv.triangularView<Eigen::StrictlyLower>()
97  = data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
98 
99  pinocchio::aba(model,data,q,v,tau);
100  {
101  CppAD::Independent(ad_tau);
102  pinocchio::aba(ad_model,ad_data,ad_q,ad_v,ad_tau);
103 
104  VectorXAD Y(model.nv);
105  Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.ddq;
106 
107  CppAD::ADFun<Scalar> ad_fun(ad_tau,Y);
108 
109  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
110  Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1) = tau;
111 
112  CPPAD_TESTVECTOR(Scalar) ddq = ad_fun.Forward(0,x);
113  BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(ddq.data(),model.nv,1).isApprox(a));
114 
115  CPPAD_TESTVECTOR(Scalar) dddq_da = ad_fun.Jacobian(x);
116  Data::MatrixXs Minv = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(dddq_da.data(),model.nv,model.nv);
117  BOOST_CHECK(Minv.isApprox(data.Minv));
118 
119  }
120 
121 }
122 
123 BOOST_AUTO_TEST_CASE(test_kinematics_jacobian)
124 {
125  using CppAD::AD;
126  using CppAD::NearEqual;
127 
128  typedef double Scalar;
129  typedef AD<Scalar> ADScalar;
130 
131  typedef pinocchio::ModelTpl<Scalar> Model;
132  typedef Model::Data Data;
133  typedef Model::Motion Motion;
134 
135  typedef pinocchio::ModelTpl<ADScalar> ADModel;
136  typedef ADModel::Data ADData;
137 
138  Model model;
139  pinocchio::buildModels::humanoidRandom(model);
140  model.lowerPositionLimit.head<3>().fill(-1.);
141  model.upperPositionLimit.head<3>().fill(1.);
142  Data data(model);
143 
144  ADModel ad_model = model.cast<ADScalar>();
145  ADData ad_data(ad_model);
146 
147  // Sample random configuration
148  typedef Model::ConfigVectorType ConfigVectorType;
149  typedef Model::TangentVectorType TangentVectorType;
150  ConfigVectorType q(model.nq);
151  q = pinocchio::randomConfiguration(model);
152 
153  TangentVectorType v(TangentVectorType::Random(model.nv));
154  TangentVectorType a(TangentVectorType::Random(model.nv));
155 
156  typedef ADModel::ConfigVectorType ADConfigVectorType;
157  typedef ADModel::TangentVectorType ADTangentVectorType;
158 
159  ADConfigVectorType ad_q = q.cast<ADScalar>();
160  ADTangentVectorType ad_v = v.cast<ADScalar>();
161  ADTangentVectorType ad_a = a.cast<ADScalar>();
162 
163  // Test if the jacobian of a precise link is given by dv_link/dv
164  const std::string joint_name = "rarm5_joint";
165  Model::JointIndex joint_id = model.getJointId(joint_name);
166  pinocchio::computeJointJacobiansTimeVariation(model,data,q,v);
167  pinocchio::forwardKinematics(model,data,q,v,a);
168 
169  Data::Matrix6x J_local(6,model.nv), J_global(6,model.nv);
170  J_local.setZero(); J_global.setZero();
171  Data::Matrix6x dJ_local(6,model.nv), dJ_global(6,model.nv);
172  dJ_local.setZero(); dJ_global.setZero();
173  pinocchio::getJointJacobian(model,data,joint_id,pinocchio::LOCAL,J_local);
174  pinocchio::getJointJacobian(model,data,joint_id,pinocchio::WORLD,J_global);
175 
176  pinocchio::getJointJacobianTimeVariation(model,data,joint_id,pinocchio::LOCAL,dJ_local);
177  pinocchio::getJointJacobianTimeVariation(model,data,joint_id,pinocchio::WORLD,dJ_global);
178 
179  const ADData::Motion & v_local = ad_data.v[joint_id];
180  const ADData::Motion & a_local = ad_data.a[joint_id];
181 
182  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> VectorXAD;
183 
184  {
185  CppAD::Independent(ad_v);
186  pinocchio::forwardKinematics(ad_model,ad_data,ad_q,ad_v,ad_a);
187 
188  const ADData::Motion v_global = ad_data.oMi[joint_id].act(v_local);
189 
190  VectorXAD Y(6*3);
191  Eigen::DenseIndex current_id = 0;
192  for(Eigen::DenseIndex k = 0; k < 3; ++k)
193  {
194  Y[current_id+k+Motion::LINEAR] = v_local.linear()[k];
195  Y[current_id+k+Motion::ANGULAR] = v_local.angular()[k];
196  }
197  current_id += 6;
198 
199  for(Eigen::DenseIndex k = 0; k < 3; ++k)
200  {
201  Y[current_id+k+Motion::LINEAR] = v_global.linear()[k];
202  Y[current_id+k+Motion::ANGULAR] = v_global.angular()[k];
203  }
204  current_id += 6;
205 
206  for(Eigen::DenseIndex k = 0; k < 3; ++k)
207  {
208  Y[current_id+k+Motion::LINEAR] = a_local.linear()[k];
209  Y[current_id+k+Motion::ANGULAR] = a_local.angular()[k];
210  }
211  current_id += 6;
212 
213  CppAD::ADFun<Scalar> vjoint(ad_v,Y);
214 
215  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
216  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
217  {
218  x[(size_t)k] = v[k];
219  }
220 
221  CPPAD_TESTVECTOR(Scalar) y = vjoint.Forward(0,x);
222  Scalar * y_ptr = y.data();
223  BOOST_CHECK(data.v[joint_id].isApprox(Motion(Eigen::Map<Motion::Vector6>(y_ptr))));
224  y_ptr += 6;
225  BOOST_CHECK(data.oMi[joint_id].act(data.v[joint_id]).isApprox(Motion(Eigen::Map<Motion::Vector6>(y_ptr))));
226  y_ptr += 6;
227  BOOST_CHECK(data.a[joint_id].isApprox(Motion(Eigen::Map<Motion::Vector6>(y_ptr))));
228  y_ptr += 6;
229 
230  CPPAD_TESTVECTOR(Scalar) dY_dv = vjoint.Jacobian(x);
231 
232  Scalar * dY_dv_ptr = dY_dv.data();
233  Data::Matrix6x ad_J_local = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::Matrix6x)>(dY_dv_ptr,6,model.nv);
234  dY_dv_ptr += ad_J_local.size();
235  Data::Matrix6x ad_J_global = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::Matrix6x)>(dY_dv_ptr,6,model.nv);
236  dY_dv_ptr += ad_J_global.size();
237 
238  BOOST_CHECK(ad_J_local.isApprox(J_local));
239  BOOST_CHECK(ad_J_global.isApprox(J_global));
240  }
241 
242  {
243  CppAD::Independent(ad_a);
244  pinocchio::forwardKinematics(ad_model,ad_data,ad_q,ad_v,ad_a);
245 
246  VectorXAD Y(6*2);
247  Eigen::DenseIndex current_id = 0;
248  for(Eigen::DenseIndex k = 0; k < 3; ++k)
249  {
250  Y[current_id+k+Motion::LINEAR] = v_local.linear()[k];
251  Y[current_id+k+Motion::ANGULAR] = v_local.angular()[k];
252  }
253  current_id += 6;
254 
255  for(Eigen::DenseIndex k = 0; k < 3; ++k)
256  {
257  Y[current_id+k+Motion::LINEAR] = a_local.linear()[k];
258  Y[current_id+k+Motion::ANGULAR] = a_local.angular()[k];
259  }
260  current_id += 6;
261 
262  CppAD::ADFun<Scalar> ajoint(ad_a,Y);
263 
264  CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
265  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
266  {
267  x[(size_t)k] = a[k];
268  }
269 
270  CPPAD_TESTVECTOR(Scalar) y = ajoint.Forward(0,x);
271  Scalar * y_ptr = y.data()+6;
272  BOOST_CHECK(data.a[joint_id].isApprox(Motion(Eigen::Map<Motion::Vector6>(y_ptr))));
273  y_ptr += 6;
274 
275  CPPAD_TESTVECTOR(Scalar) dY_da = ajoint.Jacobian(x);
276 
277  Scalar * dY_da_ptr = dY_da.data();
278  Data::Matrix6x ad_dv_da = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::Matrix6x)>(dY_da_ptr,6,model.nv);
279  dY_da_ptr += ad_dv_da.size();
280  Data::Matrix6x ad_J_local = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::Matrix6x)>(dY_da_ptr,6,model.nv);
281  dY_da_ptr += ad_J_local.size();
282 
283  BOOST_CHECK(ad_dv_da.isZero());
284  BOOST_CHECK(ad_J_local.isApprox(J_local));
285  }
286 }
287 
288 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::WORLD
The WORLD frame convention corresponds to the frame concident with the Universe/Inertial frame but mo...
Definition: src/multibody/fwd.hpp:46
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
y
y
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::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
pinocchio::getJointJacobian
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame or i...
fill
fill
pinocchio::python::Scalar
SE3::Scalar Scalar
Definition: conversions.cpp:13
pinocchio::forwardKinematics
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
isApprox
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
x
x
pinocchio::LOCAL
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
Definition: src/multibody/fwd.hpp:47
pinocchio::Motion
MotionTpl< double, 0 > Motion
Definition: src/spatial/fwd.hpp:57
pinocchio::computeJointJacobiansTimeVariation
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobiansTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the full model Jacobian variations with respect to time. It corresponds to dJ/dt which depen...
Matrix6x
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6x
Definition: joint-mimic.cpp:15
a
Vec3f a
pinocchio::Data
DataTpl< double > Data
Definition: src/multibody/fwd.hpp:36
pinocchio::getJointJacobianTimeVariation
void getJointJacobianTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6Like > &dJ)
Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (r...
pinocchio::JointIndex
Index JointIndex
Definition: src/multibody/fwd.hpp:28
M
M
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_mass_matrix)
Definition: cppad-algo.cpp:26
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