GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/mass-apparent.cpp Lines: 0 33 0.0 %
Date: 2023-01-28 11:04:48 Branches: 0 112 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/mass-apparent.h>
12
13
#include <sot/core/debug.hh>
14
15
using namespace dynamicgraph::sot;
16
using namespace dynamicgraph;
17
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MassApparent, "MassApparent");
18
19
MassApparent::MassApparent(const std::string& name)
20
    : Entity(name),
21
      jacobianSIN(NULL,
22
                  "sotMassApparent(" + name + ")::input(vector)::jacobian"),
23
      inertiaInverseSIN(
24
          NULL, "sotMassApparent(" + name + ")::input(vector)::inertiaInverse"),
25
      massInverseSOUT(
26
          boost::bind(&MassApparent::computeMassInverse, this, _1, _2),
27
          jacobianSIN << inertiaInverseSIN,
28
          "sotMassApparent(" + name + ")::output(Vector)::massInverse"),
29
      massSOUT(boost::bind(&MassApparent::computeMass, this, _1, _2),
30
               massInverseSOUT,
31
               "sotMassApparent(" + name + ")::output(Vector)::mass")
32
33
      ,
34
      inertiaSIN(NULL, "sotMassApparent(" + name + ")::input(vector)::inertia"),
35
      inertiaInverseSOUT(
36
          boost::bind(&MassApparent::computeInertiaInverse, this, _1, _2),
37
          inertiaSIN,
38
          "sotMassApparent(" + name + ")::input(vector)::inertiaInverseOUT") {
39
  sotDEBUGIN(5);
40
41
  signalRegistration(jacobianSIN);
42
  signalRegistration(inertiaInverseSIN);
43
  signalRegistration(massInverseSOUT);
44
  signalRegistration(massSOUT);
45
  signalRegistration(inertiaSIN);
46
  signalRegistration(inertiaInverseSOUT);
47
  inertiaInverseSIN.plug(&inertiaInverseSOUT);
48
49
  sotDEBUGOUT(5);
50
}
51
52
MassApparent::~MassApparent(void) { return; }
53
54
/* --- SIGNALS -------------------------------------------------------------- */
55
/* --- SIGNALS -------------------------------------------------------------- */
56
/* --- SIGNALS -------------------------------------------------------------- */
57
dynamicgraph::Matrix& MassApparent::computeMassInverse(
58
    dynamicgraph::Matrix& res, const int& time) {
59
  sotDEBUGIN(15);
60
61
  const dynamicgraph::Matrix& J = jacobianSIN(time);
62
  const dynamicgraph::Matrix& A = inertiaInverseSIN(time);
63
64
  dynamicgraph::Matrix AJt(J.cols(), J.rows());
65
  AJt = A * J.transpose();
66
67
  res.resize(J.rows(), J.rows());
68
  res = J * AJt;
69
70
  sotDEBUGOUT(15);
71
  return res;
72
}
73
74
dynamicgraph::Matrix& MassApparent::computeMass(dynamicgraph::Matrix& res,
75
                                                const int& time) {
76
  sotDEBUGIN(15);
77
78
  const dynamicgraph::Matrix& omega = massInverseSOUT(time);
79
  res = omega.inverse();
80
81
  sotDEBUGOUT(15);
82
  return res;
83
}
84
85
dynamicgraph::Matrix& MassApparent::computeInertiaInverse(
86
    dynamicgraph::Matrix& res, const int& time) {
87
  sotDEBUGIN(15);
88
89
  const dynamicgraph::Matrix& A = inertiaSIN(time);
90
  res = A.inverse();
91
92
  sotDEBUGOUT(15);
93
  return res;
94
}