GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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/parsers/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 |
4 |
void run_test(const Model & model, const Eigen::VectorXd & q, const Eigen::VectorXd & v) |
|
25 |
{ |
||
26 |
✓✗ | 8 |
pinocchio::Data data(model); |
27 |
✓✗ | 8 |
pinocchio::Data data_other(model); |
28 |
|||
29 |
✓✗ | 4 |
computeAllTerms(model,data,q,v); |
30 |
|||
31 |
✓✗ | 4 |
nonLinearEffects(model,data_other,q,v); |
32 |
✓✗ | 4 |
crba(model,data_other,q); |
33 |
✓✗ | 4 |
getJacobianComFromCrba(model, data_other); |
34 |
✓✗ | 4 |
computeJointJacobiansTimeVariation(model,data_other,q,v); |
35 |
✓✗ | 4 |
centerOfMass(model, data_other, q, v, true); |
36 |
✓✗ | 4 |
computeKineticEnergy(model, data_other, q, v); |
37 |
✓✗ | 4 |
computePotentialEnergy(model, data_other, q); |
38 |
✓✗ | 4 |
dccrba(model, data_other, q, v); |
39 |
✓✗ | 4 |
computeGeneralizedGravity(model, data_other, q); |
40 |
|||
41 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK(data.nle.isApprox(data_other.nle)); |
42 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK(Eigen::MatrixXd(data.M.triangularView<Eigen::Upper>()) |
43 |
.isApprox(Eigen::MatrixXd(data_other.M.triangularView<Eigen::Upper>()))); |
||
44 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK(data.J.isApprox(data_other.J)); |
45 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK(data.dJ.isApprox(data_other.dJ)); |
46 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK(data.Jcom.isApprox(data_other.Jcom)); |
47 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK(data.Ag.isApprox(data_other.Ag)); |
48 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK(data.dAg.isApprox(data_other.dAg)); |
49 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK(data.hg.isApprox(data_other.hg)); |
50 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK(data.Ig.isApprox(data_other.Ig)); |
51 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK(data.g.isApprox(data_other.g)); |
52 |
|||
53 |
✓✓ | 116 |
for(int k=0; k<model.njoints; ++k) |
54 |
{ |
||
55 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
112 |
BOOST_CHECK(data.com[(size_t)k].isApprox(data_other.com[(size_t)k])); |
56 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
112 |
BOOST_CHECK(data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k])); |
57 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
112 |
BOOST_CHECK_CLOSE(data.mass[(size_t)k], data_other.mass[(size_t)k], 1e-12); |
58 |
} |
||
59 |
|||
60 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
4 |
BOOST_CHECK_CLOSE(data.kinetic_energy, data_other.kinetic_energy, 1e-12); |
61 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
4 |
BOOST_CHECK_CLOSE(data.potential_energy, data_other.potential_energy, 1e-12); |
62 |
4 |
} |
|
63 |
|||
64 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE( test_against_algo ) |
65 |
{ |
||
66 |
using namespace Eigen; |
||
67 |
|||
68 |
✓✗✓✗ |
4 |
pinocchio::Model model; buildModels::humanoidRandom(model); |
69 |
|||
70 |
✓✗✓✗ |
4 |
VectorXd q(VectorXd::Random(model.nq)); |
71 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
72 |
|||
73 |
// ------- |
||
74 |
✓✗ | 2 |
q.setZero(); |
75 |
✓✗ | 2 |
v.setZero(); |
76 |
|||
77 |
✓✗ | 2 |
run_test(model,q,v); |
78 |
|||
79 |
// ------- |
||
80 |
✓✗ | 2 |
q.setZero(); |
81 |
✓✗ | 2 |
v.setOnes(); |
82 |
|||
83 |
✓✗ | 2 |
run_test(model,q,v); |
84 |
|||
85 |
// ------- |
||
86 |
✓✗ | 2 |
q.setOnes(); |
87 |
✓✗✓✗ |
2 |
q.segment<4>(3).normalize(); |
88 |
✓✗ | 2 |
v.setOnes(); |
89 |
|||
90 |
✓✗ | 2 |
run_test(model,q,v); |
91 |
|||
92 |
// ------- |
||
93 |
✓✗ | 2 |
q.setRandom(); |
94 |
✓✗✓✗ |
2 |
q.segment<4>(3).normalize(); |
95 |
✓✗ | 2 |
v.setRandom(); |
96 |
|||
97 |
✓✗ | 2 |
run_test(model,q,v); |
98 |
2 |
} |
|
99 |
|||
100 |
BOOST_AUTO_TEST_SUITE_END() |
Generated by: GCOVR (Version 4.2) |