GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: tests/features/test_feature_generic.cpp Lines: 344 353 97.5 %
Date: 2023-03-13 12:09:37 Branches: 738 1454 50.8 %

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 <iostream>
14
#include <pinocchio/algorithm/frames.hpp>
15
#include <pinocchio/algorithm/jacobian.hpp>
16
#include <pinocchio/algorithm/joint-configuration.hpp>
17
#include <pinocchio/algorithm/kinematics.hpp>
18
#include <pinocchio/multibody/data.hpp>
19
#include <pinocchio/multibody/liegroup/liegroup.hpp>
20
#include <pinocchio/multibody/model.hpp>
21
#include <pinocchio/parsers/sample-models.hpp>
22
23
#define BOOST_TEST_MODULE features
24
#include <dynamic-graph/factory.h>
25
#include <dynamic-graph/linear-algebra.h>
26
27
#include <Eigen/SVD>
28
#include <boost/test/unit_test.hpp>
29
#include <sot/core/debug.hh>
30
#include <sot/core/feature-abstract.hh>
31
#include <sot/core/feature-generic.hh>
32
#include <sot/core/feature-pose.hh>
33
#include <sot/core/gain-adaptive.hh>
34
#include <sot/core/sot.hh>
35
#include <sot/core/task.hh>
36
37
namespace dynamicgraph {
38
namespace sot {
39
namespace dg = dynamicgraph;
40
41
typedef pinocchio::CartesianProductOperation<
42
    pinocchio::VectorSpaceOperationTpl<3, double>,
43
    pinocchio::SpecialOrthogonalOperationTpl<3, double> >
44
    R3xSO3_t;
45
typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t;
46
47
namespace internal {
48
template <Representation_t representation>
49
struct LG_t {
50
  typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
51
                                    R3xSO3_t>::type type;
52
};
53
}  // namespace internal
54
}  // namespace sot
55
}  // namespace dynamicgraph
56
57
using namespace std;
58
using namespace dynamicgraph::sot;
59
using namespace dynamicgraph;
60
61
#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision)         \
62
  BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision),       \
63
                      "check " #Va ".isApprox(" #Vb       \
64
                      ") failed "                         \
65
                      "[\n"                               \
66
                          << (Va).transpose() << "\n!=\n" \
67
                          << (Vb).transpose() << "\n]")
68
#define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision)                           \
69
  BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), "check " #Va            \
70
                                                    ".isApprox(" #Vb        \
71
                                                    ") failed "             \
72
                                                    "[\n"                   \
73
                                                        << (Va) << "\n!=\n" \
74
                                                        << (Vb) << "\n]")
