Directory: | ./ |
---|---|
File: | tests/features/test_feature_point6d.cpp |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 127 | 135 | 94.1% |
Branches: | 120 | 252 | 47.6% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /* | ||
2 | * Copyright 2019, | ||
3 | * François Bleibel, | ||
4 | * Olivier Stasse, | ||
5 | * | ||
6 | * CNRS/AIST | ||
7 | * | ||
8 | */ | ||
9 | |||
10 | /* -------------------------------------------------------------------------- */ | ||
11 | /* --- INCLUDES ------------------------------------------------------------- */ | ||
12 | /* -------------------------------------------------------------------------- */ | ||
13 | #include <dynamic-graph/linear-algebra.h> | ||
14 | |||
15 | #include <iostream> | ||
16 | #include <sot/core/debug.hh> | ||
17 | #include <sot/core/feature-abstract.hh> | ||
18 | #include <sot/core/feature-point6d.hh> | ||
19 | #include <sot/core/gain-adaptive.hh> | ||
20 | #include <sot/core/macros.hh> | ||
21 | #include <sot/core/sot.hh> | ||
22 | #include <sot/core/task.hh> | ||
23 | using namespace std; | ||
24 | using namespace dynamicgraph::sot; | ||
25 | |||
26 | class TestPoint6d { | ||
27 | public: | ||
28 | SOT_CORE_DISABLE_WARNING_PUSH | ||
29 | SOT_CORE_DISABLE_WARNING_DEPRECATED | ||
30 | FeaturePoint6d feature_, featureDes_; | ||
31 | SOT_CORE_DISABLE_WARNING_POP | ||
32 | Task task_; | ||
33 | int time_; | ||
34 | int dim_, robotDim_, featureDim_; | ||
35 | dynamicgraph::Vector manual_; | ||
36 | |||
37 | 1 | TestPoint6d(unsigned dim, std::string &name) | |
38 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | : feature_("feature" + name), |
39 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | featureDes_("featureDes" + name), |
40 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | task_("task" + name), |
41 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | time_(0) |
42 | |||
43 | { | ||
44 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | feature_.computationFrame("desired"); |
45 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | feature_.setReference(&featureDes_); |
46 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | feature_.selectionSIN = Flags(true); |
47 | |||
48 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | task_.addFeature(feature_); |
49 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | task_.setWithDerivative(true); |
50 | 1 | dim_ = dim; | |
51 | 1 | robotDim_ = featureDim_ = dim; | |
52 | |||
53 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | dynamicgraph::Matrix Jq(dim, dim); |
54 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Jq.setIdentity(); |
55 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | feature_.articularJacobianSIN.setReference(&Jq); |
56 | |||
57 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | manual_.resize(dim); |
58 | 1 | } | |
59 | |||
60 | 4 | void setInputs(MatrixHomogeneous &s, MatrixHomogeneous &sd, | |
61 | dynamicgraph::Vector &vd, double gain) { | ||
62 | 4 | feature_.positionSIN = s; | |
63 | 4 | feature_.positionSIN.access(time_); | |
64 | 4 | feature_.positionSIN.setReady(); | |
65 | |||
66 | 4 | featureDes_.positionSIN = sd; | |
67 | 4 | featureDes_.positionSIN.access(time_); | |
68 | 4 | featureDes_.positionSIN.setReady(); | |
69 | |||
70 | 4 | featureDes_.velocitySIN = vd; | |
71 | 4 | featureDes_.velocitySIN.access(time_); | |
72 | 4 | featureDes_.velocitySIN.setReady(); | |
73 | |||
74 | 4 | task_.controlGainSIN = gain; | |
75 | 4 | task_.controlGainSIN.access(time_); | |
76 | 4 | task_.controlGainSIN.setReady(); | |
77 | 4 | } | |
78 | |||
79 | 4 | void printInputs() { | |
80 | 4 | std::cout << "----- inputs -----" << std::endl; | |
81 |
1/2✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | std::cout << "feature_.position: " << feature_.positionSIN(time_) |
82 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | << std::endl; |
83 |
1/2✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | std::cout << "featureDes_.position: " << featureDes_.positionSIN(time_) |
84 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | << std::endl; |
85 | 4 | std::cout << "featureDes_.velocity: " | |
86 |
2/4✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
|
4 | << featureDes_.velocitySIN(time_).transpose() << std::endl; |
87 | 4 | std::cout << "task.controlGain: " << task_.controlGainSIN(time_) | |
88 | 4 | << std::endl; | |
89 | 4 | } | |
90 | |||
91 | 4 | int recompute() { | |
92 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | feature_.errorSOUT.recompute(time_); |
93 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | feature_.errordotSOUT.recompute(time_); |
94 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | task_.taskSOUT.recompute(time_); |
95 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | task_.errorSOUT.recompute(time_); |
96 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | task_.errorTimeDerivativeSOUT.recompute(time_); |
97 | 4 | time_++; | |
98 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | MatrixHomogeneous s = feature_.positionSIN; |
99 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | MatrixHomogeneous sd = featureDes_.positionSIN; |
100 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | dynamicgraph::Vector vd = featureDes_.velocitySIN; |
101 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | double gain = task_.controlGainSIN; |
102 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | dynamicgraph::Vector manual; |
103 | const std::vector<dynamicgraph::sot::MultiBound> &taskTaskSOUT = | ||
104 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | task_.taskSOUT(time_); |
105 | |||
106 | /// Verify the computation of the desired frame. | ||
107 | /// -gain *(s-sd) - ([w]x (sd -s)-vd) | ||
108 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | dynamicgraph::Matrix aM; |
109 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | dynamicgraph::Vector aV; |
110 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | aM.resize(3, 3); |
111 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | aV.resize(3); |
112 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | aM(0, 0) = 0.0; |
113 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | aM(0, 1) = -vd(5); |
114 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | aM(0, 2) = vd(4); |
115 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | aM(1, 0) = vd(5); |
116 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | aM(1, 1) = 0.0; |
117 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | aM(1, 2) = -vd(3); |
118 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | aM(2, 0) = -vd(4); |
119 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | aM(2, 1) = vd(3); |
120 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | aM(2, 2) = 0.0; |
121 |
5/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 9 taken 12 times.
✓ Branch 10 taken 4 times.
|
16 | for (unsigned int i = 0; i < 3; i++) aV(i) = sd(i, 3) - s(i, 3); |
122 | |||
123 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | aV = aM * aV; |
124 | |||
125 | /// Recompute error_th. | ||
126 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 4 times.
|
16 | for (unsigned int i = 0; i < 3; i++) { |
127 |
5/10✓ 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.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
|
12 | manual_[i] = -gain * (s(i, 3) - sd(i, 3)) - (aV(i) - vd(i)); |
128 |
3/6✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 12 times.
|
12 | if (manual_[i] != taskTaskSOUT[i].getSingleBound()) return -1; |
129 | } | ||
130 | 4 | return 0; | |
131 | 4 | } | |
132 | |||
133 | 4 | void printOutput() { | |
134 | 4 | std::cout << "----- output -----" << std::endl; | |
135 | 4 | std::cout << "time: " << time_ << std::endl; | |
136 |
1/2✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | std::cout << "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose() |
137 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | << std::endl; |
138 | 4 | std::cout << "feature.errordotSOUT: " | |
139 |
2/4✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
|
4 | << feature_.errordotSOUT(time_).transpose() << std::endl; |
140 | 4 | std::cout << "task.taskSOUT: " << task_.taskSOUT(time_) << std::endl; | |
141 | // std::cout << "task.errorSOUT: " << task_.errorSOUT(time_) | ||
142 | //<< std::endl; | ||
143 | // std::cout << "task.errordtSOUT: " << task_.errorTimeDerivativeSOUT(time_) | ||
144 | //<< std::endl; | ||
145 |
2/4✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
|
4 | std::cout << "manual: " << manual_.transpose() << std::endl; |
146 | 4 | } | |
147 | |||
148 | 4 | int runTest(MatrixHomogeneous &s, MatrixHomogeneous &sd, | |
149 | dynamicgraph::Vector &vd, double gain) { | ||
150 | 4 | setInputs(s, sd, vd, gain); | |
151 | 4 | printInputs(); | |
152 | 4 | int r = recompute(); | |
153 | 4 | printOutput(); | |
154 | 4 | return r; | |
155 | } | ||
156 | }; | ||
157 | |||
158 | 1 | int main(void) { | |
159 | // Name of the robot | ||
160 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | std::string srobot("Robot"); |
161 | // Dimension of the robot. | ||
162 | 1 | unsigned int dim = 6; | |
163 | // Feature and Desired Feature | ||
164 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | MatrixHomogeneous s, sd; |
165 | // Desired velocity | ||
166 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | dynamicgraph::Vector vd(6); |
167 | // Task gain. | ||
168 | double gain; | ||
169 | // Result of test | ||
170 | int r; | ||
171 | |||
172 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | TestPoint6d testFeaturePoint6d(dim, srobot); |
173 | |||
174 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | std::cout << " ----- Test Velocity -----" << std::endl; |
175 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | s.setIdentity(); |
176 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | sd.setIdentity(); |
177 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | vd.setConstant(1.); |
178 | 1 | gain = 0.0; | |
179 | |||
180 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
|
1 | if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) { |
181 | ✗ | std::cerr << "Failure on 1st test." << std::endl; | |
182 | ✗ | return r; | |
183 | } | ||
184 | |||
185 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | std::cout << " ----- Test Position -----" << std::endl; |
186 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | s.setIdentity(); |
187 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | sd.setIdentity(); |
188 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | sd.translation()[2] = 2.0; |
189 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | vd.setZero(); |
190 | 1 | gain = 1.0; | |
191 | |||
192 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
|
1 | if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) { |
193 | ✗ | std::cerr << "Failure on 2nd test." << std::endl; | |
194 | ✗ | return r; | |
195 | } | ||
196 | |||
197 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | std::cout << " ----- Test both -----" << std::endl; |
198 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | s.setIdentity(); |
199 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | sd.setIdentity(); |
200 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | sd.translation()[2] = 2.0; |
201 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | vd.setConstant(1.); |
202 | 1 | gain = 3.0; | |
203 | |||
204 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
|
1 | if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) { |
205 | ✗ | std::cerr << "Failure on 3th test." << std::endl; | |
206 | ✗ | return r; | |
207 | } | ||
208 | |||
209 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | std::cout << " ----- Test both again -----" << std::endl; |
210 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | s.setIdentity(); |
211 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | sd.setIdentity(); |
212 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | sd.translation()[2] = 2.0; |
213 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | vd.setConstant(1.); |
214 | 1 | gain = 3.0; | |
215 | |||
216 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
|
1 | if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) { |
217 | ✗ | std::cerr << "Failure on 4th test." << std::endl; | |
218 | ✗ | return r; | |
219 | } | ||
220 | |||
221 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | std::cout << "Test successfull !" << std::endl; |
222 | 1 | return 0; | |
223 | 1 | } | |
224 |