GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/integrator-force-exact.cpp Lines: 0 90 0.0 %
Date: 2023-01-28 11:04:48 Branches: 0 204 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/factory.h>
11
#include <sot/dynamic-pinocchio/integrator-force-exact.h>
12
13
#include <sot/core/debug.hh>
14
#include <sot/core/exception-dynamic.hh>
15
16
using namespace dynamicgraph::sot;
17
using namespace dynamicgraph;
18
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(IntegratorForceExact,
19
                                   "IntegratorForceExact");
20
21
IntegratorForceExact::IntegratorForceExact(const std::string& name)
22
    : IntegratorForce(name) {
23
  sotDEBUGIN(5);
24
25
  velocitySOUT.setFunction(
26
      boost::bind(&IntegratorForceExact::computeVelocityExact, this, _1, _2));
27
  velocitySOUT.removeDependency(velocityDerivativeSOUT);
28
29
  sotDEBUGOUT(5);
30
}
31
32
IntegratorForceExact::~IntegratorForceExact(void) {
33
  sotDEBUGIN(5);
34
35
  sotDEBUGOUT(5);
36
  return;
37
}
38
39
/* --- EIGEN VALUE ---------------------------------------------------------- */
40
/* --- EIGEN VALUE ---------------------------------------------------------- */
41
/* --- EIGEN VALUE ---------------------------------------------------------- */
42
43
extern "C" {
44
void dgeev_(const char* jobvl, const char* jobvr, const int* n, double* a,
45
            const int* lda, double* wr, double* wi, double* vl, const int* ldvl,
46
            double* vr, const int* ldvr, double* work, const int* lwork,
47
            int* info);
48
}
49
50
int geev(Matrix& a, Eigen::VectorXcd& w, Matrix& vl2, Matrix& vr2) {
51
  char jobvl = 'V';
52
  char jobvr = 'V';
53
  int const n = (int)a.rows();
54
55
  Vector wr(n);
56
  Vector wi(n);
57
  double* vl_real = MRAWDATA(vl2);
58
  const int ldvl = (int)vl2.rows();
59
  double* vr_real = MRAWDATA(vr2);
60
  const int ldvr = (int)vr2.rows();
61
62
  // workspace query
63
  int lwork = -1;
64
  double work_temp;
65
  int result = 0;
66
  dgeev_(&jobvl, &jobvr, &n, MRAWDATA(a), &n, MRAWDATA(wr), MRAWDATA(wi),
67
         vl_real, &ldvl, vr_real, &ldvr, &work_temp, &lwork, &result);
68
  if (result != 0) return result;
69
70
  lwork = (int)work_temp;
71
  Vector work(lwork);
72
  dgeev_(&jobvl, &jobvr, &n, MRAWDATA(a), &n, MRAWDATA(wr), MRAWDATA(wi),
73
         vl_real, &ldvl, vr_real, &ldvr, MRAWDATA(work), &lwork, &result);
74
75
  for (int i = 0; i < n; i++) w[i] = std::complex<double>(wr[i], wi[i]);
76
77
  return result;
78
}
79
80
static void eigenDecomp(const dynamicgraph::Matrix& M, dynamicgraph::Matrix& P,
81
                        dynamicgraph::Vector& eig) {
82
  long int SIZE = M.cols();
83
  Matrix Y(M);
84
  Eigen::VectorXcd evals(SIZE);
85
  Matrix vl(SIZE, SIZE);
86
  Matrix vr(SIZE, SIZE);
87
88
  //  const int errCode = lapack::geev(Y, evals, &vl, &vr,
89
  //  lapack::optimal_workspace());
90
  const int errCode = geev(Y, evals, vl, vr);
91
  if (errCode < 0) {
92
    SOT_THROW ExceptionDynamic(ExceptionDynamic::INTEGRATION,
93
                               "Invalid argument to geev", "");
94
  } else if (errCode > 0) {
95
    SOT_THROW ExceptionDynamic(ExceptionDynamic::INTEGRATION,
96
                               "No convergence for given matrix", "");
97
  }
98
99
  P.resize(SIZE, SIZE);
100
  eig.resize(SIZE);
101
  for (unsigned int i = 0; i < SIZE; ++i) {
102
    for (unsigned int j = 0; j < SIZE; ++j) {
103
      P(i, j) = vr(i, j);
104
    }
105
    eig(i) = evals(i).real();
106
    if (fabsf(static_cast<float>(evals(i).imag())) > 1e-5) {
107
      SOT_THROW ExceptionDynamic(ExceptionDynamic::INTEGRATION,
108
                                 "Error imaginary part not null. ",
109
                                 "(at position %d: %f)", i, evals(i).imag());
110
    }
111
  }
112
  return;
113
}
114
115
static void expMatrix(const dynamicgraph::Matrix& MiB,
116
                      dynamicgraph::Matrix& Mexp) {
117
  long int SIZE = MiB.cols();
118
119
  dynamicgraph::Matrix Pmib(MiB.cols(), MiB.cols());
120
  dynamicgraph::Vector eig_mib(MiB.cols());
121
  eigenDecomp(MiB, Pmib, eig_mib);
122
  sotDEBUG(45) << "V = " << Pmib;
123
  sotDEBUG(45) << "d = " << eig_mib;
124
125
  dynamicgraph::Matrix Pinv(SIZE, SIZE);
126
  Pinv = Pmib.inverse();
127
  sotDEBUG(45) << "Vinv = " << Pinv;
128
129
  Mexp.resize(SIZE, SIZE);
130
  for (unsigned int i = 0; i < SIZE; ++i)
131
    for (unsigned int j = 0; j < SIZE; ++j) Pmib(i, j) *= exp(eig_mib(j));
132
  Mexp = Pmib * Pinv;
133
134
  sotDEBUG(45) << "expMiB = " << Mexp;
135
  return;
136
}
137
138
/* --- SIGNALS -------------------------------------------------------------- */
139
/* --- SIGNALS -------------------------------------------------------------- */
140
/* --- SIGNALS -------------------------------------------------------------- */
141
142
/* The derivative of the signal is such that: M v_dot + B v = f. We deduce:
143
 * v_dot =  M^-1 (f - Bv)
144
 * Using Exact method:
145
 * dv = exp( M^-1.B.t) ( v0-B^-1.f ) + B^-1.f
146
 */
