Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2021 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/spatial/fwd.hpp" |
6 |
|
|
#include "pinocchio/spatial/se3.hpp" |
7 |
|
|
#include "pinocchio/multibody/visitor.hpp" |
8 |
|
|
#include "pinocchio/multibody/model.hpp" |
9 |
|
|
#include "pinocchio/multibody/data.hpp" |
10 |
|
|
#include "pinocchio/algorithm/crba.hpp" |
11 |
|
|
#include "pinocchio/algorithm/centroidal.hpp" |
12 |
|
|
#include "pinocchio/algorithm/rnea.hpp" |
13 |
|
|
#include "pinocchio/algorithm/jacobian.hpp" |
14 |
|
|
#include "pinocchio/algorithm/compute-all-terms.hpp" |
15 |
|
|
#include "pinocchio/multibody/sample-models.hpp" |
16 |
|
|
#include "pinocchio/utils/timer.hpp" |
17 |
|
|
|
18 |
|
|
#include <boost/test/unit_test.hpp> |
19 |
|
|
|
20 |
|
|
using namespace pinocchio; |
21 |
|
|
|
22 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
23 |
|
|
|
24 |
|
✗ |
void run_test(const Model & model, const Eigen::VectorXd & q, const Eigen::VectorXd & v) |
25 |
|
|
{ |
26 |
|
✗ |
pinocchio::Data data(model); |
27 |
|
✗ |
pinocchio::Data data_other(model); |
28 |
|
|
|
29 |
|
✗ |
computeAllTerms(model, data, q, v); |
30 |
|
|
|
31 |
|
✗ |
nonLinearEffects(model, data_other, q, v); |
32 |
|
✗ |
crba(model, data_other, q, Convention::WORLD); |
33 |
|
✗ |
getJacobianComFromCrba(model, data_other); |
34 |
|
✗ |
computeJointJacobiansTimeVariation(model, data_other, q, v); |
35 |
|
✗ |
centerOfMass(model, data_other, q, v, true); |
36 |
|
✗ |
computeKineticEnergy(model, data_other, q, v); |
37 |
|
✗ |
computePotentialEnergy(model, data_other, q); |
38 |
|
✗ |
dccrba(model, data_other, q, v); |
39 |
|
✗ |
computeGeneralizedGravity(model, data_other, q); |
40 |
|
|
|
41 |
|
✗ |
BOOST_CHECK(data.nle.isApprox(data_other.nle)); |
42 |
|
✗ |
BOOST_CHECK(Eigen::MatrixXd(data.M.triangularView<Eigen::Upper>()) |
43 |
|
|
.isApprox(Eigen::MatrixXd(data_other.M.triangularView<Eigen::Upper>()))); |
44 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_other.J)); |
45 |
|
✗ |
BOOST_CHECK(data.dJ.isApprox(data_other.dJ)); |
46 |
|
✗ |
BOOST_CHECK(data.Jcom.isApprox(data_other.Jcom)); |
47 |
|
✗ |
BOOST_CHECK(data.Ag.isApprox(data_other.Ag)); |
48 |
|
✗ |
BOOST_CHECK(data.dAg.isApprox(data_other.dAg)); |
49 |
|
✗ |
BOOST_CHECK(data.hg.isApprox(data_other.hg)); |
50 |
|
✗ |
BOOST_CHECK(data.Ig.isApprox(data_other.Ig)); |
51 |
|
✗ |
BOOST_CHECK(data.g.isApprox(data_other.g)); |
52 |
|
|
|
53 |
|
✗ |
for (int k = 0; k < model.njoints; ++k) |
54 |
|
|
{ |
55 |
|
✗ |
BOOST_CHECK(data.com[(size_t)k].isApprox(data_other.com[(size_t)k])); |
56 |
|
✗ |
BOOST_CHECK(data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k])); |
57 |
|
✗ |
BOOST_CHECK_CLOSE(data.mass[(size_t)k], data_other.mass[(size_t)k], 1e-12); |
58 |
|
|
} |
59 |
|
|
|
60 |
|
✗ |
BOOST_CHECK_CLOSE(data.kinetic_energy, data_other.kinetic_energy, 1e-12); |
61 |
|
✗ |
BOOST_CHECK_CLOSE(data.potential_energy, data_other.potential_energy, 1e-12); |
62 |
|
|
} |
63 |
|
|
|
64 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_against_algo) |
65 |
|
|
{ |
66 |
|
|
using namespace Eigen; |
67 |
|
|
|
68 |
|
✗ |
pinocchio::Model model; |
69 |
|
✗ |
buildModels::humanoidRandom(model); |
70 |
|
|
|
71 |
|
✗ |
VectorXd q(VectorXd::Random(model.nq)); |
72 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
73 |
|
|
|
74 |
|
|
// ------- |
75 |
|
✗ |
q.setZero(); |
76 |
|
✗ |
v.setZero(); |
77 |
|
|
|
78 |
|
✗ |
run_test(model, q, v); |
79 |
|
|
|
80 |
|
|
// ------- |
81 |
|
✗ |
q.setZero(); |
82 |
|
✗ |
v.setOnes(); |
83 |
|
|
|
84 |
|
✗ |
run_test(model, q, v); |
85 |
|
|
|
86 |
|
|
// ------- |
87 |
|
✗ |
q.setOnes(); |
88 |
|
✗ |
q.segment<4>(3).normalize(); |
89 |
|
✗ |
v.setOnes(); |
90 |
|
|
|
91 |
|
✗ |
run_test(model, q, v); |
92 |
|
|
|
93 |
|
|
// ------- |
94 |
|
✗ |
q.setRandom(); |
95 |
|
✗ |
q.segment<4>(3).normalize(); |
96 |
|
✗ |
v.setRandom(); |
97 |
|
|
|
98 |
|
✗ |
run_test(model, q, v); |
99 |
|
|
} |
100 |
|
|
|
101 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
102 |
|
|
|