Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/integrator-force-exact.cpp	Lines:	0	90	0.0 %
Date:	2023-03-28 11:05:13	Branches:	0	204	0.0 %


/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#include <dynamic-graph/factory.h>
#include <sot/dynamic-pinocchio/integrator-force-exact.h>

#include <sot/core/debug.hh>
#include <sot/core/exception-dynamic.hh>

using namespace dynamicgraph::sot;
using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(IntegratorForceExact,
                                   "IntegratorForceExact");

IntegratorForceExact::IntegratorForceExact(const std::string& name)
    : IntegratorForce(name) {
  sotDEBUGIN(5);

  velocitySOUT.setFunction(
      boost::bind(&IntegratorForceExact::computeVelocityExact, this, _1, _2));
  velocitySOUT.removeDependency(velocityDerivativeSOUT);

  sotDEBUGOUT(5);
}

IntegratorForceExact::~IntegratorForceExact(void) {
  sotDEBUGIN(5);

  sotDEBUGOUT(5);
  return;
}

/* --- EIGEN VALUE ---------------------------------------------------------- */
/* --- EIGEN VALUE ---------------------------------------------------------- */
/* --- EIGEN VALUE ---------------------------------------------------------- */

extern "C" {
void dgeev_(const char* jobvl, const char* jobvr, const int* n, double* a,
            const int* lda, double* wr, double* wi, double* vl, const int* ldvl,
            double* vr, const int* ldvr, double* work, const int* lwork,
            int* info);
}

int geev(Matrix& a, Eigen::VectorXcd& w, Matrix& vl2, Matrix& vr2) {
  char jobvl = 'V';
  char jobvr = 'V';
  int const n = (int)a.rows();

  Vector wr(n);
  Vector wi(n);
  double* vl_real = MRAWDATA(vl2);
  const int ldvl = (int)vl2.rows();
  double* vr_real = MRAWDATA(vr2);
  const int ldvr = (int)vr2.rows();

  // workspace query
  int lwork = -1;
  double work_temp;
  int result = 0;
  dgeev_(&jobvl, &jobvr, &n, MRAWDATA(a), &n, MRAWDATA(wr), MRAWDATA(wi),
         vl_real, &ldvl, vr_real, &ldvr, &work_temp, &lwork, &result);
  if (result != 0) return result;

  lwork = (int)work_temp;
  Vector work(lwork);
  dgeev_(&jobvl, &jobvr, &n, MRAWDATA(a), &n, MRAWDATA(wr), MRAWDATA(wi),
         vl_real, &ldvl, vr_real, &ldvr, MRAWDATA(work), &lwork, &result);

  for (int i = 0; i < n; i++) w[i] = std::complex<double>(wr[i], wi[i]);

  return result;
}

static void eigenDecomp(const dynamicgraph::Matrix& M, dynamicgraph::Matrix& P,
                        dynamicgraph::Vector& eig) {
  long int SIZE = M.cols();
  Matrix Y(M);
  Eigen::VectorXcd evals(SIZE);
  Matrix vl(SIZE, SIZE);
  Matrix vr(SIZE, SIZE);

  //  const int errCode = lapack::geev(Y, evals, &vl, &vr,
  //  lapack::optimal_workspace());
  const int errCode = geev(Y, evals, vl, vr);
  if (errCode < 0) {
    SOT_THROW ExceptionDynamic(ExceptionDynamic::INTEGRATION,
                               "Invalid argument to geev", "");
  } else if (errCode > 0) {
    SOT_THROW ExceptionDynamic(ExceptionDynamic::INTEGRATION,
                               "No convergence for given matrix", "");
  }

  P.resize(SIZE, SIZE);
  eig.resize(SIZE);
  for (unsigned int i = 0; i < SIZE; ++i) {
    for (unsigned int j = 0; j < SIZE; ++j) {
      P(i, j) = vr(i, j);
    }
    eig(i) = evals(i).real();
    if (fabsf(static_cast<float>(evals(i).imag())) > 1e-5) {
      SOT_THROW ExceptionDynamic(ExceptionDynamic::INTEGRATION,
                                 "Error imaginary part not null. ",
                                 "(at position %d: %f)", i, evals(i).imag());
    }
  }
  return;
}