147
148
dynamicgraph::Vector& IntegratorForceExact::computeVelocityExact(
149
    dynamicgraph::Vector& res, const int& time) {
150
  sotDEBUGIN(15);
151
152
  const dynamicgraph::Vector& force = forceSIN(time);
153
  const dynamicgraph::Matrix& massInverse = massInverseSIN(time);
154
  const dynamicgraph::Matrix& friction = frictionSIN(time);
155
  long int nf = force.size(), nv = friction.cols();
156
  res.resize(nv);
157
  res.setZero();
158
159
  if (!velocityPrecSIN) {
160
    dynamicgraph::Vector zero(nv);
161
    zero.fill(0);
162
    velocityPrecSIN = zero;
163
  }
164
  const dynamicgraph::Vector& vel = velocityPrecSIN(time);
165
  double& dt = this->IntegratorForce::timeStep;  // this is &
166
167
  sotDEBUG(15) << "force = " << force;
168
  sotDEBUG(15) << "vel = " << vel;
169
  sotDEBUG(25) << "Mi = " << massInverse;
170
  sotDEBUG(25) << "B = " << friction;
171
  sotDEBUG(25) << "dt = " << dt << std::endl;
172
173
  dynamicgraph::Matrix MiB(nv, nv);
174
  MiB = massInverse * friction;
175
  sotDEBUG(25) << "MiB = " << MiB;
176
177
  dynamicgraph::Matrix MiBexp(nv, nv);
178
  MiB *= -dt;
179
  expMatrix(MiB, MiBexp);
180
  sotDEBUG(25) << "expMiB = " << MiBexp;
181
182
  dynamicgraph::Matrix Binv(nv, nv);
183
  Binv = friction.inverse();
184
  dynamicgraph::Vector Bif(nf);
185
  Bif = Binv * force;
186
  sotDEBUG(25) << "Binv = " << Binv;
187
  sotDEBUG(25) << "Bif = " << Bif;
188
189
  dynamicgraph::Vector v0_bif = vel;
190
  v0_bif -= Bif;
191
  sotDEBUG(25) << "Kst = " << v0_bif;
192
193
  res.resize(MiBexp.rows());
194
  res = MiBexp * v0_bif;
195
196
  res += Bif;
197
  velocityPrecSIN = res;
198
  sotDEBUG(25) << "vfin = " << res;
199
200
  sotDEBUGOUT(15);
201
  return res;
202
}