GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/angle-estimator.cpp Lines: 0 171 0.0 %
Date: 2023-01-28 11:04:48 Branches: 0 448 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
}