75
76
class FeatureTestBase {
77
 public:
78
  Task task_;
79
  int time_;
80
  dynamicgraph::Vector expectedTaskOutput_;
81
82
5
  FeatureTestBase(unsigned dim, const std::string &name)
83

5
      : task_("task" + name), time_(0) {
84
5
    expectedTaskOutput_.resize(dim);
85
5
  }
86
87
5
  virtual void init() {
88
5
    task_.addFeature(featureAbstract());
89
5
    task_.setWithDerivative(true);
90
5
  }
91
92
26
  void computeExpectedTaskOutput(const Vector &error,
93
                                 const Vector &errorDrift) {
94
26
    double gain = task_.controlGainSIN;
95

26
    expectedTaskOutput_ = -gain * error - errorDrift;
96
26
  }
97
98
  template <typename LG_t>
99
32
  void computeExpectedTaskOutput(const Vector &s, const Vector &sdes,
100
                                 const Vector &sDesDot, const LG_t &lg) {
101

32
    Vector s_sd(lg.nv());
102
32
    lg.difference(sdes, s, s_sd);
103
104

32
    Eigen::Matrix<typename LG_t::Scalar, LG_t::NV, LG_t::NV> Jminus(lg.nv(),
105
                                                                    lg.nv());
106
32
    lg.template dDifference<pinocchio::ARG0>(sdes, s, Jminus);
107
108

32
    computeExpectedTaskOutput(s_sd, Jminus * sDesDot);
109
32
  }
110
111
26
  void checkTaskOutput() {
112



26
    BOOST_REQUIRE_EQUAL(task_.taskSOUT.accessCopy().size(),
113
                        expectedTaskOutput_.size());
114

52
    Vector taskOutput(expectedTaskOutput_.size());
115

182
    for (int i = 0; i < expectedTaskOutput_.size(); ++i)
116

156
      taskOutput[i] = task_.taskSOUT.accessCopy()[i].getSingleBound();
117






26
    EIGEN_VECTOR_IS_APPROX(taskOutput, expectedTaskOutput_, 1e-6);
118
26
  }
119
120
58
  void setGain(double gain) {
121
58
    task_.controlGainSIN = gain;
122
58
    task_.controlGainSIN.access(time_);
123
58
    task_.controlGainSIN.setReady();
124
58
  }
125
126
  template <typename SignalType, typename ValueType>
127
2280
  void setSignal(SignalType &sig, const ValueType &v) {
128
2280
    sig = v;
129
2280
    sig.access(time_);
130
2280
    sig.setReady();
131
2280
  }
132
133
  virtual FeatureAbstract &featureAbstract() = 0;
134
135
  virtual void setInputs() = 0;
136
137
  virtual void printInputs() {}
138
139
26
  virtual void recompute() {
140
26
    task_.taskSOUT.recompute(time_);
141
142
    // Check that recomputing went fine.
143
26
    FeatureAbstract &f(featureAbstract());
144


26
    BOOST_CHECK_EQUAL(time_, f.errorSOUT.getTime());
145


26
    BOOST_CHECK_EQUAL(time_, f.errordotSOUT.getTime());
146


26
    BOOST_CHECK_EQUAL(time_, task_.errorSOUT.getTime());
147


26
    BOOST_CHECK_EQUAL(time_, task_.errorTimeDerivativeSOUT.getTime());
148
149
    // TODO check that the output is correct
150
    // computeExpectedTaskOutput (s - sd, - vd);
151
26
  }
152
153
  virtual void printOutputs() {}
154
155
26
  virtual void checkValue() {
156
26
    time_++;
157
26
    setInputs();
158
26
    printInputs();
159
26
    recompute();
160
26
    printOutputs();
161
26
  }
162
};
163
164
class TestFeatureGeneric : public FeatureTestBase {
165
 public:
166
  FeatureGeneric feature_, featureDes_;
167
  int dim_;
168
169
1
  TestFeatureGeneric(unsigned dim, const std::string &name)
170
1
      : FeatureTestBase(dim, name),
171
1
        feature_("feature" + name),
172
1
        featureDes_("featureDes" + name),
173


2
        dim_(dim) {
174
1
    feature_.setReference(&featureDes_);
175

1
    feature_.selectionSIN = Flags(true);
176
177
2
    dynamicgraph::Matrix Jq(dim, dim);
178
1
    Jq.setIdentity();
179
1
    feature_.jacobianSIN.setReference(&Jq);
180
181
1
    init();
182
1
  }
183
184
11
  FeatureAbstract &featureAbstract() { return feature_; }
185
186
10
  void setInputs() {
187

20
    dynamicgraph::Vector s(dim_), sd(dim_), vd(dim_);
188
    double gain;
189
190

10
    switch (time_) {
191
      case 0:
192
        BOOST_TEST_MESSAGE(" ----- Test Velocity -----");
193
        s.setZero();
194
        sd.setZero();
195
        vd.setZero();
196
        gain = 0.0;
197
        break;
198
1
      case 1:
199


1
        BOOST_TEST_MESSAGE(" ----- Test Position -----");
200
1
        s.setZero();
201
1
        sd.setConstant(2.);
202
1
        vd.setZero();
203
1
        gain = 1.0;
204
1
        break;
205
1
      case 2:
206


1
        BOOST_TEST_MESSAGE(" ----- Test both -----");
207
1
        s.setZero();
208
1
        sd.setConstant(2.);
209
1
        vd.setConstant(1.);
210
1
        gain = 3.0;
211
1
        break;
212
8
      default:
213
8
        s.setRandom();
214
8
        sd.setRandom();
215
8
        vd.setRandom();
216
8
        gain = 1.0;
217
    }
218
219
10
    feature_.errorSIN = s;
220
10
    feature_.errorSIN.access(time_);
221
10
    feature_.errorSIN.setReady();
222
223
10
    featureDes_.errorSIN = sd;
224
10
    featureDes_.errorSIN.access(time_);
225
10
    featureDes_.errorSIN.setReady();
226
227
10
    featureDes_.errordotSIN = vd;
228
10
    featureDes_.errordotSIN.access(time_);
229
10
    featureDes_.errordotSIN.setReady();
230
231
10
    setGain(gain);
232
10
  }
233
234
10
  void printInputs() {
235


10
    BOOST_TEST_MESSAGE("----- inputs -----");
236




10
    BOOST_TEST_MESSAGE(
237
        "feature_.errorSIN: " << feature_.errorSIN(time_).transpose());
238




10
    BOOST_TEST_MESSAGE(
239
        "featureDes_.errorSIN: " << featureDes_.errorSIN(time_).transpose());
240




10
    BOOST_TEST_MESSAGE("featureDes_.errordotSIN: "
241
                       << featureDes_.errordotSIN(time_).transpose());
242



10
    BOOST_TEST_MESSAGE("task.controlGain: " << task_.controlGainSIN(time_));
243
10
  }
244
245
10
  void recompute() {
246
10
    FeatureTestBase::recompute();
247
248

20
    dynamicgraph::Vector s = feature_.errorSIN;
249

20
    dynamicgraph::Vector sd = featureDes_.errorSIN;
250

20
    dynamicgraph::Vector vd = featureDes_.errordotSIN;
251
252


10
    computeExpectedTaskOutput(s - sd, -vd);
253
254
10
    checkTaskOutput();
255
10
  }
256
257
10
  void printOutputs() {
258


10
    BOOST_TEST_MESSAGE("----- output -----");
259



10
    BOOST_TEST_MESSAGE("time: " << time_);
260




10
    BOOST_TEST_MESSAGE(
261
        "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose());
262




10
    BOOST_TEST_MESSAGE(
263
        "feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose());
264



10
    BOOST_TEST_MESSAGE("task.taskSOUT: " << task_.taskSOUT(time_));
265
    // BOOST_TEST_MESSAGE("task.errorSOUT: " << task_.errorSOUT(time_));
266
    // BOOST_TEST_MESSAGE("task.errordtSOUT: " <<
267
    // task_.errorTimeDerivativeSOUT(time_));
268



10
    BOOST_TEST_MESSAGE(
269
        "Expected task output: " << expectedTaskOutput_.transpose());
270
10
  }
271
};
272
273
BOOST_AUTO_TEST_SUITE(feature_generic)
274
275
















4
BOOST_AUTO_TEST_CASE(check_value) {
276
4
  std::string srobot("test");
277
2
  unsigned int dim = 6;
278
279
4
  TestFeatureGeneric testFeatureGeneric(dim, srobot);
280
281

22
  for (int i = 0; i < 10; ++i) testFeatureGeneric.checkValue();
282
2
}
283
284
BOOST_AUTO_TEST_SUITE_END()  // feature_generic
285
286
48
MatrixHomogeneous randomM() {
287
48
  MatrixHomogeneous M;
288

48
  M.translation().setRandom();
289

48
  Eigen::Vector3d w(Eigen::Vector3d::Random());
290



48
  M.linear() = Eigen::AngleAxisd(w.norm(), w.normalized()).matrix();
291
96
  return M;
292
}
293
294
typedef pinocchio::SE3 SE3;
295
296
32
Vector7 toVector(const pinocchio::SE3 &M) {
297
32
  Vector7 v;
298
32
  v.head<3>() = M.translation();
299

32
  QuaternionMap(v.tail<4>().data()) = M.rotation();
300
32
  return v;
301
}
302
303
96
Vector toVector(const std::vector<MultiBound> &in) {
304
96
  Vector out(in.size());
305

672
  for (int i = 0; i < (int)in.size(); ++i) out[i] = in[i].getSingleBound();
306
96
  return out;
307
}
308
309
template <Representation_t representation>
310
class TestFeaturePose : public FeatureTestBase {
311
 public:
312
  typedef typename dg::sot::internal::LG_t<representation>::type LieGroup_t;
313
  FeaturePose<representation> feature_;
314
  bool relative_;
315
  pinocchio::Model model_;
316
  pinocchio::Data data_;
317
  pinocchio::JointIndex ja_, jb_;
318
  pinocchio::FrameIndex fa_, fb_;
319
320
8
  TestFeaturePose(bool relative, const std::string &name)
321
      : FeatureTestBase(6, name),
322
        feature_("feature" + name),
323
        relative_(relative),
324


8
        data_(model_) {
325
8
    pinocchio::buildModels::humanoid(model_, true);  // use freeflyer
326

8
    model_.lowerPositionLimit.head<3>().setConstant(-1.);
327

8
    model_.upperPositionLimit.head<3>().setConstant(1.);
328

8
    jb_ = model_.getJointId("rarm_wrist2_joint");
329



16
    fb_ = model_.addFrame(pinocchio::Model::Frame(
330
        "frame_b", jb_,
331
8
        model_.getFrameId("rarm_wrist2_joint", pinocchio::JOINT),
332
        SE3::Identity(), pinocchio::OP_FRAME));
333
8
    if (relative) {
334

4
      ja_ = model_.getJointId("larm_wrist2_joint");
335



8
      fa_ = model_.addFrame(pinocchio::Model::Frame(
336
          "frame_a", ja_,
337
8
          model_.getFrameId("larm_wrist2_joint", pinocchio::JOINT),
338
          SE3::Identity(), pinocchio::OP_FRAME));
339
    } else {
340
4
      ja_ = 0;
341


4
      fa_ = model_.addFrame(pinocchio::Model::Frame(
342
          "frame_a", 0, 0, SE3::Identity(), pinocchio::OP_FRAME));
343
    }
344

8
    data_ = pinocchio::Data(model_);
345
346
8
    init();
347
348
8
    setJointFrame();
349
8
  }
350
351
20
  void _setFrame() {
352
20
    setSignal(
353
20
        feature_.jaMfa,
354
20
        MatrixHomogeneous(model_.frames[fa_].placement.toHomogeneousMatrix()));
355
20
    setSignal(
356
20
        feature_.jbMfb,
357
20
        MatrixHomogeneous(model_.frames[fb_].placement.toHomogeneousMatrix()));
358
20
  }
359
360
12
  void setJointFrame() {
361
12
    model_.frames[fa_].placement.setIdentity();
362
12
    model_.frames[fb_].placement.setIdentity();
363
12
    _setFrame();
364
12
  }
365
366
8
  void setRandomFrame() {
367
8
    model_.frames[fa_].placement.setRandom();
368
8
    model_.frames[fb_].placement.setRandom();
369
8
    _setFrame();
370
8
  }
371
372
40
  FeatureAbstract &featureAbstract() { return feature_; }
373
374
32
  void setInputs() {
375
64
    Vector q(pinocchio::randomConfiguration(model_));
376
32
    pinocchio::framesForwardKinematics(model_, data_, q);
377
32
    pinocchio::computeJointJacobians(model_, data_);
378
379
    // Poses
380

32
    setSignal(feature_.oMjb,
381
32
              MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
382
32
    if (relative_) {
383

16
      setSignal(feature_.oMja,
384
16
                MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
385
    }
386
387
    // Jacobians
388
64
    Matrix J(6, model_.nv);
389
32
    J.setZero();
390
32
    pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
391
32
    setSignal(feature_.jbJjb, J);
392
32
    if (relative_) {
393
16
      J.setZero();
394
16
      pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
395
16
      setSignal(feature_.jaJja, J);
396
    }
397
398
    // Desired
399

32
    setSignal(feature_.faMfbDes, randomM());
400

32
    setSignal(feature_.faNufafbDes, Vector::Random(6));
401
402
32
    double gain = 0;
403
    // if (time_ % 5 != 0)
404
    // gain = 2 * (double)rand() / RAND_MAX;
405
32
    if (time_ % 2 != 0) gain = 1;
406
32
    setGain(gain);
407
32
  }
408
409
32
  void printInputs() {
410


32
    BOOST_TEST_MESSAGE("----- inputs -----");
411
    // BOOST_TEST_MESSAGE("feature_.errorSIN: " <<
412
    // feature_.errorSIN(time_).transpose());
413



32
    BOOST_TEST_MESSAGE("task.controlGain: " << task_.controlGainSIN(time_));
414
32
  }
415
416
32
  void recompute() {
417
32
    FeatureTestBase::recompute();
418
419

32
    const SE3 oMfb = data_.oMf[fb_], oMfa = data_.oMf[fa_],
420
32
              faMfb(feature_.faMfb.accessCopy().matrix()),
421

32
              faMfbDes(feature_.faMfbDes.accessCopy().matrix());
422
32
    const Vector &nu(feature_.faNufafbDes.accessCopy());
423
424




64
    computeExpectedTaskOutput(
425
        toVector(oMfa.inverse() * oMfb), toVector(faMfbDes),
426
        (boost::is_same<LieGroup_t, SE3_t>::value
427

16
             ? faMfbDes.toActionMatrixInverse() * nu
428
             : (Vector6d() << nu.head<3>() -
429





32
                                  faMfb.translation().cross(nu.tail<3>()),
430

32
                faMfbDes.rotation().transpose() * nu.tail<3>())
431

16
                   .finished()),
432
        LieGroup_t());
433
434
32
    checkTaskOutput();
435
32
  }
436
437
32
  void printOutputs() {
438


32
    BOOST_TEST_MESSAGE("----- output -----");
439



32
    BOOST_TEST_MESSAGE("time: " << time_);
440




32
    BOOST_TEST_MESSAGE(
441
        "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose());
442




32
    BOOST_TEST_MESSAGE(
443
        "feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose());
444



32
    BOOST_TEST_MESSAGE("task.taskSOUT: " << task_.taskSOUT(time_));
445
    // BOOST_TEST_MESSAGE("task.errorSOUT: " << task_.errorSOUT(time_));
446
    // BOOST_TEST_MESSAGE("task.errordtSOUT: " <<
447
    // task_.errorTimeDerivativeSOUT(time_));
448



32
    BOOST_TEST_MESSAGE(
449
        "Expected task output: " << expectedTaskOutput_.transpose());
450
32
  }
451
452
32
  virtual void checkJacobian() {
453
32
    time_++;
454
    // We want to check that e (q+dq, t) ~ e(q, t) + de/dq(q,t) dq
455
456
32
    setGain(1.);
457
458
64
    Vector q(pinocchio::randomConfiguration(model_));
459
32
    pinocchio::framesForwardKinematics(model_, data_, q);
460
32
    pinocchio::computeJointJacobians(model_, data_);
461
462
    // Poses
463

32
    setSignal(feature_.oMjb,
464
32
              MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
465
32
    if (relative_) {
466

16
      setSignal(feature_.oMja,
467
16
                MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
468
    }
469
470
    // Jacobians
471
64
    Matrix J(6, model_.nv);
472
32
    J.setZero();
473
32
    pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
474
32
    setSignal(feature_.jbJjb, J);
475
32
    if (relative_) {
476
16
      J.setZero();
477
16
      pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
478
16
      setSignal(feature_.jaJja, J);
479
    }
480
481
    // Desired
482

32
    setSignal(feature_.faMfbDes, randomM());
483
484
    // Get task jacobian
485

64
    Vector e_q = task_.errorSOUT.access(time_);
486

32
    J = task_.jacobianSOUT.access(time_);
487
488



96
    Eigen::IOFormat PyVectorFmt(Eigen::FullPrecision, 0, ", ", ", ", "", "",
489
                                "[", "]\n");
490



96
    Eigen::IOFormat PyMatrixFmt(Eigen::FullPrecision, 0, ", ", ",\n", "[", "]",
491
                                "[", "]\n");
492
493

32
    Vector qdot(Vector::Zero(model_.nv));
494
32
    double eps = 1e-6;
495









32
    BOOST_TEST_MESSAGE(
496
        data_.oMi[ja_].toHomogeneousMatrix().format(PyMatrixFmt)
497
        << data_.oMi[jb_].toHomogeneousMatrix().format(PyMatrixFmt)
498
        << model_.frames[fa_].placement.toHomogeneousMatrix().format(
499
               PyMatrixFmt)
500
        << model_.frames[fb_].placement.toHomogeneousMatrix().format(
501
               PyMatrixFmt)
502
        << J.format(PyMatrixFmt));
503
504
1120
    for (int i = 0; i < model_.nv; ++i) {
505
1088
      time_++;
506
1088
      qdot(i) = eps;
507
508
      // Update pose signals
509
2176
      Vector q_qdot = pinocchio::integrate(model_, q, qdot);
510
1088
      pinocchio::framesForwardKinematics(model_, data_, q_qdot);
511

1088
      setSignal(feature_.oMjb,
512
1088
                MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
513
1088
      if (relative_) {
514

544
        setSignal(feature_.oMja,
515
544
                  MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
516
      }
517
518
      // Recompute output and check finite diff
519

1088
      Vector e_q_dq = task_.errorSOUT.access(time_);
520











1088
      BOOST_CHECK_MESSAGE(
521
          (((e_q_dq - e_q) / eps) - J.col(i)).maxCoeff() < 1e-4,
522
          "Jacobian col " << i << " does not match finite difference.\n"
523
                          << ((e_q_dq - e_q) / eps).format(PyVectorFmt) << '\n'
524
                          << J.col(i).format(PyVectorFmt) << '\n');
525
526
1088
      qdot(i) = 0.;
527
    }
528
32
    time_++;
529
32
  }
530
531
32
  virtual void checkFeedForward() {
532
32
    setGain(0.);
533
    // random config
534
    // set inputs
535
    // compute e = task_.taskSOUT and J = task_.jacobianSOUT
536
    // check that e (q + eps*qdot) - e (q) ~= eps * J * qdot
537
    // compute qdot = J^+ * e
538
    // check that faMfb (q+eps*qdot) ~= faMfb(q) + eps * faNufafbDes
539
540
32
    time_++;
541
64
    Vector q(pinocchio::randomConfiguration(model_));
542
32
    pinocchio::framesForwardKinematics(model_, data_, q);
543
32
    pinocchio::computeJointJacobians(model_, data_);
544
545
32
    SE3 faMfb = data_.oMf[fa_].actInv(data_.oMf[fb_]);
546
547
    // Poses
548

32
    setSignal(feature_.oMjb,
549
32
              MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
550
32
    if (relative_) {
551

16
      setSignal(feature_.oMja,
552
16
                MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
553
    }
554
555
    // Jacobians
556
64
    Matrix J(6, model_.nv);
557
32
    J.setZero();
558
32
    pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
559
32
    setSignal(feature_.jbJjb, J);
560
32
    if (relative_) {
561
16
      J.setZero();
562
16
      pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
563
16
      setSignal(feature_.jaJja, J);
564
    }
565
566
64
    Matrix faJfafb;
567
32
    J.setZero();
568
32
    pinocchio::getFrameJacobian(model_, data_, fb_, pinocchio::LOCAL, J);
569

32
    faJfafb = faMfb.toActionMatrix() * J;
570
32
    J.setZero();
571
32
    pinocchio::getFrameJacobian(model_, data_, fa_, pinocchio::LOCAL, J);
572
32
    faJfafb -= J;
573
574
    // Desired
575

32
    setSignal(feature_.faMfbDes, randomM());
576
577
    // Get task jacobian
578
32
    task_.jacobianSOUT.recompute(time_);
579
32
    J = task_.jacobianSOUT.accessCopy();
580
64
    Eigen::JacobiSVD<Matrix> svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV);
581
582

32
    Vector faNufafbDes(Vector::Zero(6));
583
32
    double eps = 1e-6;
584
224
    for (int i = 0; i < 6; ++i) {
585
192
      time_++;
586
192
      faNufafbDes(i) = 1.;
587
192
      setSignal(feature_.faNufafbDes, faNufafbDes);
588
192
      task_.taskSOUT.recompute(time_);
589
590

384
      Vector qdot = svd.solve(toVector(task_.taskSOUT.accessCopy()));
591
592

192
      Vector faNufafb = faJfafb * qdot;
593






192
      EIGEN_VECTOR_IS_APPROX(faNufafbDes, faNufafb, eps);
594
595
192
      faNufafbDes(i) = 0.;
596
    }
597
32
    time_++;
598
32
  }
599
};
600
601
template <typename TestClass>
602
16
void runTest(TestClass &runner, int N = 2)
603
// int N = 10)
604
{
605
48
  for (int i = 0; i < N; ++i) runner.checkValue();
606
48
  for (int i = 0; i < N; ++i) runner.checkJacobian();
607
48
  for (int i = 0; i < N; ++i) runner.checkFeedForward();
608
16
}
609
610
BOOST_AUTO_TEST_SUITE(feature_pose)
611
612
template <Representation_t representation>
613
4
void feature_pose_absolute_tpl(const std::string &repr) {
614



4
  BOOST_TEST_MESSAGE("absolute " << repr);
615

8
  TestFeaturePose<representation> testAbsolute(false, "abs" + repr);
616
4
  testAbsolute.setJointFrame();
617
4
  runTest(testAbsolute);
618
619
4
  testAbsolute.setRandomFrame();
620
4
  runTest(testAbsolute);
621
4
}
622
623
BOOST_AUTO_TEST_SUITE(absolute)
624
625
















4
BOOST_AUTO_TEST_CASE(r3xso3) {
626

2
  feature_pose_absolute_tpl<R3xSO3Representation>("R3xSO3");
627
2
}
628
629
















4
BOOST_AUTO_TEST_CASE(se3) {
630

2
  feature_pose_absolute_tpl<SE3Representation>("SE3");
631
2
}
632
633
BOOST_AUTO_TEST_SUITE_END()  // absolute
634
635
template <Representation_t representation>
636
4
void feature_pose_relative_tpl(const std::string &repr) {
637



4
  BOOST_TEST_MESSAGE("relative " << repr);
638

8
  TestFeaturePose<representation> testRelative(true, "rel" + repr);
639
4
  runTest(testRelative);
640
641
4
  testRelative.setRandomFrame();
642
4
  runTest(testRelative);
643
4
}
644
645
BOOST_AUTO_TEST_SUITE(relative)
646
647
















4
BOOST_AUTO_TEST_CASE(r3xso3) {
648

2
  feature_pose_relative_tpl<R3xSO3Representation>("R3xSO3");
649
2
}
650
651
















4
BOOST_AUTO_TEST_CASE(se3) {
652

2
  feature_pose_relative_tpl<SE3Representation>("SE3");
653
2
}
654
655
BOOST_AUTO_TEST_SUITE_END()  // relative
656
657
BOOST_AUTO_TEST_SUITE_END()  // feature_pose