GCC Code Coverage Report

Directory: ./
File: src/angle-estimator.cpp
Date: 2024-09-28 11:08:19
Exec Total Coverage
Lines: 0 184 0.0%
Branches: 0 444 0.0%
Line Branch Exec Source
1 /*
2 * Copyright 2010,
3 * François Bleibel,
4 * Olivier Stasse,
5 *
6 * CNRS/AIST
7 *
8 */
9
10 #include <dynamic-graph/command-getter.h>
11 #include <dynamic-graph/command-setter.h>
12 #include <dynamic-graph/command.h>
13 #include <dynamic-graph/factory.h>
14 #include <sot/dynamic-pinocchio/angle-estimator.h>
15
16 #include <sot/core/debug.hh>
17
18 using namespace dynamicgraph::sot;
19 using namespace dynamicgraph;
20
21 ✗ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AngleEstimator, "AngleEstimator");
22
23 ✗ AngleEstimator::AngleEstimator(const std::string& name)
24 : Entity(name),
25 ✗ sensorWorldRotationSIN(
26 ✗ NULL, "sotAngleEstimator(" + name +
27 ")::input(MatrixRotation)::sensorWorldRotation"),
28 ✗ sensorEmbeddedPositionSIN(
29 ✗ NULL, "sotAngleEstimator(" + name +
30 ")::input(MatrixHomo)::sensorEmbeddedPosition"),
31 ✗ contactWorldPositionSIN(NULL,
32 ✗ "sotAngleEstimator(" + name +
33 ")::input(MatrixHomo)::contactWorldPosition"),
34 ✗ contactEmbeddedPositionSIN(
35 ✗ NULL, "sotAngleEstimator(" + name +
36 ")::input(MatrixHomo)::contactEmbeddedPosition")
37
38 ,
39 ✗ anglesSOUT(boost::bind(&AngleEstimator::computeAngles, this, _1, _2),
40 ✗ sensorWorldRotationSIN << sensorEmbeddedPositionSIN
41 ✗ << contactWorldPositionSIN
42 ✗ << contactEmbeddedPositionSIN,
43 ✗ "sotAngleEstimator(" + name + ")::output(Vector)::angles"),
44 ✗ flexibilitySOUT(boost::bind(&AngleEstimator::computeFlexibilityFromAngles,
45 this, _1, _2),
46 anglesSOUT,
47 ✗ "sotAngleEstimator(" + name +
48 ")::output(matrixRotation)::flexibility"),
49 ✗ driftSOUT(
50 boost::bind(&AngleEstimator::computeDriftFromAngles, this, _1, _2),
51 anglesSOUT,
52 ✗ "sotAngleEstimator(" + name + ")::output(matrixRotation)::drift"),
53 ✗ sensorWorldRotationSOUT(
54 boost::bind(&AngleEstimator::computeSensorWorldRotation, this, _1,
55 _2),
56 ✗ anglesSOUT << sensorWorldRotationSIN,
57 ✗ "sotAngleEstimator(" + name +
58 ")::output(matrixRotation)::sensorCorrectedRotation"),
59 ✗ waistWorldRotationSOUT(
60 boost::bind(&AngleEstimator::computeWaistWorldRotation, this, _1, _2),
61 ✗ sensorWorldRotationSOUT << sensorEmbeddedPositionSIN,
62 ✗ "sotAngleEstimator(" + name +
63 ")::output(matrixRotation)::waistWorldRotation"),
64 ✗ waistWorldPositionSOUT(
65 boost::bind(&AngleEstimator::computeWaistWorldPosition, this, _1, _2),
66 ✗ flexibilitySOUT << contactEmbeddedPositionSIN,
67 ✗ "sotAngleEstimator(" + name +
68 ")::output(MatrixHomogeneous)::waistWorldPosition"),
69 ✗ waistWorldPoseRPYSOUT(
70 boost::bind(&AngleEstimator::computeWaistWorldPoseRPY, this, _1, _2),
71 waistWorldPositionSOUT,
72 ✗ "sotAngleEstimator(" + name +
73 ")::output(vectorRollPitchYaw)::waistWorldPoseRPY")
74
75 ,
76 ✗ jacobianSIN(NULL, "sotAngleEstimator(" + name + ")::input()::jacobian"),
77 ✗ qdotSIN(NULL, "sotAngleEstimator(" + name + ")::input()::qdot"),
78 ✗ xff_dotSOUT(
79 boost::bind(&AngleEstimator::compute_xff_dotSOUT, this, _1, _2),
80 ✗ jacobianSIN << qdotSIN,
81 ✗ "sotAngleEstimator(" + name + ")::output(vector)::xff_dot"),
82 ✗ qdotSOUT(boost::bind(&AngleEstimator::compute_qdotSOUT, this, _1, _2),
83 ✗ xff_dotSOUT << qdotSIN,
84 ✗ "sotAngleEstimator(" + name + ")::output(vector)::qdotOUT")
85
86 ,
87 ✗ fromSensor_(true) {
88 sotDEBUGIN(5);
89
90 ✗ signalRegistration(sensorWorldRotationSIN);
91 ✗ signalRegistration(sensorEmbeddedPositionSIN);
92 ✗ signalRegistration(contactWorldPositionSIN);
93 ✗ signalRegistration(contactEmbeddedPositionSIN);
94 ✗ signalRegistration(anglesSOUT);
95 ✗ signalRegistration(flexibilitySOUT);
96 ✗ signalRegistration(driftSOUT);
97 ✗ signalRegistration(sensorWorldRotationSOUT);
98 ✗ signalRegistration(waistWorldRotationSOUT);
99 ✗ signalRegistration(waistWorldPositionSOUT);
100 ✗ signalRegistration(waistWorldPoseRPYSOUT);
101 ✗ signalRegistration(jacobianSIN);
102 ✗ signalRegistration(qdotSIN);
103 ✗ signalRegistration(xff_dotSOUT);
104 ✗ signalRegistration(qdotSOUT);
105
106 /* Commands. */
107 {
108 ✗ std::string docstring;
109 docstring =
110 " \n"
111 " Set flag specifying whether angle is measured from sensors or "
112 "simulated.\n"
113 " \n"
114 " Input:\n"
115 " - a boolean value.\n"
116 ✗ " \n";
117 ✗ addCommand("setFromSensor",
118 new ::dynamicgraph::command::Setter<AngleEstimator, bool>(
119 ✗ *this, &AngleEstimator::fromSensor, docstring));
120
121 docstring =
122 " \n"
123 " Get flag specifying whether angle is measured from sensors or "
124 "simulated.\n"
125 " \n"
126 " No input,\n"
127 " return a boolean value.\n"
128 ✗ " \n";
129 ✗ addCommand("getFromSensor",
130 new ::dynamicgraph::command::Getter<AngleEstimator, bool>(
131 ✗ *this, &AngleEstimator::fromSensor, docstring));
132 }
133
134 sotDEBUGOUT(5);
135 }
136
137 ✗ AngleEstimator::~AngleEstimator(void) {
138 sotDEBUGIN(5);
139
140 sotDEBUGOUT(5);
141 ✗ return;
142 }
143
144 /* --- SIGNALS -------------------------------------------------------------- */
145 /* --- SIGNALS -------------------------------------------------------------- */
146 /* --- SIGNALS -------------------------------------------------------------- */
147 ✗ dynamicgraph::Vector& AngleEstimator::computeAngles(dynamicgraph::Vector& res,
148 const int& time) {
149 sotDEBUGIN(15);
150
151 ✗ res.resize(3);
152
153 ✗ const MatrixRotation& worldestRchest = sensorWorldRotationSIN(time);
154 sotDEBUG(35) << "worldestRchest = " << std::endl << worldestRchest;
155 ✗ const MatrixHomogeneous& waistMchest = sensorEmbeddedPositionSIN(time);
156 ✗ MatrixRotation waistRchest;
157 ✗ waistRchest = waistMchest.linear();
158
159 ✗ const MatrixHomogeneous& waistMleg = contactEmbeddedPositionSIN(time);
160 ✗ MatrixRotation waistRleg;
161 ✗ waistRleg = waistMleg.linear();
162 ✗ MatrixRotation chestRleg;
163 ✗ chestRleg = waistRchest.transpose() * waistRleg;
164 ✗ MatrixRotation worldestRleg;
165 ✗ worldestRleg = worldestRchest * chestRleg;
166
167 sotDEBUG(35) << "worldestRleg = " << std::endl << worldestRleg;
168
169 /* Euler angles with following code: (-z)xy, -z being the yaw drift, x the
170 * first flexibility and y the second flexibility. */
171 ✗ const double TOLERANCE_TH = 1e-6;
172
173 ✗ const MatrixRotation& R = worldestRleg;
174 ✗ if ((fabs(R(0, 1)) < TOLERANCE_TH) && (fabs(R(1, 1)) < TOLERANCE_TH)) {
175 /* This means that cos(X) is very low, ie flex1 is close to 90deg.
176 * I take the case into account, but it is bloody never going
177 * to happens. */
178 ✗ if (R(2, 1) > 0)
179 ✗ res(0) = M_PI / 2;
180 else
181 ✗ res(0) = -M_PI / 2;
182 ✗ res(2) = atan2(-R(0, 2), R(0, 0));
183 ✗ res(1) = 0;
184
185 /* To sum up: if X=PI/2, then Y and Z are in singularity.
186 * we cannot decide both of then. I fixed Y=0, which
187 * means that all the measurement coming from the sensor
188 * is assumed to be drift of the gyro. */
189 } else {
190 ✗ double& X = res(0);
191 ✗ double& Y = res(1);
192 ✗ double& Z = res(2);
193
194 ✗ Y = atan2(R(2, 0), R(2, 2));
195 ✗ Z = atan2(R(0, 1), R(1, 1));
196 ✗ if (fabs(R(2, 0)) > fabs(R(2, 2))) {
197 ✗ X = atan2(R(2, 1) * sin(Y), R(2, 0));
198 } else {
199 ✗ X = atan2(R(2, 1) * cos(Y), R(2, 2));
200 }
201 }
202
203 sotDEBUG(35) << "angles = " << res;
204
205 sotDEBUGOUT(15);
206 ✗ return res;
207 }
208
209 /* compute the transformation matrix of the flexibility, ie
210 * feetRleg.
211 */
212 ✗ MatrixRotation& AngleEstimator::computeFlexibilityFromAngles(
213 MatrixRotation& res, const int& time) {
214 sotDEBUGIN(15);
215
216 ✗ const dynamicgraph::Vector& angles = anglesSOUT(time);
217 ✗ double cx = cos(angles(0));
218 ✗ double sx = sin(angles(0));
219 ✗ double cy = cos(angles(1));
220 ✗ double sy = sin(angles(1));
221
222 ✗ res(0, 0) = cy;
223 ✗ res(0, 1) = 0;
224 ✗ res(0, 2) = -sy;
225
226 ✗ res(1, 0) = -sx * sy;
227 ✗ res(1, 1) = cx;
228 ✗ res(1, 2) = -sx * cy;
229
230 ✗ res(2, 0) = cx * sy;
231 ✗ res(2, 1) = sx;
232 ✗ res(2, 2) = cx * cy;
233
234 sotDEBUGOUT(15);
235 ✗ return res;
236 }
237
238 /* Compute the rotation matrix of the drift, ie the
239 * transfo from the world frame to the estimated (drifted) world
240 * frame: worldRworldest.
241 */
242 ✗ MatrixRotation& AngleEstimator::computeDriftFromAngles(MatrixRotation& res,
243 const int& time) {
244 sotDEBUGIN(15);
245
246 ✗ const dynamicgraph::Vector& angles = anglesSOUT(time);
247 ✗ double cz = cos(angles(2));
248 ✗ double sz = sin(angles(2));
249
250 ✗ res(0, 0) = cz;
251 ✗ res(0, 1) = -sz;
252 ✗ res(0, 2) = 0;
253
254 /* z is the positive angle (R_{-z} has been computed
255 *in the <angles> function). Thus, the /-/sin(z) is in 0,1. */
256 ✗ res(1, 0) = sz;
257 ✗ res(1, 1) = cz;
258 ✗ res(1, 2) = 0;
259
260 ✗ res(2, 0) = 0;
261 ✗ res(2, 1) = 0;
262 ✗ res(2, 2) = 1;
263
264 sotDEBUGOUT(15);
265 ✗ return res;
266 }
267
268 ✗ MatrixRotation& AngleEstimator::computeSensorWorldRotation(MatrixRotation& res,
269 const int& time) {
270 sotDEBUGIN(15);
271
272 ✗ const MatrixRotation& worldRworldest = driftSOUT(time);
273 ✗ const MatrixRotation& worldestRsensor = sensorWorldRotationSIN(time);
274
275 ✗ res = worldRworldest * worldestRsensor;
276
277 sotDEBUGOUT(15);
278 ✗ return res;
279 }
280
281 ✗ MatrixRotation& AngleEstimator::computeWaistWorldRotation(MatrixRotation& res,
282 const int& time) {
283 sotDEBUGIN(15);
284
285 // chest = sensor
286 ✗ const MatrixRotation& worldRsensor = sensorWorldRotationSOUT(time);
287 ✗ const MatrixHomogeneous& waistMchest = sensorEmbeddedPositionSIN(time);
288 ✗ MatrixRotation waistRchest;
289 ✗ waistRchest = waistMchest.linear();
290
291 ✗ res = worldRsensor * waistRchest.transpose();
292
293 sotDEBUGOUT(15);
294 ✗ return res;
295 }
296
297 ✗ MatrixHomogeneous& AngleEstimator::computeWaistWorldPosition(
298 MatrixHomogeneous& res, const int& time) {
299 sotDEBUGIN(15);
300
301 ✗ const MatrixHomogeneous& waistMleg = contactEmbeddedPositionSIN(time);
302 ✗ const MatrixHomogeneous& contactPos = contactWorldPositionSIN(time);
303 ✗ MatrixHomogeneous legMwaist(waistMleg.inverse());
304 ✗ MatrixHomogeneous tmpRes;
305 ✗ if (fromSensor_) {
306 ✗ const MatrixRotation& Rflex = flexibilitySOUT(time); // footRleg
307 ✗ MatrixHomogeneous footMleg;
308 ✗ footMleg.linear() = Rflex;
309 ✗ footMleg.translation().setZero();
310
311 ✗ tmpRes = footMleg * legMwaist;
312 } else {
313 ✗ tmpRes = legMwaist;
314 }
315
316 ✗ res = contactPos * tmpRes;
317 sotDEBUGOUT(15);
318 ✗ return res;
319 }
320
321 ✗ dynamicgraph::Vector& AngleEstimator::computeWaistWorldPoseRPY(
322 dynamicgraph::Vector& res, const int& time) {
323 ✗ const MatrixHomogeneous& M = waistWorldPositionSOUT(time);
324
325 ✗ VectorRollPitchYaw r = (M.linear().eulerAngles(2, 1, 0)).reverse();
326 ✗ dynamicgraph::Vector t(3);
327 ✗ t = M.translation();
328
329 ✗ res.resize(6);
330 ✗ for (int i = 0; i < 3; ++i) {
331 ✗ res(i) = t(i);
332 ✗ res(i + 3) = r(i);
333 }
334
335 ✗ return res;
336 }
337
338 /* --- VELOCITY SIGS -------------------------------------------------------- */
339
340 ✗ dynamicgraph::Vector& AngleEstimator::compute_xff_dotSOUT(
341 dynamicgraph::Vector& res, const int& time) {
342 ✗ const dynamicgraph::Matrix& J = jacobianSIN(time);
343 ✗ const dynamicgraph::Vector& dq = qdotSIN(time);
344
345 ✗ const Eigen::DenseIndex nr = J.rows(), nc = J.cols() - 6;
346 ✗ assert(nr == 6);
347 ✗ dynamicgraph::Matrix Ja(nr, nc);
348 ✗ dynamicgraph::Vector dqa(nc);
349 ✗ for (int j = 0; j < nc; ++j) {
350 ✗ for (int i = 0; i < nr; ++i) Ja(i, j) = J(i, j + 6);
351 ✗ dqa(j) = dq(j + 6);
352 }
353 ✗ dynamicgraph::Matrix Jff(6, 6);
354 ✗ for (int j = 0; j < 6; ++j)
355 ✗ for (int i = 0; i < 6; ++i) Jff(i, j) = J(i, j);
356
357 ✗ res.resize(nr);
358 ✗ res = (Jff.inverse() * (Ja * dqa)) * (-1);
359 ✗ return res;
360 }
361
362 ✗ dynamicgraph::Vector& AngleEstimator::compute_qdotSOUT(
363 dynamicgraph::Vector& res, const int& time) {
364 ✗ const dynamicgraph::Vector& dq = qdotSIN(time);
365 ✗ const dynamicgraph::Vector& dx = xff_dotSOUT(time);
366
367 ✗ assert(dx.size() == 6);
368
369 ✗ const Eigen::DenseIndex nr = dq.size();
370 ✗ res.resize(nr);
371 ✗ res = dq;
372 ✗ for (int i = 0; i < 6; ++i) res(i) = dx(i);
373
374 ✗ return res;
375 }
376