GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// Copyright (C) 2018 LAAS-CNRS |
||
2 |
// Author: Pierre Fernbach |
||
3 |
// |
||
4 |
// This file is part of the hpp-rbprm. |
||
5 |
// |
||
6 |
// hpp-rbprm is free software: you can redistribute it and/or modify |
||
7 |
// it under the terms of the GNU Lesser General Public License as published by |
||
8 |
// the Free Software Foundation, either version 3 of the License, or |
||
9 |
// (at your option) any later version. |
||
10 |
// |
||
11 |
// test-hpp is distributed in the hope that it will be useful, |
||
12 |
// but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
13 |
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
14 |
// GNU Lesser General Public License for more details. |
||
15 |
// |
||
16 |
// You should have received a copy of the GNU Lesser General Public License |
||
17 |
// along with hpp-core. If not, see <http://www.gnu.org/licenses/>. |
||
18 |
|||
19 |
#define BOOST_TEST_MODULE test - reachability |
||
20 |
|||
21 |
#include <boost/test/included/unit_test.hpp> |
||
22 |
#include <hpp/rbprm/contact_generation/reachability.hh> |
||
23 |
|||
24 |
#include "tools-fullbody.hh" |
||
25 |
|||
26 |
using namespace hpp; |
||
27 |
using namespace rbprm; |
||
28 |
|||
29 |
BOOST_AUTO_TEST_SUITE(reachability_quasiStatic) |
||
30 |
|||
31 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(kin_constraints_loaded) { |
32 |
✓✗ | 4 |
RbPrmFullBodyPtr_t fullBody = loadTalos(); |
33 |
|||
34 |
6 |
for (rbprm::CIT_Limb lit = fullBody->GetLimbs().begin(); |
|
35 |
✓✓ | 10 |
lit != fullBody->GetLimbs().end(); ++lit) { |
36 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK(lit->second->kinematicConstraints_.first.rows() > 0); |
37 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK(lit->second->kinematicConstraints_.second.rows() > 0); |
38 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK_EQUAL(lit->second->kinematicConstraints_.first.rows(), |
39 |
lit->second->kinematicConstraints_.second.rows()); |
||
40 |
} |
||
41 |
2 |
} |
|
42 |
|||
43 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(reachable_quasiStatic_rightFoot_front) { |
44 |
✓✗ | 4 |
RbPrmFullBodyPtr_t fullBody = loadTalos(); |
45 |
|||
46 |
4 |
std::vector<std::string> rleg, lleg; |
|
47 |
✓✗✓✗ |
2 |
rleg.push_back("talos_rleg_rom"); |
48 |
✓✗✓✗ |
2 |
lleg.push_back("talos_lleg_rom"); |
49 |
|||
50 |
✓✗✓✗ |
4 |
core::Configuration_t q0(fullBody->device_->configSize()), |
51 |
✓✗✓✗ |
4 |
q02(fullBody->device_->configSize()), |
52 |
✓✗✓✗ |
4 |
q04(fullBody->device_->configSize()), |
53 |
✓✗✓✗ |
4 |
q06(fullBody->device_->configSize()), |
54 |
✓✗✓✗ |
4 |
q07(fullBody->device_->configSize()), |
55 |
✓✗✓✗ |
4 |
q08(fullBody->device_->configSize()), |
56 |
✓✗✓✗ |
4 |
q085(fullBody->device_->configSize()), |
57 |
✓✗✓✗ |
4 |
q09(fullBody->device_->configSize()), |
58 |
✓✗✓✗ |
4 |
q10(fullBody->device_->configSize()); |
59 |
|||
60 |
// configurations found in python, for a step with the right foot in a |
||
61 |
// straight line of various length |
||
62 |
|||
63 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q0 << 0.0, 0.0, 1.02127, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, -0.411354, 0.859395, |
64 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
-0.448041, -0.001708, 0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, |
65 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
0.0, 0.006761, 0.25847, 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1, |
66 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
-0.005, -0.25847, -0.173046, 0.0002, -0.525366, 0.0, 0.0, 0.1, -0.005, |
67 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
68 |
✓✗✓✗ ✓✗ |
2 |
q02 << 0.0862078969760845, -0.008573254826838248, 1.012966620241554, |
69 |
✓✗✓✗ ✓✗ |
2 |
0.00824138443696191, -0.011008100927513427, 0.014396883219563509, |
70 |
✓✗✓✗ ✓✗ |
2 |
0.999801795882611, -0.028773943202354116, -0.009480497694556939, |
71 |
✓✗✓✗ ✓✗ |
2 |
-0.256695879641705, 0.876446746404094, -0.5979736009941322, |
72 |
✓✗✓✗ ✓✗ |
2 |
-0.009026468249301131, -0.028771659024478216, -0.009585374914341768, |
73 |
✓✗✓✗ ✓✗ |
2 |
-0.5646859416963076, 0.8507868052041541, -0.26432357597021416, |
74 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
-0.007213760150053602, 0.0, 0.006761, 0.25847, 0.173046, -0.0002, |
75 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
-0.525366, 0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, 0.0002, -0.525366, |
76 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
77 |
|||
78 |
✓✗✓✗ ✓✗ |
2 |
q04 << 0.1829294446377488, -0.017967821575754266, 0.9903038554763945, |
79 |
✓✗✓✗ ✓✗ |
2 |
0.017824630924904807, -0.015026065345066729, 0.028875263951008496, |
80 |
✓✗✓✗ ✓✗ |
2 |
0.9993111222359112, -0.057690383452244295, -0.021144092441181338, |
81 |
✓✗✓✗ ✓✗ |
2 |
-0.09933157527927024, 0.8860787584528044, -0.7577344674780488, |
82 |
✓✗✓✗ ✓✗ |
2 |
-0.017070899833595346, -0.05767672664576321, -0.02161457909094697, |
83 |
✓✗✓✗ ✓✗ |
2 |
-0.7038877298290985, 0.8255300912929601, -0.09262935387447445, |
84 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
-0.014892409096643178, 0.0, 0.006761, 0.25847, 0.173046, -0.0002, |
85 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
-0.525366, 0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, 0.0002, -0.525366, |
86 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
87 |
|||
88 |
✓✗✓✗ ✓✗ |
2 |
q06 << .28591419128265677, -0.024200128594736508, 0.9521454180806798, |
89 |
✓✗✓✗ ✓✗ |
2 |
0.02515240454918969, -0.015725533189828913, 0.04458195130774132, |
90 |
✓✗✓✗ ✓✗ |
2 |
0.9985652275985198, -0.08910767667902286, -0.030118264459553564, |
91 |
✓✗✓✗ ✓✗ |
2 |
0.07202772781974394, 0.8785365506294869, -0.9213836161899026, |
92 |
✓✗✓✗ ✓✗ |
2 |
-0.023256667177381514, -0.08907213527526688, -0.03133528753804178, |
93 |
✓✗✓✗ ✓✗ |
2 |
-0.8480559868843588, 0.7964272216477291, 0.08081051937323923, |
94 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
-0.02033115495888424, 0.0, 0.006761, 0.25847, 0.173046, -0.0002, |
95 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
-0.525366, 0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, 0.0002, -0.525366, |
96 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
97 |
|||
98 |
✓✗✓✗ ✓✗ |
2 |
q07 << 0.33831430286275344, -0.026102535439026707, 0.9264014385989923, |
99 |
✓✗✓✗ ✓✗ |
2 |
0.028009040301046218, -0.01536050225246519, 0.0532029127793272, |
100 |
✓✗✓✗ ✓✗ |
2 |
0.9980726419974458, -0.10637786677404223, -0.033578521388441446, |
101 |
✓✗✓✗ ✓✗ |
2 |
0.1636825181746803, 0.8679835254526652, -1.0039653759037674, |
102 |
✓✗✓✗ ✓✗ |
2 |
-0.025715064511826095, -0.10632747543186663, -0.03539602266269706, |
103 |
✓✗✓✗ ✓✗ |
2 |
-0.92397707758145, 0.7801364258578981, 0.17154305693047556, |
104 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
-0.022188894866708588, 0.0, 0.006761, 0.25847, 0.173046, -0.0002, |
105 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
-0.525366, 0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, 0.0002, -0.525366, |
106 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
107 |
|||
108 |
✓✗✓✗ ✓✗ |
2 |
q08 << 0.3910778394839933, -0.027254196111867062, 0.8955068488856367, |
109 |
✓✗✓✗ ✓✗ |
2 |
0.030419142092522528, -0.014567894246306272, 0.06259457790467278, |
110 |
✓✗✓✗ ✓✗ |
2 |
0.9974689825094909, -0.125212905834524, -0.03640180298045043, |
111 |
✓✗✓✗ ✓✗ |
2 |
0.26025722064293966, 0.852471868056844, -1.0874557066979949, |
112 |
✓✗✓✗ ✓✗ |
2 |
-0.027863389262476116, -0.12514549123930718, -0.03906653420181955, |
113 |
✓✗✓✗ ✓✗ |
2 |
-1.0034360788707313, 0.7619074499566504, 0.26680455410601817, |
114 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
-0.0234898358739585, 0.0, 0.006761, 0.25847, 0.173046, -0.0002, -0.525366, |
115 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, 0.0002, -0.525366, 0.0, 0.0, |
116 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
117 |
|||
118 |
✓✗✓✗ ✓✗ |
2 |
q085 << 0.4175844725073921, -0.02755478388727119, 0.8778918356809489, |
119 |
✓✗✓✗ ✓✗ |
2 |
0.03147398128923869, -0.013996348865992127, 0.06766853992051468, |
120 |
✓✗✓✗ ✓✗ |
2 |
0.9971130625085862, -0.13539766363327477, -0.03756933495270927, |
121 |
✓✗✓✗ ✓✗ |
2 |
0.31073976407032217, 0.8426367138186124, -1.129705221677871, |
122 |
✓✗✓✗ ✓✗ |
2 |
-0.028851896417215885, -0.13532132251363385, -0.04079097828587768, |
123 |
✓✗✓✗ ✓✗ |
2 |
-1.0447764119976486, 0.7518028052381551, 0.31664785321930555, |
124 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
-0.023921379343580407, 0.0, 0.006761, 0.25847, 0.173046, -0.0002, |
125 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
-0.525366, 0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, 0.0002, -0.525366, |
126 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
127 |
|||
128 |
✓✗✓✗ ✓✗ |
2 |
q09 << 0.4441791069457251, -0.027671061547016696, 0.8586668752399875, |
129 |
✓✗✓✗ ✓✗ |
2 |
0.032435659988807417, -0.013291512485512254, 0.0730592572981457, |
130 |
✓✗✓✗ ✓✗ |
2 |
0.9967113968346009, -0.14622490504118635, -0.03856085996297086, |
131 |
✓✗✓✗ ✓✗ |
2 |
0.36293907011755616, 0.8312357164664202, -1.1724006678502423, |
132 |
✓✗✓✗ ✓✗ |
2 |
-0.02980328498379143, -0.1461398509807246, -0.0424627165490291, |
133 |
✓✗✓✗ ✓✗ |
2 |
-1.087380902472244, 0.7408550080133347, 0.36830345789686386, |
134 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
-0.024192532270797932, 0.0, 0.006761, 0.25847, 0.173046, -0.0002, |
135 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
-0.525366, 0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, 0.0002, -0.525366, |
136 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
137 |
|||
138 |
✓✗✓✗ ✓✗ |
2 |
q10 << 0.4976763718908363, -0.027332201675632755, 0.8147190005401498, |
139 |
✓✗✓✗ ✓✗ |
2 |
0.03409365310845229, -0.011400939949673313, 0.08506958650346917, |
140 |
✓✗✓✗ ✓✗ |
2 |
0.9957262710394424, -0.17037128918299413, -0.03992325545762563, |
141 |
✓✗✓✗ ✓✗ |
2 |
0.47353812314710386, 0.8029167894150365, -1.2595367953020702, |
142 |
✓✗✓✗ ✓✗ |
2 |
-0.031681617573907826, -0.1702728964348347, -0.045731843518339, |
143 |
✓✗✓✗ ✓✗ |
2 |
-1.177132131592403, 0.7156714360272368, 0.47838302540976296, |
144 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
-0.024164227084087855, 0.0, 0.006761, 0.25847, 0.173046, -0.0002, |
145 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
-0.525366, 0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, 0.0002, -0.525366, |
146 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
147 |
|||
148 |
✓✗✓✗ |
4 |
State s0 = createState(fullBody, q0); |
149 |
✓✗✓✗ |
4 |
State s02 = createState(fullBody, q02); |
150 |
✓✗✓✗ |
4 |
State s04 = createState(fullBody, q04); |
151 |
✓✗✓✗ |
4 |
State s06 = createState(fullBody, q06); |
152 |
✓✗✓✗ |
4 |
State s07 = createState(fullBody, q07); |
153 |
✓✗✓✗ |
4 |
State s08 = createState(fullBody, q08); |
154 |
✓✗✓✗ |
4 |
State s085 = createState(fullBody, q085); |
155 |
✓✗✓✗ |
4 |
State s09 = createState(fullBody, q09); |
156 |
✓✗✓✗ |
6 |
State s10 = createState(fullBody, q10); |
157 |
|||
158 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reachability::isReachable(fullBody, s0, s02).success()); |
159 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reachability::isReachable(fullBody, s0, s04).success()); |
160 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reachability::isReachable(fullBody, s0, s06).success()); |
161 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reachability::isReachable(fullBody, s0, s07).success()); |
162 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reachability::isReachable(fullBody, s0, s08).success()); |
163 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reachability::isReachable(fullBody, s0, s085).success()); |
164 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!reachability::isReachable(fullBody, s0, s09) |
165 |
.success()); // should fail in quasi-static |
||
166 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!reachability::isReachable(fullBody, s0, s10) |
167 |
.success()); // should fail in quasi-static |
||
168 |
2 |
} |
|
169 |
|||
170 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(reachable_quasiStatic_leftFoot_left) { |
171 |
✓✗ | 4 |
RbPrmFullBodyPtr_t fullBody = loadTalos(); |
172 |
|||
173 |
4 |
std::vector<std::string> rleg, lleg; |
|
174 |
✓✗✓✗ |
2 |
rleg.push_back("talos_rleg_rom"); |
175 |
✓✗✓✗ |
2 |
lleg.push_back("talos_lleg_rom"); |
176 |
|||
177 |
✓✗✓✗ |
4 |
core::Configuration_t q0(fullBody->device_->configSize()), |
178 |
✓✗✓✗ |
4 |
q01(fullBody->device_->configSize()), |
179 |
✓✗✓✗ |
4 |
q02(fullBody->device_->configSize()), |
180 |
✓✗✓✗ |
4 |
q03(fullBody->device_->configSize()), |
181 |
✓✗✓✗ |
4 |
q04(fullBody->device_->configSize()), |
182 |
✓✗✓✗ |
4 |
q05(fullBody->device_->configSize()), |
183 |
✓✗✓✗ |
4 |
q06(fullBody->device_->configSize()), |
184 |
✓✗✓✗ |
4 |
q065(fullBody->device_->configSize()), |
185 |
✓✗✓✗ |
4 |
q07(fullBody->device_->configSize()); |
186 |
|||
187 |
// configurations found in python, for a step with the left foot on the left |
||
188 |
// of the robot |
||
189 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q0 << 0.0, 0.0, 1.02127, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, -0.411354, 0.859395, |
190 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
-0.448041, -0.001708, 0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, |
191 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
0.0, 0.006761, 0.25847, 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1, |
192 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
-0.005, -0.25847, -0.173046, 0.0002, -0.525366, 0.0, 0.0, 0.1, -0.005, |
193 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
194 |
|||
195 |
✓✗✓✗ ✓✗ |
2 |
q01 << 0.0036867479652359157, 0.04141617830294171, 1.0198444240469953, |
196 |
✓✗✓✗ ✓✗ |
2 |
0.003721019682859438, -0.0003634242360845229, -0.0007735418152011256, |
197 |
✓✗✓✗ ✓✗ |
2 |
0.999992711757643, 0.0014850993363459172, 0.080732684646178, |
198 |
✓✗✓✗ ✓✗✓✗ |
2 |
-0.41564883291835314, 0.8539384505473944, -0.43755462450471494, -0.1, |
199 |
✓✗✓✗ ✓✗ |
2 |
0.0015996662652822415, -0.07534308967968532, -0.406768476698486, |
200 |
✓✗✓✗ ✓✗✓✗ |
2 |
0.8637938693855636, -0.4562907092518919, 0.06619361634089803, 0.0, |
201 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
0.006761, 0.25847, 0.173046, -0.0002, -0.525366, 0.0, 0.0, 0.1, -0.005, |
202 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
-0.25847, -0.173046, 0.0002, -0.525366, 0.0, 0.0, 0.1, -0.005, 0.0, 0.0, |
203 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
204 |
|||
205 |
✓✗✓✗ ✓✗ |
2 |
q02 << 0.004611116432736838, 0.08680712725221325, 1.0142922199588116, |
206 |
✓✗✓✗ ✓✗ |
2 |
0.005072406112276357, 0.00030869559083044056, -0.0009934072891473668, |
207 |
✓✗✓✗ ✓✗ |
2 |
0.9999865941827532, 0.002089274701162499, 0.16220081143931303, |
208 |
✓✗✓✗ ✓✗ |
2 |
-0.4105366851345917, 0.8418780718606007, -0.43195676615110906, |
209 |
✓✗✓✗ ✓✗ |
2 |
-0.17234650085701872, 0.0018977415329604398, -0.15067184703358083, |
210 |
✓✗✓✗ ✓✗ |
2 |
-0.4101634630269449, 0.8700837151779192, -0.4605345140772428, |
211 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
0.13881840967987907, 0.0, 0.006761, 0.25847, 0.173046, -0.0002, -0.525366, |
212 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, 0.0002, -0.525366, 0.0, 0.0, |
213 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
214 |
|||
215 |
✓✗✓✗ ✓✗ |
2 |
q03 << 0.006069812147578944, 0.1343723191240031, 1.0049801899856554, |
216 |
✓✗✓✗ ✓✗ |
2 |
0.005480689651040333, 0.001368164254379433, -0.0012172097433430249, |
217 |
✓✗✓✗ ✓✗ |
2 |
0.9999833041446056, 0.003125863669880124, 0.24354644209627116, |
218 |
✓✗✓✗ ✓✗ |
2 |
-0.4046852082868577, 0.8274651032796254, -0.4255856138696762, |
219 |
✓✗✓✗ ✓✗ |
2 |
-0.2545123432305772, 0.001818627245265134, -0.22767007588570956, |
220 |
✓✗✓✗ ✓✗ |
2 |
-0.4131883186597945, 0.8748488272630103, -0.4644555570728859, |
221 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
0.2149981473489983, 0.0, 0.006761, 0.25847, 0.173046, -0.0002, -0.525366, |
222 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, 0.0002, -0.525366, 0.0, 0.0, |
223 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
224 |
|||
225 |
✓✗✓✗ ✓✗ |
2 |
q04 << 0.008047990227901055, 0.18330948995980373, 0.9917605461686454, |
226 |
✓✗✓✗ ✓✗ |
2 |
0.005434860332749788, 0.002782316410775649, -0.0014357323921921495, |
227 |
✓✗✓✗ ✓✗ |
2 |
0.9999803296470645, 0.0047717517773860465, 0.3251042702252173, |
228 |
✓✗✓✗ ✓✗✓✗ |
2 |
-0.3984614627687164, 0.8114498061429737, -0.4188439337907671, -0.35, |
229 |
✓✗✓✗ ✓✗ |
2 |
0.0011434892860631533, -0.3068945999578258, -0.4153829571441145, |
230 |
✓✗✓✗ ✓✗✓✗ |
2 |
0.8770635608839165, -0.46750144268392374, 0.2943135984824668, 0.0, |
231 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
0.006761, 0.25847, 0.173046, -0.0002, -0.525366, 0.0, 0.0, 0.1, -0.005, |
232 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
-0.25847, -0.173046, 0.0002, -0.525366, 0.0, 0.0, 0.1, -0.005, 0.0, 0.0, |
233 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
234 |
|||
235 |
✓✗✓✗ ✓✗ |
2 |
q05 << 0.010542435097660823, 0.23308066239699562, 0.9743851150936924, |
236 |
✓✗✓✗ ✓✗ |
2 |
0.005209114244873106, 0.004512698422774107, -0.0016462845098365647, |
237 |
✓✗✓✗ ✓✗ |
2 |
0.9999748948993877, 0.007228558140170314, 0.4075401576171006, |
238 |
✓✗✓✗ ✓✗ |
2 |
-0.3919367425959246, 0.7939276458088704, -0.41180262591157674, |
239 |
✓✗✓✗ ✓✗ |
2 |
-0.41799113910459745, -0.00035020261335618185, -0.38876451539856144, |
240 |
✓✗✓✗ ✓✗ |
2 |
-0.4165148957723023, 0.876178212168283, -0.4693978551510617, |
241 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
0.3766396959626528, 0.0, 0.006761, 0.25847, 0.173046, -0.0002, -0.525366, |
242 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, 0.0002, -0.525366, 0.0, 0.0, |
243 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
244 |
|||
245 |
✓✗✓✗ ✓✗ |
2 |
q06 << 0.013546170850372475, 0.28330427053738083, 0.9525367632048968, |
246 |
✓✗✓✗ ✓✗ |
2 |
0.004940803361470111, 0.006504975641915366, -0.0018487631504211497, |
247 |
✓✗✓✗ ✓✗ |
2 |
0.9999649272993804, 0.010717509677867299, 0.49153826649213456, |
248 |
✓✗✓✗ ✓✗ |
2 |
-0.3850468983549773, 0.7746895648627559, -0.4043789467167576, |
249 |
✓✗✓✗ ✓✗ |
2 |
-0.5014897845433318, -0.00289712423546666, -0.4736404723719533, |
250 |
✓✗✓✗ ✓✗ |
2 |
-0.4164338355907511, 0.8717900228565013, -0.46995492385053944, |
251 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
0.4620694161855139, 0.0, 0.006761, 0.25847, 0.173046, -0.0002, -0.525366, |
252 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, 0.0002, -0.525366, 0.0, 0.0, |
253 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
254 |
|||
255 |
✓✗✓✗ ✓✗ |
2 |
q065 << 0.021891810880261597, 0.32304524700609955, 0.9448469859415559, |
256 |
✓✗✓✗ ✓✗ |
2 |
0.0016067539573882779, 0.012823634427603679, -0.0014630220630010497, |
257 |
✓✗✓✗ ✓✗ |
2 |
0.9999154125765993, 0.017659417102092918, 0.5201615415690942, |
258 |
✓✗✓✗ ✓✗✓✗ |
2 |
-0.3960551713518669, 0.7826393539294886, -0.41613687880523537, -0.5236, |
259 |
✓✗✓✗ ✓✗✓✗ |
2 |
-0.011840941433258895, -0.5236, -0.3854201120060934, 0.7872744369817667, |
260 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
-0.4314657207843003, 0.5188406437977323, 0.0, 0.006761, 0.25847, 0.173046, |
261 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
-0.0002, -0.525366, 0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, 0.0002, |
262 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
-0.525366, 0.0, 0.0, 0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
263 |
|||
264 |
✓✗✓✗ ✓✗ |
2 |
q07 << 0.06322988906397728, 0.3508698009342632, 0.9806950681724664, |
265 |
✓✗✓✗ ✓✗ |
2 |
0.00028625197889660365, 0.06457115154609387, 0.0008041046986942251, |
266 |
✓✗✓✗ ✓✗ |
2 |
0.9979127406058355, 0.0726080777845305, 0.5183314220963501, |
267 |
✓✗✓✗ ✓✗✓✗ |
2 |
-0.2664866396755079, 0.2810914407985471, -0.16351595053624693, -0.5236, |
268 |
✓✗✓✗ ✓✗ |
2 |
-0.07623262260052481, -0.5209821932487237, -0.23445732616773088, |
269 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.23922956642519388, -0.15391308646685575, 0.5236, 0.0, 0.006761, 0.25847, |
270 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
0.173046, -0.0002, -0.525366, 0.0, 0.0, 0.1, -0.005, -0.25847, -0.173046, |
271 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
0.0002, -0.525366, 0.0, 0.0, 0.1, -0.005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, |
272 |
✓✗✓✗ |
2 |
0.0, 0.0; |
273 |
|||
274 |
✓✗✓✗ |
4 |
State s0 = createState(fullBody, q0); |
275 |
✓✗✓✗ |
4 |
State s01 = createState(fullBody, q01); |
276 |
✓✗✓✗ |
4 |
State s02 = createState(fullBody, q02); |
277 |
✓✗✓✗ |
4 |
State s03 = createState(fullBody, q03); |
278 |
✓✗✓✗ |
4 |
State s04 = createState(fullBody, q04); |
279 |
✓✗✓✗ |
4 |
State s05 = createState(fullBody, q05); |
280 |
✓✗✓✗ |
4 |
State s06 = createState(fullBody, q06); |
281 |
✓✗✓✗ |
4 |
State s065 = createState(fullBody, q065); |
282 |
✓✗✓✗ |
6 |
State s07 = createState(fullBody, q07); |
283 |
|||
284 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reachability::isReachable(fullBody, s0, s01).success()); |
285 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reachability::isReachable(fullBody, s0, s02).success()); |
286 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reachability::isReachable(fullBody, s0, s03).success()); |
287 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reachability::isReachable(fullBody, s0, s04).success()); |
288 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reachability::isReachable(fullBody, s0, s05).success()); |
289 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reachability::isReachable(fullBody, s0, s06).success()); |
290 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reachability::isReachable(fullBody, s0, s065).success()); |
291 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!reachability::isReachable(fullBody, s0, s07).success()); |
292 |
2 |
} |
|
293 |
|||
294 |
BOOST_AUTO_TEST_SUITE_END() |
Generated by: GCOVR (Version 4.2) |