GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/integrator-force-rk4.cpp Lines: 0 37 0.0 %
Date: 2023-01-28 11:04:48 Branches: 0 70 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-rk4.h>
12
13
#include <sot/core/debug.hh>
14
15
using namespace dynamicgraph::sot;
16
using namespace dynamicgraph;
17
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(IntegratorForceRK4, "IntegratorForceRK4");
18
19
IntegratorForceRK4::IntegratorForceRK4(const std::string& name)
20
    : IntegratorForce(name) {
21
  sotDEBUGIN(5);
22
23
  velocityDerivativeSOUT.setFunction(
24
      boost::bind(&IntegratorForceRK4::computeDerivativeRK4, this, _1, _2));
25
26
  sotDEBUGOUT(5);
27
}
28
29
IntegratorForceRK4::~IntegratorForceRK4(void) {
30
  sotDEBUGIN(5);
31
32
  sotDEBUGOUT(5);
33
  return;
34
}
35
36
/* --- SIGNALS -------------------------------------------------------------- */
37
/* --- SIGNALS -------------------------------------------------------------- */
38
/* --- SIGNALS -------------------------------------------------------------- */
39
40
/* The derivative of the signal is such that: M v_dot + B v = f. We deduce:
41
 * v_dot =  M^-1 (f - Bv)
42
 * Using RK4 method (doc: wikipedia ;) ): dv= dt/6 ( k1 + 2.k2 + 2.k3 + k4)
43
 */
44
static const double rk_fact[4] = {1., 2., 2., 1.};
45
46
dynamicgraph::Vector& IntegratorForceRK4::computeDerivativeRK4(
47
    dynamicgraph::Vector& res, const int& time) {
48
  sotDEBUGIN(15);
49
50
  const dynamicgraph::Vector& force = forceSIN(time);
51
  const dynamicgraph::Matrix& massInverse = massInverseSIN(time);
52
  const dynamicgraph::Matrix& friction = frictionSIN(time);
53
  long int nf = force.size(), nv = friction.cols();
54
  res.resize(nv);
55
  res.fill(0);
56
57
  if (!velocityPrecSIN) {
58
    dynamicgraph::Vector zero(nv);
59
    zero.fill(0);
60
    velocityPrecSIN = zero;
61
  }
62
  const dynamicgraph::Vector& vel = velocityPrecSIN(time);
63
  double& dt = this->IntegratorForce::timeStep;  // this is &
64
65
  sotDEBUG(15) << "force = " << force;
66
  sotDEBUG(15) << "vel = " << vel;
67
  sotDEBUG(25) << "Mi = " << massInverse;
68
  sotDEBUG(25) << "B = " << friction;
69
  sotDEBUG(25) << "dt = " << dt << std::endl;
70
71
  std::vector<dynamicgraph::Vector> v(4);
72
  dynamicgraph::Vector ki(nv), fi(nf);
73
  double sumFact = 0;
74
  v[0] = vel;
75
76
  for (unsigned int i = 0; i < 4; ++i) {
77
    sotDEBUG(35) << "v" << i << " = " << v[i];
78
    fi = friction * v[i];
79
    fi *= -1;
80
    fi += force;
81
    sotDEBUG(35) << "f" << i << " = " << fi;
82
    ki = massInverse * fi;
83
    sotDEBUG(35) << "k" << i << " = " << ki;
84
    if (i + 1 < 4) {
85
      v[i + 1] = ki;
86
      v[i + 1] *= (dt / rk_fact[i + 1]);
87
      v[i + 1] += vel;
88
    }
89
    ki *= rk_fact[i];
90
    res += ki;
91
    sotDEBUG(35) << "sum_k" << i << " = " << res;
92
    sumFact += rk_fact[i];
93
  }
94
95
  sotDEBUG(35) << "sum_ki = " << res;
96
  res *= (1 / sumFact);
97
98
  sotDEBUGOUT(15);
99
  return res;
100
}