| 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 |
|
|
} |
| 203 |
|
|
|