GCC Code Coverage Report

Directory: ./
File: tests/features/test_feature_point6d.cpp
Date: 2024-11-13 12:35:17
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