GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/integrator-force.cpp Lines: 0 49 0.0 %
Date: 2023-01-28 11:04:48 Branches: 0 140 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.h>
12
13
#include <sot/core/debug.hh>
14
15
using namespace dynamicgraph::sot;
16
using namespace dynamicgraph;
17
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(IntegratorForce, "IntegratorForce");
18
19
const double IntegratorForce::TIME_STEP_DEFAULT = 5e-3;
20
21
IntegratorForce::IntegratorForce(const std::string& name)
22
    : Entity(name),
23
      timeStep(TIME_STEP_DEFAULT),
24
      forceSIN(NULL, "sotIntegratorForce(" + name + ")::input(vector)::force"),
25
      massInverseSIN(
26
          NULL, "sotIntegratorForce(" + name + ")::input(matrix)::massInverse"),
27
      frictionSIN(NULL,
28
                  "sotIntegratorForce(" + name + ")::input(matrix)::friction"),
29
      velocityPrecSIN(NULL,
30
                      "sotIntegratorForce(" + name + ")::input(matrix)::vprec")
31
32
      ,
33
      velocityDerivativeSOUT(
34
          boost::bind(&IntegratorForce::computeDerivative, this, _1, _2),
35
          velocityPrecSIN << forceSIN << massInverseSIN << frictionSIN,
36
          "sotIntegratorForce(" + name +
37
              ")::output(Vector)::velocityDerivative"),
38
      velocitySOUT(boost::bind(&IntegratorForce::computeIntegral, this, _1, _2),
39
                   velocityPrecSIN << velocityDerivativeSOUT,
40
                   "sotIntegratorForce(" + name + ")::output(Vector)::velocity")
41
42
      ,
43
      massSIN(NULL, "sotIntegratorForce(" + name + ")::input(matrix)::mass"),
44
      massInverseSOUT(
45
          boost::bind(&IntegratorForce::computeMassInverse, this, _1, _2),
46
          massSIN,
47
          "sotIntegratorForce(" + name + ")::input(matrix)::massInverseOUT") {
48
  sotDEBUGIN(5);
49
50
  signalRegistration(forceSIN);
51
  signalRegistration(massInverseSIN);
52
  signalRegistration(frictionSIN);
53
  signalRegistration(velocityPrecSIN);
54
  signalRegistration(velocityDerivativeSOUT);
55
  signalRegistration(velocitySOUT);
56
  signalRegistration(massInverseSOUT);
57
  signalRegistration(massSIN);
58
59
  massInverseSIN.plug(&massInverseSOUT);
60
61
  sotDEBUGOUT(5);
62
}
63
64
IntegratorForce::~IntegratorForce(void) {
65
  sotDEBUGIN(5);
66
67
  sotDEBUGOUT(5);
68
  return;
69
}
70
71
/* --- SIGNALS -------------------------------------------------------------- */
72
/* --- SIGNALS -------------------------------------------------------------- */
73
/* --- SIGNALS -------------------------------------------------------------- */
74
75
/* The derivative of the signal is such that: M v_dot + B v = f. We deduce:
76
 * v_dot =  M^-1 (f - Bv)
77
 */
78
dynamicgraph::Vector& IntegratorForce::computeDerivative(
79
    dynamicgraph::Vector& res, const int& time) {
80
  sotDEBUGIN(15);
81
82
  const dynamicgraph::Vector& force = forceSIN(time);
83
  const dynamicgraph::Matrix& massInverse = massInverseSIN(time);
84
  const dynamicgraph::Matrix& friction = frictionSIN(time);
85
86
  sotDEBUG(15) << "force = " << force << std::endl;
87
88
  dynamicgraph::Vector f_bv(force.size());
89
90
  if (velocityPrecSIN) {
91
    const dynamicgraph::Vector& vel = velocityPrecSIN(time);
92
    sotDEBUG(15) << "vel = " << vel << std::endl;
93
    f_bv = friction * vel;
94
    f_bv *= -1;
95
  } else {
96
    f_bv.fill(0);
97
  }  // vel is not set yet.
98
99
  f_bv += force;
100
  res = massInverse * f_bv;
101
102
  sotDEBUGOUT(15);
103
  return res;
104
}
105
106
dynamicgraph::Vector& IntegratorForce::computeIntegral(
107
    dynamicgraph::Vector& res, const int& time) {
108
  sotDEBUGIN(15);
109
110
  const dynamicgraph::Vector& dvel = velocityDerivativeSOUT(time);
111
  res = dvel;
112
  res *= timeStep;
113
  if (velocityPrecSIN) {
114
    const dynamicgraph::Vector& vel = velocityPrecSIN(time);
115
    res += vel;
116
  } else { /* vprec not set yet. */
117
  }
118
  velocityPrecSIN = res;
119
120
  sotDEBUGOUT(15);
121
  return res;
122
}
123
124
dynamicgraph::Matrix& IntegratorForce::computeMassInverse(
125
    dynamicgraph::Matrix& res, const int& time) {
126
  sotDEBUGIN(15);
127
128
  const dynamicgraph::Matrix& mass = massSIN(time);
129
  res = mass.inverse();
130
131
  sotDEBUGOUT(15);
132
  return res;
133
}