GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/zmpreffromcom.cpp Lines: 0 24 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/zmpreffromcom.h>
12
13
#include <sot/core/debug.hh>
14
using namespace dynamicgraph::sot;
15
using namespace dynamicgraph;
16
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ZmprefFromCom, "ZmprefFromCom");
17
18
const double ZmprefFromCom::DT_DEFAULT = 5e-3;
19
const double ZmprefFromCom::FOOT_HEIGHT_DEFAULT = .105;
20
21
ZmprefFromCom::ZmprefFromCom(const std::string& name)
22
    : Entity(name),
23
      dt(DT_DEFAULT),
24
      footHeight(FOOT_HEIGHT_DEFAULT),
25
      waistPositionSIN(
26
          NULL, "sotZmprefFromCom(" + name + ")::input(MatrixHomo)::waist"),
27
      comPositionSIN(NULL,
28
                     "sotZmprefFromCom(" + name + ")::input(Vector)::com"),
29
      dcomSIN(NULL, "sotZmprefFromCom(" + name + ")::input(Vector)::dcom"),
30
      zmprefSOUT(boost::bind(&ZmprefFromCom::computeZmpref, this, _1, _2),
31
                 waistPositionSIN << comPositionSIN << dcomSIN,
32
                 "sotZmprefFromCom(" + name + ")::output(RPY)::zmpref") {
33
  sotDEBUGIN(5);
34
35
  signalRegistration(waistPositionSIN);
36
  signalRegistration(comPositionSIN);
37
  signalRegistration(dcomSIN);
38
  signalRegistration(zmprefSOUT);
39
40
  sotDEBUGOUT(5);
41
}
42
43
ZmprefFromCom::~ZmprefFromCom(void) {
44
  sotDEBUGIN(5);
45
46
  sotDEBUGOUT(5);
47
  return;
48
}
49
50
/* --- SIGNALS -------------------------------------------------------------- */
51
/* --- SIGNALS -------------------------------------------------------------- */
52
/* --- SIGNALS -------------------------------------------------------------- */
53
dynamicgraph::Vector& ZmprefFromCom::computeZmpref(dynamicgraph::Vector& res,
54
                                                   const int& time) {
55
  sotDEBUGIN(15);
56
57
  const dynamicgraph::Vector& com = comPositionSIN(time);
58
  const dynamicgraph::Vector& dcom = dcomSIN(time);
59
  const MatrixHomogeneous& oTw = waistPositionSIN(time);
60
61
  MatrixHomogeneous wTo = oTw.inverse();
62
  dynamicgraph::Vector nextComRef = dcom;
63
  nextComRef *= dt;
64
  nextComRef += com;
65
66
  nextComRef(2) = -footHeight;  // projection on the ground.
67
  res = wTo.matrix() * nextComRef;
68
69
  sotDEBUGOUT(15);
70
  return res;
71
}