sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
imu_offset_compensation.cpp
Go to the documentation of this file.
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>
12
13namespace dynamicgraph {
14namespace sot {
15namespace torque_control {
16namespace dg = ::dynamicgraph;
17using namespace dg;
18using namespace dg::command;
19using 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
31typedef ImuOffsetCompensation EntityClassName;
32
33/* --- DG FACTORY ---------------------------------------------------- */
35 "ImuOffsetCompensation");
36
37/* ------------------------------------------------------------------- */
38/* --- CONSTRUCTION -------------------------------------------------- */
39/* ------------------------------------------------------------------- */
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(
71 docCommandVoid1("Update the IMU offsets.",
72 "Duration of the update phase in seconds (double)")));
73 addCommand(
74 "setGyroDCBlockerParameter",
76 docCommandVoid1("Set DC Blocker filter parameter.",
77 "alpha (double)")));
78}
79
80/* ------------------------------------------------------------------- */
81/* --- COMMANDS ------------------------------------------------------ */
82/* ------------------------------------------------------------------- */
83
84void 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
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
140void 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);
146}
147
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
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
194DEFINE_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
209DEFINE_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
230void 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
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
float m_dt
true if the entity has been successfully initialized
EIGEN_MAKE_ALIGNED_OPERATOR_NEW ImuOffsetCompensation(const std::string &name)
double m_a_gyro_DC_blocker
total number of update cycles to perform
virtual void display(std::ostream &os) const
compensated gyrometer data
#define CALIBRATION_FILE_NAME
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition treeview.dox:22