static void expMatrix(const dynamicgraph::Matrix& MiB,
                      dynamicgraph::Matrix& Mexp) {
  long int SIZE = MiB.cols();

  dynamicgraph::Matrix Pmib(MiB.cols(), MiB.cols());
  dynamicgraph::Vector eig_mib(MiB.cols());
  eigenDecomp(MiB, Pmib, eig_mib);
  sotDEBUG(45) << "V = " << Pmib;
  sotDEBUG(45) << "d = " << eig_mib;

  dynamicgraph::Matrix Pinv(SIZE, SIZE);
  Pinv = Pmib.inverse();
  sotDEBUG(45) << "Vinv = " << Pinv;

  Mexp.resize(SIZE, SIZE);
  for (unsigned int i = 0; i < SIZE; ++i)
    for (unsigned int j = 0; j < SIZE; ++j) Pmib(i, j) *= exp(eig_mib(j));
  Mexp = Pmib * Pinv;

  sotDEBUG(45) << "expMiB = " << Mexp;
  return;
}

/* --- SIGNALS -------------------------------------------------------------- */
/* --- SIGNALS -------------------------------------------------------------- */
/* --- SIGNALS -------------------------------------------------------------- */

/* The derivative of the signal is such that: M v_dot + B v = f. We deduce:
 * v_dot =  M^-1 (f - Bv)
 * Using Exact method:
 * dv = exp( M^-1.B.t) ( v0-B^-1.f ) + B^-1.f
 */

dynamicgraph::Vector& IntegratorForceExact::computeVelocityExact(
    dynamicgraph::Vector& res, const int& time) {
  sotDEBUGIN(15);

  const dynamicgraph::Vector& force = forceSIN(time);
  const dynamicgraph::Matrix& massInverse = massInverseSIN(time);
  const dynamicgraph::Matrix& friction = frictionSIN(time);
  long int nf = force.size(), nv = friction.cols();
  res.resize(nv);
  res.setZero();

  if (!velocityPrecSIN) {
    dynamicgraph::Vector zero(nv);
    zero.fill(0);
    velocityPrecSIN = zero;
  }
  const dynamicgraph::Vector& vel = velocityPrecSIN(time);
  double& dt = this->IntegratorForce::timeStep;  // this is &

  sotDEBUG(15) << "force = " << force;
  sotDEBUG(15) << "vel = " << vel;
  sotDEBUG(25) << "Mi = " << massInverse;
  sotDEBUG(25) << "B = " << friction;
  sotDEBUG(25) << "dt = " << dt << std::endl;

  dynamicgraph::Matrix MiB(nv, nv);
  MiB = massInverse * friction;
  sotDEBUG(25) << "MiB = " << MiB;

  dynamicgraph::Matrix MiBexp(nv, nv);
  MiB *= -dt;
  expMatrix(MiB, MiBexp);
  sotDEBUG(25) << "expMiB = " << MiBexp;

  dynamicgraph::Matrix Binv(nv, nv);
  Binv = friction.inverse();
  dynamicgraph::Vector Bif(nf);
  Bif = Binv * force;
  sotDEBUG(25) << "Binv = " << Binv;
  sotDEBUG(25) << "Bif = " << Bif;

  dynamicgraph::Vector v0_bif = vel;
  v0_bif -= Bif;
  sotDEBUG(25) << "Kst = " << v0_bif;

  res.resize(MiBexp.rows());
  res = MiBexp * v0_bif;

  res += Bif;
  velocityPrecSIN = res;
  sotDEBUG(25) << "vfin = " << res;

  sotDEBUGOUT(15);
  return res;
}


Generated by: GCOVR (Version 4.2)

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			}