GCC Code Coverage Report

Directory: ./
File: src/waist-attitude-from-sensor.cpp
Date: 2024-09-28 11:08:19
Exec Total Coverage
Lines: 0 59 0.0%
Branches: 0 154 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 }
167