GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/waist-attitude-from-sensor.cpp Lines: 0 56 0.0 %
Date: 2023-01-28 11:04:48 Branches: 0 156 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/command-getter.h>
11
#include <dynamic-graph/command-setter.h>
12
#include <dynamic-graph/command.h>
13
#include <dynamic-graph/factory.h>
14
#include <sot/dynamic-pinocchio/waist-attitude-from-sensor.h>
15
16
#include <sot/core/debug.hh>
17
18
using namespace dynamicgraph::sot;
19
using namespace dynamicgraph;
20
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(WaistAttitudeFromSensor,
21
                                   "WaistAttitudeFromSensor");
22
23
WaistAttitudeFromSensor::WaistAttitudeFromSensor(const std::string& name)
24
    : Entity(name),
25
      attitudeSensorSIN(NULL, "sotWaistAttitudeFromSensor(" + name +
26
                                  ")::input(MatrixRotation)::attitudeIN"),
27
      positionSensorSIN(NULL, "sotWaistAttitudeFromSensor(" + name +
28
                                  ")::input(matrixHomo)::position"),
29
      attitudeWaistSOUT(
30
          boost::bind(&WaistAttitudeFromSensor::computeAttitudeWaist, this, _1,
31
                      _2),
32
          attitudeSensorSIN << positionSensorSIN,
33
          "sotWaistAttitudeFromSensor(" + name + ")::output(RPY)::attitude") {
34
  sotDEBUGIN(5);
35
36
  signalRegistration(attitudeSensorSIN);
37
  signalRegistration(positionSensorSIN);
38
  signalRegistration(attitudeWaistSOUT);
39
40
  sotDEBUGOUT(5);
41
}
42
43
WaistAttitudeFromSensor::~WaistAttitudeFromSensor(void) {
44
  sotDEBUGIN(5);
45
46
  sotDEBUGOUT(5);
47
  return;
48
}
49
50
/* --- SIGNALS -------------------------------------------------------------- */
51
/* --- SIGNALS -------------------------------------------------------------- */
52
/* --- SIGNALS -------------------------------------------------------------- */
53
VectorRollPitchYaw& WaistAttitudeFromSensor::computeAttitudeWaist(
54
    VectorRollPitchYaw& res, const int& time) {
55
  sotDEBUGIN(15);
56
57
  const MatrixHomogeneous& waistMchest = positionSensorSIN(time);
58
  const MatrixRotation& worldRchest = attitudeSensorSIN(time);
59
60
  MatrixRotation waistRchest;
61
  waistRchest = waistMchest.linear();
62
  MatrixRotation chestRwaist;
63
  chestRwaist = waistRchest.transpose();
64
65
  MatrixRotation worldrchest;
66
  worldrchest = worldRchest * chestRwaist;
67
  res = (worldrchest.eulerAngles(2, 1, 0)).reverse();
68
  sotDEBUGOUT(15);
69
  return res;
70
}
71
72
/* === WaistPoseFromSensorAndContact ===================================== */
73
/* === WaistPoseFromSensorAndContact ===================================== */
74
/* === WaistPoseFromSensorAndContact ===================================== */
75
76
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(WaistPoseFromSensorAndContact,
77
                                   "WaistPoseFromSensorAndContact");
78
79
WaistPoseFromSensorAndContact::WaistPoseFromSensorAndContact(
80
    const std::string& name)
81
    : WaistAttitudeFromSensor(name),
82
      fromSensor_(false),
83
      positionContactSIN(NULL, "sotWaistPoseFromSensorAndContact(" + name +
84
                                   ")::input(matrixHomo)::contact"),
85
      positionWaistSOUT(
86
          boost::bind(&WaistPoseFromSensorAndContact::computePositionWaist,
87
                      this, _1, _2),
88
          attitudeWaistSOUT << positionContactSIN,
89
          "sotWaistPoseFromSensorAndContact(" + name +
90
              ")::output(RPY+T)::positionWaist") {
91
  sotDEBUGIN(5);
92
93
  signalRegistration(positionContactSIN);
94
  signalRegistration(positionWaistSOUT);
95
96
  // Commands
97
  std::string docstring;
98
  docstring =
99
      "    \n"
100
      "    Set flag specifying whether angles are measured from sensors or "
101
      "simulated.\n"
102
      "    \n"
103
      "      Input:\n"
104
      "        - a boolean value.\n"
105
      "    \n";
106
  addCommand(
107
      "setFromSensor",
108
      new ::dynamicgraph::command::Setter<WaistPoseFromSensorAndContact, bool>(
109
          *this, &WaistPoseFromSensorAndContact::fromSensor, docstring));
110
111
  docstring =
112
      "    \n"
113
      "    Get flag specifying whether angles are measured from sensors or "
114
      "simulated.\n"
115
      "    \n"
116
      "      No input,\n"
117
      "      return a boolean value.\n"
118
      "    \n";
119
  addCommand(
120
      "getFromSensor",
121
      new ::dynamicgraph::command::Getter<WaistPoseFromSensorAndContact, bool>(
122
          *this, &WaistPoseFromSensorAndContact::fromSensor, docstring));
123
124
  sotDEBUGOUT(5);
125
}
126
127
WaistPoseFromSensorAndContact::~WaistPoseFromSensorAndContact(void) {
128
  sotDEBUGIN(5);
129
130
  sotDEBUGOUT(5);
131
  return;
132
}
133
134
/* --- SIGNALS -------------------------------------------------------------- */
135
/* --- SIGNALS -------------------------------------------------------------- */
136
/* --- SIGNALS -------------------------------------------------------------- */
137
dynamicgraph::Vector& WaistPoseFromSensorAndContact::computePositionWaist(
138
    dynamicgraph::Vector& res, const int& time) {
139
  sotDEBUGIN(15);
140
141
  const MatrixHomogeneous& waistMcontact = positionContactSIN(time);
142
  MatrixHomogeneous contactMwaist;
143
  contactMwaist = waistMcontact.inverse();
144
145
  res.resize(6);
146
  for (unsigned int i = 0; i < 3; ++i) {
147
    res(i) = contactMwaist(i, 3);
148
  }
149
150
  if (fromSensor_) {
151
    const VectorRollPitchYaw& worldrwaist = attitudeWaistSOUT(time);
152
    for (unsigned int i = 0; i < 3; ++i) {
153
      res(i + 3) = worldrwaist(i);
154
    }
155
  } else {
156
    VectorRollPitchYaw contactrwaist;
157
    contactrwaist = contactMwaist.linear().eulerAngles(2, 1, 0).reverse();
158
159
    for (unsigned int i = 0; i < 3; ++i) {
160
      res(i + 3) = contactrwaist(i);
161
    }
162
  }
163
164
  sotDEBUGOUT(15);
165
  return res;
166
}