GCC Code Coverage Report

Directory: ./
File: tests/tconfiguration.cc
Date: 2025-05-04 12:09:19
Exec Total Coverage
Lines: 202 248 81.5%
Branches: 907 1992 45.5%
Line Branch Exec Source
1 ///
2 /// Copyright (c) 2011 CNRS
3 /// Author: Florent Lamiraux
4 ///
5 ///
6
7 // Redistribution and use in source and binary forms, with or without
8 // modification, are permitted provided that the following conditions are
9 // met:
10 //
11 // 1. Redistributions of source code must retain the above copyright
12 // notice, this list of conditions and the following disclaimer.
13 //
14 // 2. Redistributions in binary form must reproduce the above copyright
15 // notice, this list of conditions and the following disclaimer in the
16 // documentation and/or other materials provided with the distribution.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
21 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
22 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
23 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
24 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
29 // DAMAGE.
30
31 // This test
32 // - builds a robot with various types of joints,
33 // - randomly samples pairs of configurations,
34 // - test consistency between hpp::pinocchio::difference and
35 // hpp::pinocchio::integrate functions.
36
37 #define BOOST_TEST_MODULE tdevice
38
39 #include <Eigen/Geometry>
40 #include <boost/test/unit_test.hpp>
41 #include <hpp/pinocchio/configuration.hh>
42 #include <hpp/pinocchio/device.hh>
43 #include <hpp/pinocchio/humanoid-robot.hh>
44 #include <hpp/pinocchio/joint-collection.hh>
45 #include <hpp/pinocchio/joint.hh>
46 #include <hpp/pinocchio/liegroup-element.hh>
47 #include <hpp/pinocchio/liegroup.hh>
48 #include <hpp/pinocchio/simple-device.hh>
49 #include <pinocchio/algorithm/joint-configuration.hpp>
50
51 using namespace hpp::pinocchio;
52 typedef Eigen::AngleAxis<value_type> AngleAxis_t;
53 typedef Eigen::Quaternion<value_type> Quaternion_t;
54
55 ✗ vector_t slerp(vectorIn_t v0, vectorIn_t v1, const value_type t) {
56 ✗ Quaternion_t q0(v0[3], v0[0], v0[1], v0[2]);
57 ✗ Quaternion_t q1(v1[3], v1[0], v1[1], v1[2]);
58 ✗ Quaternion_t q = q0.slerp(t, q1);
59 ✗ vector_t res(4);
60 ✗ res[0] = q.x();
61 ✗ res[1] = q.y();
62 ✗ res[2] = q.z();
63 ✗ res[3] = q.w();
64 ✗ return res;
65 }
66
67 ✗ AngleAxis_t aa(vectorIn_t v0, vectorIn_t v1) {
68 ✗ Quaternion_t q0(v0[3], v0[0], v0[1], v0[2]);
69 ✗ Quaternion_t q1(v1[3], v1[0], v1[1], v1[2]);
70 ✗ return AngleAxis_t(q0 * q1.conjugate());
71 }
72
73 ✗ vector_t interpolation(hpp::pinocchio::DevicePtr_t robot, vectorIn_t q0,
74 vectorIn_t q1, int n) {
75 ✗ const size_type rso3 = 3;
76 ✗ bool print = false;
77
78 ✗ vector_t q2(q0);
79 ✗ vector_t v(robot->numberDof());
80 ✗ hpp::pinocchio::difference(robot, q1, q0, v);
81
82 /// Check that q1 - q0 is the same as (-q1) - q0
83 ✗ vector_t mq1(q1);
84 ✗ mq1.segment<4>(rso3) *= -1;
85 ✗ vector_t mv(robot->numberDof());
86 ✗ hpp::pinocchio::difference(robot, mq1, q0, mv);
87 ✗ BOOST_CHECK(mv.isApprox(v));
88
89 ✗ value_type u = 0;
90 ✗ value_type step = (value_type)1 / (value_type)(n + 2);
91 ✗ vector_t angle1(n + 2), angle2(n + 2);
92
93 ✗ if (print) std::cout << "HPP : ";
94 ✗ for (int i = 0; i < n + 2; ++i) {
95 ✗ u = 0 + i * step;
96 ✗ interpolate(robot, q0, q1, u, q2);
97 ✗ AngleAxis_t aa1(aa(q0.segment<4>(rso3), q2.segment<4>(rso3)));
98 ✗ angle1[i] = aa1.angle();
99 ✗ if (print) std::cout << aa1.angle() << ", ";
100 ✗ if (print) std::cout << q2.segment<4>(rso3).transpose() << "\n";
101 }
102 ✗ if (print) std::cout << "\nEigen: ";
103 ✗ for (int i = 0; i < n + 2; ++i) {
104 ✗ u = 0 + i * step;
105 ✗ vector_t eigen_slerp = slerp(q0.segment<4>(rso3), q1.segment<4>(rso3), u);
106 ✗ AngleAxis_t aa2(aa(q0.segment<4>(rso3), eigen_slerp));
107 ✗ angle2[i] = aa2.angle();
108 ✗ if (print) std::cout << aa2.angle() << ", ";
109 ✗ if (print) std::cout << eigen_slerp.transpose() << "\n";
110 }
111 ✗ if (print) std::cout << "\n";
112 ✗ return angle1 - angle2;
113 }
114
115 typedef std::vector<DevicePtr_t> Robots_t;
116 5 Robots_t createRobots() {
117 5 Robots_t r;
118
3/6
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
 5 r.push_back(unittest::makeDevice(unittest::HumanoidSimple));
119
3/6
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
 5 r.push_back(unittest::makeDevice(unittest::CarLike));
120
3/6
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
 5 r.push_back(unittest::makeDevice(unittest::ManipulatorArm2));
121 5 return r;
122 }
123
124 const size_type NB_CONF = 4;
125 const size_type NB_SUCCESSIVE_INTERPOLATION = 1000;
126 const value_type eps = sqrt(Eigen::NumTraits<value_type>::dummy_precision());
127
128
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(is_valid_configuration) {
129 2 DevicePtr_t robot;
130
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 robot = unittest::makeDevice(unittest::HumanoidSimple);
131
132
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 Configuration_t q = robot->neutralConfiguration();
133
8/16
✓ 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 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(isNormalized(robot, q, eps));
134
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 LiegroupElementConstRef P1(q, robot->configSpace());
135
7/14
✓ 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 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 2 BOOST_CHECK(checkNormalized(P1, eps));
136
137 /// Set a quaternion of norm != 1
138
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 q[3] = 1;
139
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 q[4] = 1;
140
8/16
✓ 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 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(!isNormalized(robot, q, eps));
141
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 LiegroupElementConstRef P2(q, robot->configSpace());
142
7/14
✓ 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 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 2 BOOST_CHECK(!checkNormalized(P2, eps));
143
144
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 robot = unittest::makeDevice(unittest::CarLike);
145
146
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 q = robot->neutralConfiguration();
147
8/16
✓ 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 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(isNormalized(robot, q, eps));
148
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 LiegroupElementConstRef P3(q, robot->configSpace());
149
7/14
✓ 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 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 2 BOOST_CHECK(checkNormalized(P3, eps));
150
151 /// Set a complex of norm != 1
152
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 q.segment<2>(2) << 1, 1;
153
8/16
✓ 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 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(!isNormalized(robot, q, eps));
154
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 LiegroupElementConstRef P4(q, robot->configSpace());
155
7/14
✓ 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 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 2 BOOST_CHECK(!checkNormalized(P4, eps));
156
157
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 robot = unittest::makeDevice(unittest::CarLike);
158
159
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 q = robot->neutralConfiguration();
160
8/16
✓ 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 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(isNormalized(robot, q, eps));
161
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 LiegroupElementConstRef P5(q, robot->configSpace());
162
7/14
✓ 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 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 2 BOOST_CHECK(checkNormalized(P5, eps));
163 2 }
164
165 3 void test_difference_and_distance(DevicePtr_t robot) {
166
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Configuration_t q0;
167
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 q0.resize(robot->configSize());
168
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Configuration_t q1;
169
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 q1.resize(robot->configSize());
170
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Configuration_t q2;
171
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 q2.resize(robot->configSize());
172
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 vector_t q1_minus_q0;
173
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 q1_minus_q0.resize(robot->numberDof());
174
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 3 times.
 15 for (size_type i = 0; i < NB_CONF; ++i) {
175
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 q0 = ::pinocchio::randomConfiguration(robot->model());
176
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 q1 = ::pinocchio::randomConfiguration(robot->model());
177
178
8/16
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 12 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 12 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 12 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 12 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 12 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 12 times.
 12 BOOST_CHECK(isNormalized(robot, q0, eps));
179
8/16
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 12 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 12 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 12 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 12 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 12 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 12 times.
 12 BOOST_CHECK(isNormalized(robot, q1, eps));
180
181
4/8
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
 12 difference<DefaultLieGroupMap>(robot, q1, q0, q1_minus_q0);
182
183 // Check that the distance is the norm of the difference
184
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 value_type d = distance(robot, q0, q1);
185
186
27/54
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 15 taken 12 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 12 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 12 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 12 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 12 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 12 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 12 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 12 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 12 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 12 times.
✗ Branch 43 not taken.
✓ Branch 45 taken 12 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 12 times.
✗ Branch 49 not taken.
✓ Branch 51 taken 12 times.
✗ Branch 52 not taken.
✓ Branch 54 taken 12 times.
✗ Branch 55 not taken.
✓ Branch 57 taken 12 times.
✗ Branch 58 not taken.
✓ Branch 60 taken 12 times.
✗ Branch 61 not taken.
✓ Branch 63 taken 12 times.
✗ Branch 64 not taken.
✓ Branch 66 taken 12 times.
✗ Branch 67 not taken.
✓ Branch 69 taken 12 times.
✗ Branch 70 not taken.
✓ Branch 72 taken 12 times.
✗ Branch 73 not taken.
✓ Branch 75 taken 12 times.
✗ Branch 76 not taken.
✓ Branch 80 taken 12 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 12 times.
✗ Branch 84 not taken.
✗ Branch 104 not taken.
✓ Branch 105 taken 12 times.
 12 BOOST_CHECK_MESSAGE(
187 std::abs(d - q1_minus_q0.norm()) <
188 Eigen::NumTraits<value_type>::dummy_precision(),
189 "\n" << robot->name()
190 << ": The distance is not the norm of the difference\n"
191 << "q0=" << q0.transpose() << "\n"
192 << "q1=" << q1.transpose() << "\n"
193 << "(q1 - q0) = " << q1_minus_q0.transpose() << '\n'
194 << "distance=" << d << "\n"
195 << "(q1 - q0).norm () = " << q1_minus_q0.norm());
196 }
197 3 }
198
199
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(difference_and_distance) {
200
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Robots_t robots = createRobots();
201
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 8 for (std::size_t i = 0; i < robots.size(); ++i) {
202
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 test_difference_and_distance(robots[i]);
203 }
204 2 }
205
206 template <typename LieGroup>
207 12 void test_difference_and_integrate(DevicePtr_t robot) {
208
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 Configuration_t q0;
209
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 q0.resize(robot->configSize());
210
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 Configuration_t q1;
211
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 q1.resize(robot->configSize());
212
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 Configuration_t q2;
213
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 q2.resize(robot->configSize());
214
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 vector_t q1_minus_q0;
215
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 q1_minus_q0.resize(robot->numberDof());
216
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 6 times.
 60 for (size_type i = 0; i < NB_CONF; ++i) {
217
1/2
✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
 48 q0 = ::pinocchio::randomConfiguration(robot->model());
218
1/2
✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
 48 q1 = ::pinocchio::randomConfiguration(robot->model());
219
220
8/16
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 24 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 24 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 24 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 24 times.
 48 BOOST_CHECK(isNormalized(robot, q0, eps));
221
8/16
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 24 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 24 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 24 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 24 times.
 48 BOOST_CHECK(isNormalized(robot, q1, eps));
222
223
4/8
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
 48 difference<LieGroup>(robot, q1, q0, q1_minus_q0);
224
4/8
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
 48 integrate<true, LieGroup>(robot, q0, q1_minus_q0, q2);
225
226
8/16
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 24 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 24 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 24 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 24 times.
 48 BOOST_CHECK(isNormalized(robot, q2, eps));
227
228 // Check that distance (q0 + (q1 - q0), q1) is zero
229
17/34
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 24 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 24 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 24 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 24 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 24 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 24 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 24 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 24 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 24 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 24 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 24 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 24 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 24 times.
✗ Branch 50 not taken.
✗ Branch 62 not taken.
✓ Branch 63 taken 24 times.
 48 BOOST_CHECK_MESSAGE(isApprox(robot, q1, q2, eps),
230 "\n(q0 + (q1 - q0)) is not equivalent to q1\n"
231 << "q1=" << q1.transpose() << "\n"
232 << "q2=" << q2.transpose() << "\n");
233 }
234 12 }
235
236
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(difference_and_integrate) {
237
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Robots_t robots = createRobots();
238
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 8 for (std::size_t i = 0; i < robots.size(); ++i) {
239
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 test_difference_and_integrate<DefaultLieGroupMap>(robots[i]);
240
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 test_difference_and_integrate<RnxSOnLieGroupMap>(robots[i]);
241 }
242 2 }
243
244 template <typename LieGroup>
245 12 void test_interpolate_and_integrate(DevicePtr_t robot) {
246
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 Configuration_t q0;
247
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 q0.resize(robot->configSize());
248
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 Configuration_t q1;
249
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 q1.resize(robot->configSize());
250
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 Configuration_t q2;
251
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 q2.resize(robot->configSize());
252
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 Configuration_t q3;
253
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 q3.resize(robot->configSize());
254
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 vector_t q1_minus_q0;
255
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 q1_minus_q0.resize(robot->numberDof());
256
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 const value_type eps_dist = value_type(robot->numberDof()) *
257 12 sqrt(Eigen::NumTraits<value_type>::epsilon());
258 value_type d0, d1;
259
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 6 times.
 60 for (size_type i = 0; i < NB_CONF; ++i) {
260
1/2
✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
 48 q0 = ::pinocchio::randomConfiguration<LieGroup>(robot->model());
261
1/2
✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
 48 q1 = ::pinocchio::randomConfiguration<LieGroup>(robot->model());
262
263
8/16
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 24 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 24 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 24 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 24 times.
 48 BOOST_CHECK(isNormalized(robot, q0, eps));
264
8/16
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 24 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 24 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 24 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 24 times.
 48 BOOST_CHECK(isNormalized(robot, q1, eps));
265
266
4/8
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
 48 difference<LieGroup>(robot, q1, q0, q1_minus_q0);
267
268
4/8
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
 48 interpolate<LieGroup>(robot, q0, q1, 0, q2);
269
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 d0 = distance(robot, q0, q2);
270
271
20/40
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 24 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 24 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 24 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 24 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 24 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 24 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 24 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 24 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 24 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 24 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 24 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 24 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 24 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 24 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 24 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 24 times.
✗ Branch 59 not taken.
✗ Branch 74 not taken.
✓ Branch 75 taken 24 times.
 48 BOOST_CHECK_MESSAGE(d0 < eps_dist,
272 "\n(q0 + 0 * (q1 - q0)) is not equivalent to q0\n"
273 << "q0=" << q0.transpose() << "\n"
274 << "q1=" << q1.transpose() << "\n"
275 << "q2=" << q2.transpose() << "\n"
276 << "distance=" << d0);
277
278 // vector_t errors = interpolation (robot, q0, q1, 4);
279 // BOOST_CHECK_MESSAGE (errors.isZero (),
280 // "The interpolation computed by HPP does not match the Eigen SLERP"
281 // );
282
283
4/8
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
 48 interpolate<LieGroup>(robot, q0, q1, 1, q2);
284
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 d1 = distance(robot, q1, q2);
285
20/40
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 24 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 24 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 24 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 24 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 24 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 24 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 24 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 24 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 24 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 24 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 24 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 24 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 24 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 24 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 24 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 24 times.
✗ Branch 59 not taken.
✗ Branch 74 not taken.
✓ Branch 75 taken 24 times.
 48 BOOST_CHECK_MESSAGE(d1 < eps_dist,
286 "\n(q0 + 1 * (q1 - q0)) is not equivalent to q1\n"
287 << "q0=" << q0.transpose() << "\n"
288 << "q1=" << q1.transpose() << "\n"
289 << "q2=" << q2.transpose() << "\n"
290 << "distance=" << d1);
291
292
4/8
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
 48 interpolate<LieGroup>(robot, q0, q1, 0.5, q2);
293 // FIXME For SE(3) and SE(2) joints, with DefaultLieGroupMap
294 // there is no guaranty that the interpolation
295 // between two configuration within bounds will be within bounds.
296 // So we should not saturate the configuration.
297
5/10
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 24 times.
✗ Branch 14 not taken.
 48 integrate<false, LieGroup>(robot, q0, 0.5 * q1_minus_q0, q3);
298
299
8/16
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 24 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 24 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 24 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 24 times.
 48 BOOST_CHECK(isNormalized(robot, q2, eps));
300
8/16
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 24 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 24 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 24 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 24 times.
 48 BOOST_CHECK(isNormalized(robot, q3, eps));
301
302
16/32
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 24 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 24 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 24 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 24 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 24 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 24 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 24 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 24 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 24 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 24 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 24 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 24 times.
✗ Branch 47 not taken.
✗ Branch 60 not taken.
✓ Branch 61 taken 24 times.
 48 BOOST_CHECK_MESSAGE(isApprox(robot, q2, q3, eps),
303 "computation of q0 + 0.5 * (q1 - q0) does not yield "
304 "the same result with interpolate: "
305 << displayConfig(q2)
306 << " as with integrate: " << displayConfig(q3));
307
308
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 d0 = distance(robot, q0, q2);
309
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 d1 = distance(robot, q1, q2);
310
20/40
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 24 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 24 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 24 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 24 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 24 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 24 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 24 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 24 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 24 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 24 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 24 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 24 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 24 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 24 times.
✗ Branch 53 not taken.
✓ Branch 56 taken 24 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 24 times.
✗ Branch 60 not taken.
✗ Branch 75 not taken.
✓ Branch 76 taken 24 times.
 48 BOOST_CHECK_MESSAGE(
311 std::abs(d0 - d1) < eps_dist,
312 "\nq2 = (q0 + 0.5 * (q1 - q0)) is not equivalent to q1\n"
313 << "q0=" << q0.transpose() << "\n"
314 << "q1=" << q1.transpose() << "\n"
315 << "q2=" << q2.transpose() << "\n"
316 << "distance=" << distance);
317 }
318 12 }
319
320
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(interpolate_and_integrate) {
321
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Robots_t robots = createRobots();
322
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 8 for (std::size_t i = 0; i < robots.size(); ++i) {
323
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 test_interpolate_and_integrate<DefaultLieGroupMap>(robots[i]);
324
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 test_interpolate_and_integrate<RnxSOnLieGroupMap>(robots[i]);
325 }
326 2 }
327
328 3 void test_saturation(DevicePtr_t robot) {
329
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Configuration_t q0;
330
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 q0.resize(robot->configSize());
331
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Configuration_t q1;
332
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 q1.resize(robot->configSize());
333
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Configuration_t q2;
334
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 q2.resize(robot->configSize());
335
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 vector_t v;
336
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 v.resize(robot->numberDof());
337
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 v.setOnes();
338
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 ArrayXb saturation(robot->numberDof());
339
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 ArrayXb expected_sat(robot->numberDof());
340
341
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 q0 = robot->model().upperPositionLimit;
342
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 normalize(robot, q0);
343
344
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 q1 = robot->model().lowerPositionLimit;
345
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 normalize(robot, q1);
346
347
8/16
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 3 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 3 times.
 3 BOOST_CHECK(isNormalized(robot, q0, eps));
348
8/16
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 3 times.
✗ Branch 24 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 3 times.
 3 BOOST_CHECK(isNormalized(robot, q1, eps));
349
350
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 3 integrate<false, DefaultLieGroupMap>(robot, q0, v, q2);
351
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 saturate(robot, q2);
352
10/20
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 3 times.
✗ Branch 32 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 3 times.
 3 BOOST_CHECK((q2.array() <= robot->model().upperPositionLimit.array()).all());
353
10/20
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 3 times.
✗ Branch 32 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 3 times.
 3 BOOST_CHECK((q2.array() >= robot->model().lowerPositionLimit.array()).all());
354
355
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
 3 integrate<false, DefaultLieGroupMap>(robot, q1, -v, q2);
356
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 saturate(robot, q2);
357
10/20
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 3 times.
✗ Branch 32 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 3 times.
 3 BOOST_CHECK((q2.array() <= robot->model().upperPositionLimit.array()).all());
358
10/20
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 3 times.
✗ Branch 32 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 3 times.
 3 BOOST_CHECK((q2.array() >= robot->model().lowerPositionLimit.array()).all());
359
360
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 3 integrate<false, DefaultLieGroupMap>(robot, q0, v, q2);
361
4/8
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
 6 expected_sat = (q2.array() > robot->model().upperPositionLimit.array()) ||
362
4/8
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
 6 (q2.array() < robot->model().lowerPositionLimit.array());
363
8/16
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 3 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 3 times.
 3 BOOST_CHECK(saturate(robot, q2, saturation));
364
10/20
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 3 times.
✗ Branch 32 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 3 times.
 3 BOOST_CHECK((q2.array() <= robot->model().upperPositionLimit.array()).all());
365
10/20
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 3 times.
✗ Branch 32 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 3 times.
 3 BOOST_CHECK((q2.array() >= robot->model().lowerPositionLimit.array()).all());
366
4/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 2 times.
 3 if (robot->numberDof() == robot->configSize()) {
367
7/14
✓ 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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
 1 BOOST_CHECK_EQUAL(saturation.matrix(), expected_sat.matrix());
368 } else {
369
4/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 1 times.
 2 size_type iq = (robot->rootJoint()->configSize() == 4 ? 2 : 3);
370
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 size_type sq = robot->rootJoint()->configSize();
371
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 size_type sv = robot->rootJoint()->numberDof();
372
373
9/18
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 2 times.
 2 BOOST_CHECK_EQUAL(saturation.head(iq).matrix(),
374 expected_sat.head(iq).matrix());
375
11/22
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 30 taken 2 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 2 times.
✗ Branch 34 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 2 times.
 2 BOOST_CHECK_EQUAL(saturation.tail(robot->numberDof() - sv).matrix(),
376 expected_sat.tail(robot->configSize() - sq).matrix());
377 }
378
379
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
 3 integrate<false, DefaultLieGroupMap>(robot, q1, -v, q2);
380
4/8
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
 6 expected_sat = (q2.array() > robot->model().upperPositionLimit.array()) ||
381
4/8
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
 6 (q2.array() < robot->model().lowerPositionLimit.array());
382
8/16
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 3 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 3 times.
 3 BOOST_CHECK(saturate(robot, q2, saturation));
383
10/20
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 3 times.
✗ Branch 32 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 3 times.
 3 BOOST_CHECK((q2.array() <= robot->model().upperPositionLimit.array()).all());
384
10/20
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 3 times.
✗ Branch 32 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 3 times.
 3 BOOST_CHECK((q2.array() >= robot->model().lowerPositionLimit.array()).all());
385
4/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 2 times.
 3 if (robot->numberDof() == robot->configSize()) {
386
7/14
✓ 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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
 1 BOOST_CHECK_EQUAL(saturation.matrix(), expected_sat.matrix());
387 } else {
388
4/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 1 times.
 2 size_type iq = (robot->rootJoint()->configSize() == 4 ? 2 : 3);
389
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 size_type sq = robot->rootJoint()->configSize();
390
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 size_type sv = robot->rootJoint()->numberDof();
391
392
9/18
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 2 times.
 2 BOOST_CHECK_EQUAL(saturation.head(iq).matrix(),
393 expected_sat.head(iq).matrix());
394
11/22
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 30 taken 2 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 2 times.
✗ Branch 34 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 2 times.
 2 BOOST_CHECK_EQUAL(saturation.tail(robot->numberDof() - sv).matrix(),
395 expected_sat.tail(robot->configSize() - sq).matrix());
396 }
397 3 }
398
399
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(saturation) {
400
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Robots_t robots = createRobots();
401
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 8 for (std::size_t i = 0; i < robots.size(); ++i) {
402
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 test_saturation(robots[i]);
403 }
404 2 }
405
406 template <typename LieGroup>
407 12 int test_successive_interpolation(DevicePtr_t robot) {
408 12 int count = 0;
409
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 Configuration_t q0(robot->configSize());
410
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 Configuration_t q1(robot->configSize());
411
412
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 vector_t q1_minus_q0(robot->numberDof());
413
414
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 6 times.
 60 for (size_type i = 0; i < NB_CONF; ++i) {
415
1/2
✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
 48 q0 = ::pinocchio::randomConfiguration(robot->model());
416
417
2/2
✓ Branch 0 taken 24000 times.
✓ Branch 1 taken 24 times.
 48048 for (size_type j = 0; j < NB_SUCCESSIVE_INTERPOLATION; ++j) {
418
1/2
✓ Branch 3 taken 24000 times.
✗ Branch 4 not taken.
 48000 q1 = ::pinocchio::randomConfiguration(robot->model());
419 // BOOST_CHECK(isNormalized(robot, q1, eps));
420
3/6
✓ Branch 1 taken 24000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24000 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 24000 times.
 48000 if (!isNormalized(robot, q1, eps)) ++count;
421
4/8
✓ Branch 1 taken 24000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24000 times.
✗ Branch 11 not taken.
 48000 difference<LieGroup>(robot, q1, q0, q1_minus_q0);
422
423
5/10
✓ Branch 1 taken 24000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24000 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 24000 times.
✗ Branch 14 not taken.
 48000 integrate<true, LieGroup>(robot, q0, 0.5 * q1_minus_q0, q0);
424 // BOOST_CHECK(isNormalized(robot, q0, eps));
425
3/6
✓ Branch 1 taken 24000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24000 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 24000 times.
 48000 if (!isNormalized(robot, q0, eps)) ++count;
426 }
427 }
428 12 return count;
429 12 }
430
431
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(successive_interpolation) {
432
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Robots_t robots = createRobots();
433
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 std::vector<int> results(2 * robots.size());
434 2 #pragma omp parallel for
435 for (std::size_t i = 0; i < robots.size(); ++i) {
436 results[2 * i] =
437 test_successive_interpolation<DefaultLieGroupMap>(robots[i]);
438 results[2 * i + 1] =
439 test_successive_interpolation<RnxSOnLieGroupMap>(robots[i]);
440 }
441
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 8 for (std::size_t i = 0; i < robots.size(); ++i) {
442
8/16
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 3 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 3 times.
✗ Branch 27 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
 6 BOOST_CHECK_MESSAGE(
443 results[2 * i] == 0,
444 "DefaultLieGroupMap failed on robot " << robots[i]->name() << ".");
445
8/16
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 3 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 3 times.
✗ Branch 27 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
 6 BOOST_CHECK_MESSAGE(
446 results[2 * i + 1] == 0,
447 " RnxSOnLieGroupMap failed on robot " << robots[i]->name() << ".");
448 }
449 2 }
450