GCC Code Coverage Report

Directory: ./
File: tests/device-sync.cc
Date: 2025-05-04 12:09:19
Exec Total Coverage
Lines: 63 63 100.0%
Branches: 196 384 51.0%
Line Branch Exec Source
1 // Copyright (c) 2017, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #define BOOST_TEST_MODULE tdevice - sync
30
31 #include <boost/date_time/posix_time/posix_time.hpp>
32 #include <boost/test/unit_test.hpp>
33 #include <ctime>
34 #include <pinocchio/fwd.hpp>
35
36 namespace bpt = boost::posix_time;
37
38 #include <hpp/pinocchio/device-sync.hh>
39 #include <hpp/pinocchio/joint-collection.hh>
40 #include <hpp/pinocchio/simple-device.hh>
41 #include <hpp/pinocchio/util.hh>
42 #include <pinocchio/algorithm/joint-configuration.hpp>
43 #include <pinocchio/multibody/data.hpp>
44
45 using namespace hpp;
46 using namespace hpp::pinocchio;
47
48
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(single_thread) {
49
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 DevicePtr_t robot = unittest::makeDevice(unittest::HumanoidSimple);
50
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_REQUIRE(robot);
51
52
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 robot->numberDeviceData(2);
53 DeviceSync *d1, *d2;
54
55 2 DataPtr_t data1, data2;
56
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 d1 = new DeviceSync(robot);
57
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 d2 = new DeviceSync(robot);
58
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 data1 = d1->dataPtr();
59
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 data2 = d2->dataPtr();
60 // Delete in same order of creation: It should swap the internal datas.
61
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 delete d1;
62
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 delete d2;
63
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 d1 = new DeviceSync(robot);
64
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 d2 = new DeviceSync(robot);
65
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(data1, d2->dataPtr());
66
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(data2, d1->dataPtr());
67
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 delete d1;
68
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 delete d2;
69
70
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 size_type nCfg = robot->configSize();
71
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 robot->setDimensionExtraConfigSpace(3);
72
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(robot->configSize(), nCfg + 3);
73
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 d1 = new DeviceSync(robot);
74
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 d2 = new DeviceSync(robot);
75
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(d1->d().currentConfiguration_.size(), nCfg + 3);
76
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(d2->d().currentConfiguration_.size(), nCfg + 3);
77
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 delete d1;
78
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 delete d2;
79 2 }
80
81 typedef ::pinocchio::container::aligned_vector<SE3> SE3Vector_t;
82
83 1000 void compute_forward_kinematics(DevicePtr_t& device, const Configuration_t& q,
84 SE3Vector_t& res) {
85
1/2
✓ Branch 3 taken 1000 times.
✗ Branch 4 not taken.
 1000 device->currentConfiguration(q);
86 1000 device->computeForwardKinematics(JOINT_POSITION);
87
88 1000 res = device->data().oMi;
89 1000 }
90
91 1000 void compute_forward_kinematics_thread_safe(DevicePtr_t& device,
92 const Configuration_t& q,
93 SE3Vector_t& res) {
94
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 DeviceSync sync(device);
95
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 1000 sync.currentConfiguration(q);
96
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 sync.computeForwardKinematics(JOINT_POSITION);
97
98
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 1000 res = sync.data().oMi;
99 1000 }
100
101
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(check_synchronization) {
102
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 DevicePtr_t robot = unittest::makeDevice(unittest::CarLike);
103
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_REQUIRE(robot);
104
105 2 const size_type Nthreads = 4;
106
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 robot->numberDeviceData(4);
107
108 2 const std::size_t N = 1000;
109 // Generate N configurations
110
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<Configuration_t> qs(N);
111
2/2
✓ Branch 1 taken 1000 times.
✓ Branch 2 taken 1 times.
 2002 for (std::size_t i = 0; i < qs.size(); ++i) {
112
1/2
✓ Branch 3 taken 1000 times.
✗ Branch 4 not taken.
 2000 qs[i] = ::pinocchio::randomConfiguration(robot->model());
113 // BOOST_CHECK (qs[i].isFinite());
114
7/14
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1000 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1000 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1000 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1000 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1000 times.
 2000 BOOST_CHECK(!qs[i].hasNaN());
115 }
116
117 typedef std::vector<SE3Vector_t> Results_t;
118
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 4 Results_t res_single(qs.size()), res_multi(qs.size());
119
120
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 bpt::ptime t0 = bpt::microsec_clock::local_time();
121
122 // In a single threaded fashion, compute the joint positions
123
2/2
✓ Branch 1 taken 1000 times.
✓ Branch 2 taken 1 times.
 2002 for (std::size_t i = 0; i < qs.size(); ++i)
124
1/2
✓ Branch 3 taken 1000 times.
✗ Branch 4 not taken.
 2000 compute_forward_kinematics(robot, qs[i], res_single[i]);
125
126
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 bpt::ptime t1 = bpt::microsec_clock::local_time();
127
9/18
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
 2 BOOST_TEST_MESSAGE("1 thread: " << (t1 - t0).total_milliseconds() << "ms");
128
129
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 t0 = bpt::microsec_clock::local_time();
130 // In a multi threaded fashion, compute the joint positions
131 2 #pragma omp parallel for
132 for (std::size_t i = 0; i < qs.size(); ++i)
133 compute_forward_kinematics_thread_safe(robot, qs[i], res_multi[i]);
134
135
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 t1 = bpt::microsec_clock::local_time();
136
10/20
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
 2 BOOST_TEST_MESSAGE(Nthreads << " threads: " << (t1 - t0).total_milliseconds()
137 << "ms");
138
139 // Check the results
140
2/2
✓ Branch 1 taken 1000 times.
✓ Branch 2 taken 1 times.
 2002 for (std::size_t i = 0; i < qs.size(); ++i) {
141
5/10
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1000 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1000 times.
 2000 BOOST_CHECK_EQUAL(res_single[i].size(), res_multi[i].size());
142
2/2
✓ Branch 2 taken 4000 times.
✓ Branch 3 taken 1000 times.
 10000 for (std::size_t j = 0; j < res_single[i].size(); ++j) {
143
5/10
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4000 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 4000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 4000 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 4000 times.
 8000 BOOST_CHECK_EQUAL(res_single[i][j], res_multi[i][j]);
144 }
145 }
146 2 }
147