1 |
|
|
/* |
2 |
|
|
* Copyright 2017, Andrea Del Prete, LAAS-CNRS |
3 |
|
|
* |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#include <dynamic-graph/factory.h> |
7 |
|
|
|
8 |
|
|
#include <sot/core/debug.hh> |
9 |
|
|
#include <sot/core/stop-watch.hh> |
10 |
|
|
#include <sot/torque_control/commands-helper.hh> |
11 |
|
|
#include <sot/torque_control/imu_offset_compensation.hh> |
12 |
|
|
|
13 |
|
|
namespace dynamicgraph { |
14 |
|
|
namespace sot { |
15 |
|
|
namespace torque_control { |
16 |
|
|
namespace dg = ::dynamicgraph; |
17 |
|
|
using namespace dg; |
18 |
|
|
using namespace dg::command; |
19 |
|
|
using namespace std; |
20 |
|
|
|
21 |
|
|
#define CALIBRATION_FILE_NAME "/opt/imu_calib.txt" |
22 |
|
|
|
23 |
|
|
#define PROFILE_IMU_OFFSET_COMPENSATION_COMPUTATION \ |
24 |
|
|
"ImuOffsetCompensation computation" |
25 |
|
|
|
26 |
|
|
#define INPUT_SIGNALS m_accelerometer_inSIN << m_gyrometer_inSIN |
27 |
|
|
#define OUTPUT_SIGNALS m_accelerometer_outSOUT << m_gyrometer_outSOUT |
28 |
|
|
|
29 |
|
|
/// Define EntityClassName here rather than in the header file |
30 |
|
|
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. |
31 |
|
|
typedef ImuOffsetCompensation EntityClassName; |
32 |
|
|
|
33 |
|
|
/* --- DG FACTORY ---------------------------------------------------- */ |
34 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ImuOffsetCompensation, |
35 |
|
|
"ImuOffsetCompensation"); |
36 |
|
|
|
37 |
|
|
/* ------------------------------------------------------------------- */ |
38 |
|
|
/* --- CONSTRUCTION -------------------------------------------------- */ |
39 |
|
|
/* ------------------------------------------------------------------- */ |
40 |
|
|
ImuOffsetCompensation::ImuOffsetCompensation(const std::string& name) |
41 |
|
|
: Entity(name), |
42 |
|
|
CONSTRUCT_SIGNAL_IN(accelerometer_in, dynamicgraph::Vector), |
43 |
|
|
CONSTRUCT_SIGNAL_IN(gyrometer_in, dynamicgraph::Vector), |
44 |
|
|
CONSTRUCT_SIGNAL_OUT(accelerometer_out, dynamicgraph::Vector, |
45 |
|
|
m_accelerometer_inSIN), |
46 |
|
|
CONSTRUCT_SIGNAL_OUT(gyrometer_out, dynamicgraph::Vector, |
47 |
|
|
m_gyrometer_inSIN), |
48 |
|
|
m_initSucceeded(false), |
49 |
|
|
m_dt(0.001f), |
50 |
|
|
m_update_cycles_left(0), |
51 |
|
|
m_update_cycles(0), |
52 |
|
|
m_a_gyro_DC_blocker(1.0f) |
53 |
|
|
|
54 |
|
|
{ |
55 |
|
|
Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS); |
56 |
|
|
|
57 |
|
|
m_gyro_offset.setZero(); |
58 |
|
|
m_acc_offset.setZero(); |
59 |
|
|
m_gyro_sum.setZero(); |
60 |
|
|
m_acc_sum.setZero(); |
61 |
|
|
|
62 |
|
|
/* Commands. */ |
63 |
|
|
addCommand("init", |
64 |
|
|
makeCommandVoid1(*this, &ImuOffsetCompensation::init, |
65 |
|
|
docCommandVoid1("Initialize the entity.", |
66 |
|
|
"Timestep in seconds (double)"))); |
67 |
|
|
addCommand( |
68 |
|
|
"update_offset", |
69 |
|
|
makeCommandVoid1( |
70 |
|
|
*this, &ImuOffsetCompensation::update_offset, |
71 |
|
|
docCommandVoid1("Update the IMU offsets.", |
72 |
|
|
"Duration of the update phase in seconds (double)"))); |
73 |
|
|
addCommand( |
74 |
|
|
"setGyroDCBlockerParameter", |
75 |
|
|
makeCommandVoid1(*this, &ImuOffsetCompensation::setGyroDCBlockerParameter, |
76 |
|
|
docCommandVoid1("Set DC Blocker filter parameter.", |
77 |
|
|
"alpha (double)"))); |
78 |
|
|
} |
79 |
|
|
|
80 |
|
|
/* ------------------------------------------------------------------- */ |
81 |
|
|
/* --- COMMANDS ------------------------------------------------------ */ |
82 |
|
|
/* ------------------------------------------------------------------- */ |
83 |
|
|
|
84 |
|
|
void ImuOffsetCompensation::init(const double& dt) { |
85 |
|
|
if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR); |
86 |
|
|
m_dt = static_cast<float>(dt); |
87 |
|
|
m_initSucceeded = true; |
88 |
|
|
|
89 |
|
|
// try to read IMU calibration data from file |
90 |
|
|
std::ifstream infile; |
91 |
|
|
infile.open(CALIBRATION_FILE_NAME, std::ios::in); |
92 |
|
|
if (!infile.is_open()) |
93 |
|
|
return SEND_MSG("Error trying to read calibration results from file " + |
94 |
|
|
toString(CALIBRATION_FILE_NAME), |
95 |
|
|
MSG_TYPE_ERROR); |
96 |
|
|
|
97 |
|
|
double z = 0; |
98 |
|
|
int i = 0; |
99 |
|
|
while (infile >> z) { |
100 |
|
|
m_gyro_offset(i) = z; |
101 |
|
|
i++; |
102 |
|
|
if (i == 3) break; |
103 |
|
|
} |
104 |
|
|
if (i != 3) { |
105 |
|
|
m_gyro_offset.setZero(); |
106 |
|
|
return SEND_MSG("Error trying to read gyro offset from file " + |
107 |
|
|
toString(CALIBRATION_FILE_NAME) + |
108 |
|
|
". Not enough values: " + toString(i), |
109 |
|
|
MSG_TYPE_ERROR); |
110 |
|
|
} |
111 |
|
|
|
112 |
|
|
i = 0; |
113 |
|
|
while (infile >> z) { |
114 |
|
|
m_acc_offset(i) = z; |
115 |
|
|
i++; |
116 |
|
|
if (i == 3) break; |
117 |
|
|
} |
118 |
|
|
if (i != 3) { |
119 |
|
|
m_gyro_offset.setZero(); |
120 |
|
|
m_acc_offset.setZero(); |
121 |
|
|
return SEND_MSG("Error trying to read acc offset from file " + |
122 |
|
|
toString(CALIBRATION_FILE_NAME) + |
123 |
|
|
". Not enough values: " + toString(i), |
124 |
|
|
MSG_TYPE_ERROR); |
125 |
|
|
} |
126 |
|
|
|
127 |
|
|
SEND_MSG("Offset read finished:\n* acc offset: " + |
128 |
|
|
toString(m_acc_offset.transpose()) + |
129 |
|
|
"\n* gyro offset: " + toString(m_gyro_offset.transpose()), |
130 |
|
|
MSG_TYPE_INFO); |
131 |
|
|
} |
132 |
|
|
|
133 |
|
|
void ImuOffsetCompensation::setGyroDCBlockerParameter(const double& alpha) { |
134 |
|
|
if (alpha > 1.0 || alpha <= 0.0) |
135 |
|
|
return SEND_MSG("GyroDCBlockerParameter must be > 0 and <= 1", |
136 |
|
|
MSG_TYPE_ERROR); |
137 |
|
|
m_a_gyro_DC_blocker = alpha; |
138 |
|
|
} |
139 |
|
|
|
140 |
|
|
void ImuOffsetCompensation::update_offset(const double& duration) { |
141 |
|
|
if (duration < m_dt) |
142 |
|
|
return SEND_MSG("Duration must be greater than the time step", |
143 |
|
|
MSG_TYPE_ERROR); |
144 |
|
|
m_update_cycles = int(duration / m_dt); |
145 |
|
|
m_update_cycles_left = m_update_cycles; |
146 |
|
|
} |
147 |
|
|
|
148 |
|
|
void ImuOffsetCompensation::update_offset_impl(int iter) { |
149 |
|
|
const dynamicgraph::Vector& accelerometer = m_accelerometer_inSIN(iter); |
150 |
|
|
const dynamicgraph::Vector& gyrometer = m_gyrometer_inSIN(iter); |
151 |
|
|
m_acc_sum += accelerometer; |
152 |
|
|
m_gyro_sum += gyrometer; |
153 |
|
|
|
154 |
|
|
m_update_cycles_left--; |
155 |
|
|
if (m_update_cycles_left == 0) { |
156 |
|
|
Vector3 g, new_acc_offset, new_gyro_offset; |
157 |
|
|
g << 0.0, 0.0, 9.81; |
158 |
|
|
new_acc_offset = (m_acc_sum / m_update_cycles) - g; |
159 |
|
|
new_gyro_offset = m_gyro_sum / m_update_cycles; |
160 |
|
|
m_acc_sum.setZero(); |
161 |
|
|
m_gyro_sum.setZero(); |
162 |
|
|
SEND_MSG( |
163 |
|
|
"Offset computation finished:" + |
164 |
|
|
("\n* old acc offset: " + toString(m_acc_offset.transpose())) + |
165 |
|
|
"\n* new acc offset: " + toString(new_acc_offset.transpose()) + |
166 |
|
|
"\n* old gyro offset: " + toString(m_gyro_offset.transpose()) + |
167 |
|
|
"\n* new gyro offset: " + toString(new_gyro_offset.transpose()), |
168 |
|
|
MSG_TYPE_INFO); |
169 |
|
|
m_acc_offset = new_acc_offset; |
170 |
|
|
m_gyro_offset = new_gyro_offset; |
171 |
|
|
|
172 |
|
|
// save to text file |
173 |
|
|
ofstream aof(CALIBRATION_FILE_NAME); |
174 |
|
|
if (!aof.is_open()) |
175 |
|
|
return SEND_MSG("Error trying to save calibration results on file " + |
176 |
|
|
toString(CALIBRATION_FILE_NAME), |
177 |
|
|
MSG_TYPE_ERROR); |
178 |
|
|
|
179 |
|
|
for (unsigned long int i = 0; i < 3; i++) aof << m_gyro_offset[i] << " "; |
180 |
|
|
aof << std::endl; |
181 |
|
|
for (unsigned long int i = 0; i < 3; i++) aof << m_acc_offset[i] << " "; |
182 |
|
|
aof << std::endl; |
183 |
|
|
aof.close(); |
184 |
|
|
SEND_MSG( |
185 |
|
|
"IMU calibration data saved to file " + toString(CALIBRATION_FILE_NAME), |
186 |
|
|
MSG_TYPE_INFO); |
187 |
|
|
} |
188 |
|
|
} |
189 |
|
|
|
190 |
|
|
/* ------------------------------------------------------------------- */ |
191 |
|
|
/* --- SIGNALS ------------------------------------------------------- */ |
192 |
|
|
/* ------------------------------------------------------------------- */ |
193 |
|
|
|
194 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(accelerometer_out, dynamicgraph::Vector) { |
195 |
|
|
if (!m_initSucceeded) { |
196 |
|
|
SEND_WARNING_STREAM_MSG( |
197 |
|
|
"Cannot compute signal accelerometer before initialization!"); |
198 |
|
|
return s; |
199 |
|
|
} |
200 |
|
|
|
201 |
|
|
if (m_update_cycles_left > 0) update_offset_impl(iter); |
202 |
|
|
|
203 |
|
|
const dynamicgraph::Vector& accelerometer = m_accelerometer_inSIN(iter); |
204 |
|
|
if (s.size() != 3) s.resize(3); |
205 |
|
|
s = accelerometer - m_acc_offset; |
206 |
|
|
return s; |
207 |
|
|
} |
208 |
|
|
|
209 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(gyrometer_out, dynamicgraph::Vector) { |
210 |
|
|
if (!m_initSucceeded) { |
211 |
|
|
SEND_WARNING_STREAM_MSG( |
212 |
|
|
"Cannot compute signal gyrometer before initialization!"); |
213 |
|
|
return s; |
214 |
|
|
} |
215 |
|
|
const dynamicgraph::Vector& gyrometer = m_gyrometer_inSIN(iter); |
216 |
|
|
if (s.size() != 3) s.resize(3); |
217 |
|
|
// estimate bias online with the assumption that average angular velocity |
218 |
|
|
// should be zero. |
219 |
|
|
if (m_a_gyro_DC_blocker != 1.0) |
220 |
|
|
m_gyro_offset = m_gyro_offset * m_a_gyro_DC_blocker + |
221 |
|
|
(1. - m_a_gyro_DC_blocker) * gyrometer; |
222 |
|
|
s = gyrometer - m_gyro_offset; |
223 |
|
|
return s; |
224 |
|
|
} |
225 |
|
|
|
226 |
|
|
/* ------------------------------------------------------------------- */ |
227 |
|
|
/* --- ENTITY -------------------------------------------------------- */ |
228 |
|
|
/* ------------------------------------------------------------------- */ |
229 |
|
|
|
230 |
|
|
void ImuOffsetCompensation::display(std::ostream& os) const { |
231 |
|
|
os << "ImuOffsetCompensation " << getName(); |
232 |
|
|
try { |
233 |
|
|
getProfiler().report_all(3, os); |
234 |
|
|
} catch (ExceptionSignal e) { |
235 |
|
|
} |
236 |
|
|
} |
237 |
|
|
} // namespace torque_control |
238 |
|
|
} // namespace sot |
239 |
|
|
} // namespace dynamicgraph |