GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright(c) 2018-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/algorithm/copy.hpp" |
||
6 |
#include "pinocchio/algorithm/kinematics.hpp" |
||
7 |
#include "pinocchio/algorithm/rnea.hpp" |
||
8 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
9 |
#include "pinocchio/parsers/sample-models.hpp" |
||
10 |
|||
11 |
#include <boost/test/unit_test.hpp> |
||
12 |
#include <boost/utility/binary.hpp> |
||
13 |
|||
14 |
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
||
15 |
|||
16 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_data_copy) |
17 |
{ |
||
18 |
using namespace Eigen; |
||
19 |
using namespace pinocchio; |
||
20 |
|||
21 |
✓✗ | 4 |
Model model; |
22 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
23 |
|||
24 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(100); |
25 |
✓✗✓✗ |
2 |
model.upperPositionLimit.segment<4>(3).setOnes(); |
26 |
✓✗✓✗ ✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>() = - model.upperPositionLimit.head<7>(); |
27 |
|||
28 |
✓✗ | 4 |
VectorXd q(model.nq); |
29 |
✓✗ | 2 |
q = randomConfiguration(model); |
30 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
31 |
✓✗✓✗ |
4 |
VectorXd a(VectorXd::Random(model.nv)); |
32 |
|||
33 |
✓✗✓✗ |
4 |
Data data_ref(model), data(model); |
34 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q,v,a); |
35 |
✓✗ | 2 |
rnea(model,data_ref,q,v,a); // for a_gf to be initialized |
36 |
|||
37 |
// Check zero order kinematic quantities |
||
38 |
✓✗ | 2 |
copy(model,data_ref,data,POSITION); |
39 |
✓✓ | 56 |
for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i) |
40 |
{ |
||
41 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oMi[i] == data_ref.oMi[i]); |
42 |
} |
||
43 |
|||
44 |
// Check first order kinematic quantities |
||
45 |
✓✗ | 2 |
copy(model,data_ref,data,VELOCITY); |
46 |
✓✓ | 56 |
for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i) |
47 |
{ |
||
48 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oMi[i] == data_ref.oMi[i]); |
49 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.v[i] == data_ref.v[i]); |
50 |
} |
||
51 |
|||
52 |
// Check second order kinematic quantities |
||
53 |
✓✗ | 2 |
copy(model,data_ref,data,ACCELERATION); |
54 |
✓✓ | 56 |
for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i) |
55 |
{ |
||
56 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oMi[i] == data_ref.oMi[i]); |
57 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.v[i] == data_ref.v[i]); |
58 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.a[i] == data_ref.a[i]); |
59 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.a_gf[i] == data_ref.a_gf[i]); |
60 |
} |
||
61 |
|||
62 |
2 |
} |
|
63 |
|||
64 |
BOOST_AUTO_TEST_SUITE_END() |
Generated by: GCOVR (Version 4.2) |