unittest/rnea.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4 
5 /*
6  * Unittest of the RNE algorithm. The code simply test that the algorithm does
7  * not cause any serious errors. The numerical values are not cross validated
8  * in any way.
9  *
10  */
11 
23 
24 #include <iostream>
25 
26 // #define __SSE3__
27 #include <fenv.h>
28 
29 #ifdef __SSE3__
30  #include <pmmintrin.h>
31 #endif
32 
33 #include <boost/test/unit_test.hpp>
34 #include <boost/utility/binary.hpp>
35 
36 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
37 
39 {
40 #ifdef __SSE3__
41  _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
42 #endif
43  using namespace Eigen;
44  using namespace pinocchio;
45 
48 
49  model.lowerPositionLimit.head<3>().fill(-1.);
50  model.upperPositionLimit.head<3>().fill(1.);
51 
53  data.v[0] = Motion::Zero();
54  data.a[0] = -model.gravity;
55 
56  VectorXd q = randomConfiguration(model);
57  VectorXd v = VectorXd::Random(model.nv);
58  VectorXd a = VectorXd::Random(model.nv);
59 
60 #ifdef NDEBUG
61  const size_t NBT = 10000;
62 #else
63  const size_t NBT = 1;
64  std::cout << "(the time score in debug mode is not relevant) ";
65 #endif
66 
68  timer.tic();
69  SMOOTH(NBT)
70  {
71  rnea(model, data, q, v, a);
72  }
73  timer.toc(std::cout, NBT);
74 }
75 
76 BOOST_AUTO_TEST_CASE(test_nle_vs_rnea)
77 {
78  using namespace Eigen;
79  using namespace pinocchio;
80 
83 
84  model.lowerPositionLimit.head<3>().fill(-1.);
85  model.upperPositionLimit.head<3>().fill(1.);
86 
87  pinocchio::Data data_nle(model);
88  pinocchio::Data data_rnea(model);
89 
90  VectorXd q = randomConfiguration(model);
91  VectorXd v = VectorXd::Random(model.nv);
92 
93  VectorXd tau_nle(VectorXd::Zero(model.nv));
94  VectorXd tau_rnea(VectorXd::Zero(model.nv));
95 
96  // -------
97  q.tail(model.nq - 7).setZero();
98  v.setZero();
99 
100  tau_nle = nonLinearEffects(model, data_nle, q, v);
101  tau_rnea = rnea(model, data_rnea, q, v, VectorXd::Zero(model.nv));
102 
103  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
104 
105  // -------
106  q.tail(model.nq - 7).setZero();
107  v.setOnes();
108 
109  tau_nle = nonLinearEffects(model, data_nle, q, v);
110  tau_rnea = rnea(model, data_rnea, q, v, VectorXd::Zero(model.nv));
111 
112  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
113 
114  // -------
115  q.tail(model.nq - 7).setOnes();
116  v.setOnes();
117 
118  tau_nle = nonLinearEffects(model, data_nle, q, v);
119  tau_rnea = rnea(model, data_rnea, q, v, VectorXd::Zero(model.nv));
120 
121  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
122 
123  // -------
125  v.setRandom();
126 
127  tau_nle = nonLinearEffects(model, data_nle, q, v);
128  tau_rnea = rnea(model, data_rnea, q, v, VectorXd::Zero(model.nv));
129 
130  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
131 }
132 
133 BOOST_AUTO_TEST_CASE(test_rnea_with_fext)
134 {
135  using namespace Eigen;
136  using namespace pinocchio;
137 
138  Model model;
140 
141  model.lowerPositionLimit.head<3>().fill(-1.);
142  model.upperPositionLimit.head<3>().fill(1.);
143 
144  Data data_rnea_fext(model);
145  Data data_rnea(model);
146 
147  VectorXd q = randomConfiguration(model);
148 
149  VectorXd v(VectorXd::Random(model.nv));
150  VectorXd a(VectorXd::Random(model.nv));
151 
152  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext(model.joints.size(), Force::Zero());
153 
154  JointIndex rf = model.getJointId("rleg6_joint");
155  Force Frf = Force::Random();
156  fext[rf] = Frf;
157  JointIndex lf = model.getJointId("lleg6_joint");
158  Force Flf = Force::Random();
159  fext[lf] = Flf;
160 
161  rnea(model, data_rnea, q, v, a);
162  VectorXd tau_ref(data_rnea.tau);
163  Data::Matrix6x Jrf(Data::Matrix6x::Zero(6, model.nv));
164  computeJointJacobian(model, data_rnea, q, rf, Jrf);
165  tau_ref -= Jrf.transpose() * Frf.toVector();
166 
167  Data::Matrix6x Jlf(Data::Matrix6x::Zero(6, model.nv));
168  computeJointJacobian(model, data_rnea, q, lf, Jlf);
169  tau_ref -= Jlf.transpose() * Flf.toVector();
170 
171  rnea(model, data_rnea_fext, q, v, a, fext);
172 
173  BOOST_CHECK(tau_ref.isApprox(data_rnea_fext.tau));
174 }
175 
176 BOOST_AUTO_TEST_CASE(test_rnea_with_armature)
177 {
178  using namespace Eigen;
179  using namespace pinocchio;
180 
181  Model model;
183  model.armature = VectorXd::Random(model.nv) + VectorXd::Ones(model.nv);
184 
185  model.lowerPositionLimit.head<3>().fill(-1.);
186  model.upperPositionLimit.head<3>().fill(1.);
187 
188  Data data(model);
189  Data data_ref(model);
190 
191  VectorXd q = randomConfiguration(model);
192  VectorXd v(VectorXd::Random(model.nv));
193  VectorXd a(VectorXd::Random(model.nv));
194 
195  crba(model, data_ref, q, Convention::WORLD);
196  data_ref.M.triangularView<StrictlyLower>() =
197  data_ref.M.transpose().triangularView<StrictlyLower>();
198  const VectorXd nle = nonLinearEffects(model, data_ref, q, v);
199 
200  const VectorXd tau_ref = data_ref.M * a + nle;
201 
202  rnea(model, data, q, v, a);
203  BOOST_CHECK(tau_ref.isApprox(data.tau));
204 }
205 
206 BOOST_AUTO_TEST_CASE(test_compute_gravity)
207 {
208  using namespace Eigen;
209  using namespace pinocchio;
210 
211  Model model;
213 
214  model.lowerPositionLimit.head<3>().fill(-1.);
215  model.upperPositionLimit.head<3>().fill(1.);
216 
217  Data data_rnea(model);
218  Data data(model);
219 
220  VectorXd q = randomConfiguration(model);
221 
222  rnea(model, data_rnea, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv));
224 
225  BOOST_CHECK(data_rnea.tau.isApprox(data.g));
226 
227  // Compare with Jcom
228  crba(model, data_rnea, q, Convention::WORLD);
229  Data::Matrix3x Jcom = getJacobianComFromCrba(model, data_rnea);
230 
231  VectorXd g_ref(-data_rnea.mass[0] * Jcom.transpose() * Model::gravity981);
232 
233  BOOST_CHECK(g_ref.isApprox(data.g));
234 }
235 
236 BOOST_AUTO_TEST_CASE(test_compute_static_torque)
237 {
238  using namespace Eigen;
239  using namespace pinocchio;
240 
241  Model model;
243 
244  model.lowerPositionLimit.head<3>().fill(-1.);
245  model.upperPositionLimit.head<3>().fill(1.);
246 
247  Data data_rnea(model);
248  Data data(model);
249 
250  VectorXd q = randomConfiguration(model);
251 
252  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
253  ForceVector fext((size_t)model.njoints);
254  for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
255  (*it).setRandom();
256 
257  rnea(model, data_rnea, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv), fext);
258  computeStaticTorque(model, data, q, fext);
259 
260  BOOST_CHECK(data_rnea.tau.isApprox(data.tau));
261 
262  // Compare with Jcom + Jacobian of joint
263  crba(model, data_rnea, q, Convention::WORLD);
264  Data::Matrix3x Jcom = getJacobianComFromCrba(model, data_rnea);
265 
266  VectorXd static_torque_ref = -data_rnea.mass[0] * Jcom.transpose() * Model::gravity981;
267  computeJointJacobians(model, data_rnea, q);
268 
269  Data::Matrix6x J_local(6, model.nv);
270  for (JointIndex joint_id = 1; joint_id < (JointIndex)(model.njoints); ++joint_id)
271  {
272  J_local.setZero();
273  getJointJacobian(model, data_rnea, joint_id, LOCAL, J_local);
274  static_torque_ref -= J_local.transpose() * fext[joint_id].toVector();
275  }
276 
277  BOOST_CHECK(static_torque_ref.isApprox(data.tau));
278 }
279 
280 BOOST_AUTO_TEST_CASE(test_compute_coriolis)
281 {
282  using namespace Eigen;
283  using namespace pinocchio;
284 
285  const double prec = Eigen::NumTraits<double>::dummy_precision();
286 
287  Model model;
289 
290  model.lowerPositionLimit.head<3>().fill(-1.);
291  model.upperPositionLimit.head<3>().fill(1.);
292 
293  Data data_ref(model);
294  Data data(model);
295 
296  VectorXd q = randomConfiguration(model);
297 
298  VectorXd v(VectorXd::Random(model.nv));
299  computeCoriolisMatrix(model, data, q, Eigen::VectorXd::Zero(model.nv));
300  BOOST_CHECK(data.C.isZero(prec));
301 
302  model.gravity.setZero();
303  rnea(model, data_ref, q, v, VectorXd::Zero(model.nv));
306 
307  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
308  BOOST_CHECK(data.J.isApprox(data_ref.J));
309 
310  VectorXd tau = data.C * v;
311  BOOST_CHECK(tau.isApprox(data_ref.tau, prec));
312 
313  dccrba(model, data_ref, q, v);
314  crba(model, data_ref, q, Convention::WORLD);
315 
316  const Data::Vector3 & com = data_ref.com[0];
317  Motion vcom(data_ref.vcom[0], Data::Vector3::Zero());
318  SE3 cM1(data.oMi[1]);
319  cM1.translation() -= com;
320 
321  BOOST_CHECK((cM1.toDualActionMatrix() * data_ref.M.topRows<6>()).isApprox(data_ref.Ag, prec));
322 
323  Force dh_ref = cM1.act(Force(data_ref.tau.head<6>()));
324  Force dh(data_ref.dAg * v);
325  BOOST_CHECK(dh.isApprox(dh_ref, prec));
326 
327  {
328  Data data_ref(model), data_ref_plus(model);
329  Eigen::MatrixXd dM(data.C + data.C.transpose());
330 
331  const double alpha = 1e-8;
332  Eigen::VectorXd q_plus(model.nq);
333  q_plus = integrate(model, q, alpha * v);
334 
335  crba(model, data_ref, q, Convention::WORLD);
336  data_ref.M.triangularView<Eigen::StrictlyLower>() =
337  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
338  crba(model, data_ref_plus, q_plus, Convention::WORLD);
339  data_ref_plus.M.triangularView<Eigen::StrictlyLower>() =
340  data_ref_plus.M.transpose().triangularView<Eigen::StrictlyLower>();
341 
342  Eigen::MatrixXd dM_ref = (data_ref_plus.M - data_ref.M) / alpha;
343  BOOST_CHECK(dM.isApprox(dM_ref, sqrt(alpha)));
344  }
345 }
346 
347 BOOST_AUTO_TEST_SUITE_END()
pinocchio::DataTpl::M
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: multibody/data.hpp:199
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
PinocchioTicToc::toc
double toc()
Definition: timer.hpp:88
fwd.hpp
Eigen
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::DataTpl::dAg
Matrix6x dAg
Centroidal Momentum Matrix Time Variation.
Definition: multibody/data.hpp:290
pinocchio::DataTpl::mass
std::vector< Scalar > mass
Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint ....
Definition: multibody/data.hpp:444
pinocchio::DataTpl::dJ
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: multibody/data.hpp:363
pinocchio::SE3Tpl< context::Scalar, context::Options >
model.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::python::context::Force
ForceTpl< Scalar, Options > Force
Definition: bindings/python/context/generic.hpp:55
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::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...
pinocchio::computeJointJacobians
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame....
pinocchio::dccrba
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & dccrba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration...
pinocchio.shortcuts.nle
nle
Definition: shortcuts.py:13
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
timer.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::SE3Tpl::translation
ConstLinearRef translation() const
Definition: se3-base.hpp:52
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
center-of-mass.hpp
SMOOTH
#define SMOOTH(s)
Definition: timer.hpp:38
joint-configuration.hpp
reachable-workspace-with-collisions.alpha
float alpha
Definition: reachable-workspace-with-collisions.py:162
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: multibody/data.hpp:94
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::DataTpl::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: multibody/data.hpp:77
pinocchio::computeStaticTorque
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & computeStaticTorque(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const container::aligned_vector< ForceTpl< Scalar, Options >> &fext)
Computes the generalized static torque contribution of the Lagrangian dynamics:
pinocchio::getJointJacobian
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in one of the pinocchio::ReferenceFrame opt...
pinocchio::DataTpl::tau
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: multibody/data.hpp:173
cartpole.v
v
Definition: cartpole.py:153
data.hpp
pinocchio::ForceTpl
Definition: context/casadi.hpp:25
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_rnea)
Definition: unittest/rnea.cpp:38
pinocchio::DataTpl::Ag
Matrix6x Ag
Centroidal Momentum Matrix.
Definition: multibody/data.hpp:284
pinocchio::nonLinearEffects
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & nonLinearEffects(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the non-linear effects (Corriolis, centrifual and gravitationnal effects),...
q
q
inverse-kinematics-3d.it
int it
Definition: inverse-kinematics-3d.py:17
pinocchio::getJacobianComFromCrba
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix3x & getJacobianComFromCrba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Extracts both the jacobian of the center of mass (CoM), the total mass of the system and the CoM posi...
a
Vec3f a
static-contact-dynamics.tau_rnea
tau_rnea
Definition: static-contact-dynamics.py:176
PinocchioTicToc::US
@ US
Definition: timer.hpp:53
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: multibody/data.hpp:360
fill
fill
centroidal.hpp
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...
dcrba.dM
dM
Definition: dcrba.py:452
pinocchio::computeJointJacobian
void computeJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const JointIndex joint_id, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in the local frame of the joint and store t...
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
PINOCCHIO_ALIGNED_STD_VECTOR
#define PINOCCHIO_ALIGNED_STD_VECTOR(Type)
Definition: container/aligned-vector.hpp:11
sample-models.hpp
pinocchio::MotionTpl
Definition: context/casadi.hpp:27
append-urdf-model-with-another-model.joint_id
joint_id
Definition: append-urdf-model-with-another-model.py:34
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::computeCoriolisMatrix
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & computeCoriolisMatrix(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the Coriolis Matrix of the Lagrangian dynamics:
crba.hpp
PinocchioTicToc
Definition: timer.hpp:47
robot-wrapper-viewer.com
com
Definition: robot-wrapper-viewer.py:45
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::computeGeneralizedGravity
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & computeGeneralizedGravity(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the generalized gravity contribution of the Lagrangian dynamics:
PinocchioTicToc::tic
void tic()
Definition: timer.hpp:82
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50


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