sot-torque-control  1.6.1
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
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>
11 #include <sot/core/stop-watch.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 "ImuOffsetCompensation computation"
24 
25 #define INPUT_SIGNALS m_accelerometer_inSIN << m_gyrometer_inSIN
26 #define OUTPUT_SIGNALS m_accelerometer_outSOUT << m_gyrometer_outSOUT
27 
30 typedef ImuOffsetCompensation EntityClassName;
31 
32 /* --- DG FACTORY ---------------------------------------------------- */
33 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ImuOffsetCompensation, "ImuOffsetCompensation");
34 
35 /* ------------------------------------------------------------------- */
36 /* --- CONSTRUCTION -------------------------------------------------- */
37 /* ------------------------------------------------------------------- */
39  : Entity(name),
40  CONSTRUCT_SIGNAL_IN(accelerometer_in, dynamicgraph::Vector),
41  CONSTRUCT_SIGNAL_IN(gyrometer_in, dynamicgraph::Vector),
42  CONSTRUCT_SIGNAL_OUT(accelerometer_out, dynamicgraph::Vector, m_accelerometer_inSIN),
43  CONSTRUCT_SIGNAL_OUT(gyrometer_out, dynamicgraph::Vector, m_gyrometer_inSIN),
44  m_initSucceeded(false),
45  m_dt(0.001f),
46  m_update_cycles_left(0),
47  m_update_cycles(0),
48  m_a_gyro_DC_blocker(1.0f)
49 
50 {
51  Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
52 
53  m_gyro_offset.setZero();
54  m_acc_offset.setZero();
55  m_gyro_sum.setZero();
56  m_acc_sum.setZero();
57 
58  /* Commands. */
59  addCommand("init", makeCommandVoid1(*this, &ImuOffsetCompensation::init,
60  docCommandVoid1("Initialize the entity.", "Timestep in seconds (double)")));
61  addCommand("update_offset", makeCommandVoid1(*this, &ImuOffsetCompensation::update_offset,
62  docCommandVoid1("Update the IMU offsets.",
63  "Duration of the update phase in seconds (double)")));
64  addCommand("setGyroDCBlockerParameter",
65  makeCommandVoid1(*this, &ImuOffsetCompensation::setGyroDCBlockerParameter,
66  docCommandVoid1("Set DC Blocker filter parameter.", "alpha (double)")));
67 }
68 
69 /* ------------------------------------------------------------------- */
70 /* --- COMMANDS ------------------------------------------------------ */
71 /* ------------------------------------------------------------------- */
72 
73 void ImuOffsetCompensation::init(const double& dt) {
74  if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
75  m_dt = static_cast<float>(dt);
76  m_initSucceeded = true;
77 
78  // try to read IMU calibration data from file
79  std::ifstream infile;
80  infile.open(CALIBRATION_FILE_NAME, std::ios::in);
81  if (!infile.is_open())
82  return SEND_MSG("Error trying to read calibration results from file " + toString(CALIBRATION_FILE_NAME),
83  MSG_TYPE_ERROR);
84 
85  double z = 0;
86  int i = 0;
87  while (infile >> z) {
88  m_gyro_offset(i) = z;
89  i++;
90  if (i == 3) break;
91  }
92  if (i != 3) {
93  m_gyro_offset.setZero();
94  return SEND_MSG("Error trying to read gyro offset from file " + toString(CALIBRATION_FILE_NAME) +
95  ". Not enough values: " + toString(i),
96  MSG_TYPE_ERROR);
97  }
98 
99  i = 0;
100  while (infile >> z) {
101  m_acc_offset(i) = z;
102  i++;
103  if (i == 3) break;
104  }
105  if (i != 3) {
106  m_gyro_offset.setZero();
107  m_acc_offset.setZero();
108  return SEND_MSG("Error trying to read acc offset from file " + toString(CALIBRATION_FILE_NAME) +
109  ". Not enough values: " + toString(i),
110  MSG_TYPE_ERROR);
111  }
112 
113  SEND_MSG("Offset read finished:\n* acc offset: " + toString(m_acc_offset.transpose()) +
114  "\n* gyro offset: " + toString(m_gyro_offset.transpose()),
115  MSG_TYPE_INFO);
116 }
117 
119  if (alpha > 1.0 || alpha <= 0.0) return SEND_MSG("GyroDCBlockerParameter must be > 0 and <= 1", MSG_TYPE_ERROR);
120  m_a_gyro_DC_blocker = alpha;
121 }
122 
123 void ImuOffsetCompensation::update_offset(const double& duration) {
124  if (duration < m_dt) return SEND_MSG("Duration must be greater than the time step", MSG_TYPE_ERROR);
125  m_update_cycles = int(duration / m_dt);
127 }
128 
130  const dynamicgraph::Vector& accelerometer = m_accelerometer_inSIN(iter);
131  const dynamicgraph::Vector& gyrometer = m_gyrometer_inSIN(iter);
132  m_acc_sum += accelerometer;
133  m_gyro_sum += gyrometer;
134 
136  if (m_update_cycles_left == 0) {
137  Vector3 g, new_acc_offset, new_gyro_offset;
138  g << 0.0, 0.0, 9.81;
139  new_acc_offset = (m_acc_sum / m_update_cycles) - g;
140  new_gyro_offset = m_gyro_sum / m_update_cycles;
141  m_acc_sum.setZero();
142  m_gyro_sum.setZero();
143  SEND_MSG("Offset computation finished:" + ("\n* old acc offset: " + toString(m_acc_offset.transpose())) +
144  "\n* new acc offset: " + toString(new_acc_offset.transpose()) + "\n* old gyro offset: " +
145  toString(m_gyro_offset.transpose()) + "\n* new gyro offset: " + toString(new_gyro_offset.transpose()),
146  MSG_TYPE_INFO);
147  m_acc_offset = new_acc_offset;
148  m_gyro_offset = new_gyro_offset;
149 
150  // save to text file
151  ofstream aof(CALIBRATION_FILE_NAME);
152  if (!aof.is_open())
153  return SEND_MSG("Error trying to save calibration results on file " + toString(CALIBRATION_FILE_NAME),
154  MSG_TYPE_ERROR);
155 
156  for (unsigned long int i = 0; i < 3; i++) aof << m_gyro_offset[i] << " ";
157  aof << std::endl;
158  for (unsigned long int i = 0; i < 3; i++) aof << m_acc_offset[i] << " ";
159  aof << std::endl;
160  aof.close();
161  SEND_MSG("IMU calibration data saved to file " + toString(CALIBRATION_FILE_NAME), MSG_TYPE_INFO);
162  }
163 }
164 
165 /* ------------------------------------------------------------------- */
166 /* --- SIGNALS ------------------------------------------------------- */
167 /* ------------------------------------------------------------------- */
168 
169 DEFINE_SIGNAL_OUT_FUNCTION(accelerometer_out, dynamicgraph::Vector) {
170  if (!m_initSucceeded) {
171  SEND_WARNING_STREAM_MSG("Cannot compute signal accelerometer before initialization!");
172  return s;
173  }
174 
175  if (m_update_cycles_left > 0) update_offset_impl(iter);
176 
177  const dynamicgraph::Vector& accelerometer = m_accelerometer_inSIN(iter);
178  if (s.size() != 3) s.resize(3);
179  s = accelerometer - m_acc_offset;
180  return s;
181 }
182 
183 DEFINE_SIGNAL_OUT_FUNCTION(gyrometer_out, dynamicgraph::Vector) {
184  if (!m_initSucceeded) {
185  SEND_WARNING_STREAM_MSG("Cannot compute signal gyrometer before initialization!");
186  return s;
187  }
188  const dynamicgraph::Vector& gyrometer = m_gyrometer_inSIN(iter);
189  if (s.size() != 3) s.resize(3);
190  // estimate bias online with the assumption that average angular velocity should be zero.
191  if (m_a_gyro_DC_blocker != 1.0)
192  m_gyro_offset = m_gyro_offset * m_a_gyro_DC_blocker + (1. - m_a_gyro_DC_blocker) * gyrometer;
193  s = gyrometer - m_gyro_offset;
194  return s;
195 }
196 
197 /* ------------------------------------------------------------------- */
198 /* --- ENTITY -------------------------------------------------------- */
199 /* ------------------------------------------------------------------- */
200 
201 void ImuOffsetCompensation::display(std::ostream& os) const {
202  os << "ImuOffsetCompensation " << getName();
203  try {
204  getProfiler().report_all(3, os);
205  } catch (ExceptionSignal e) {
206  }
207 }
208 } // namespace torque_control
209 } // namespace sot
210 } // namespace dynamicgraph
dynamicgraph::sot::torque_control::ImuOffsetCompensation::init
void init(const double &dt)
Definition: imu_offset_compensation.cpp:73
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_update_cycles_left
int m_update_cycles_left
sampling time in seconds
Definition: imu_offset_compensation.hh:80
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_initSucceeded
bool m_initSucceeded
Definition: imu_offset_compensation.hh:78
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_dt
float m_dt
true if the entity has been successfully initialized
Definition: imu_offset_compensation.hh:79
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_gyro_offset
Vector3 m_gyro_offset
Definition: imu_offset_compensation.hh:84
CALIBRATION_FILE_NAME
#define CALIBRATION_FILE_NAME
Definition: imu_offset_compensation.cpp:21
commands-helper.hh
dynamicgraph::sot::torque_control::ImuOffsetCompensation::update_offset
void update_offset(const double &duration)
Definition: imu_offset_compensation.cpp:123
dynamicgraph::sot::torque_control::ImuOffsetCompensation::update_offset_impl
void update_offset_impl(int iter)
Definition: imu_offset_compensation.cpp:129
imu_offset_compensation.hh
OUTPUT_SIGNALS
#define OUTPUT_SIGNALS
Definition: imu_offset_compensation.cpp:26
dynamicgraph::sot::torque_control::ImuOffsetCompensation::setGyroDCBlockerParameter
void setGyroDCBlockerParameter(const double &alpha)
Definition: imu_offset_compensation.cpp:118
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::ImuOffsetCompensation::display
virtual void display(std::ostream &os) const
compensated gyrometer data
Definition: imu_offset_compensation.cpp:201
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_update_cycles
int m_update_cycles
number of update cycles left
Definition: imu_offset_compensation.hh:81
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_gyro_sum
Vector3 m_gyro_sum
accelerometer offset
Definition: imu_offset_compensation.hh:87
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_a_gyro_DC_blocker
double m_a_gyro_DC_blocker
total number of update cycles to perform
Definition: imu_offset_compensation.hh:82
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_acc_offset
Vector3 m_acc_offset
gyrometer offset
Definition: imu_offset_compensation.hh:85
dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
INPUT_SIGNALS
#define INPUT_SIGNALS
Definition: imu_offset_compensation.cpp:25
dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
Definition: admittance-controller.cpp:160
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_acc_sum
Vector3 m_acc_sum
tmp variable to store the sum of the gyro measurements during update phase
Definition: imu_offset_compensation.hh:88
dynamicgraph::sot::torque_control::ImuOffsetCompensation::ImuOffsetCompensation
EIGEN_MAKE_ALIGNED_OPERATOR_NEW ImuOffsetCompensation(const std::string &name)
Definition: imu_offset_compensation.cpp:38