Add test of madgwick

python/dynamic_graph/sot/torque_control/tests/test_magdwick.py

+# -*- coding: utf-8 -*-
+"""
+Created on Tue Oct  3 14:01:08 2017
+
+@author: adelpret
+"""
+
+from dynamic_graph.sot.torque_control.madgwickahrs import MadgwickAHRS
+
+dt = 0.001;
+imu_filter = MadgwickAHRS('imu_filter');
+imu_filter.init(dt);
+imu_filter.setBeta(0.0)
+
+for i in range(10):
+    imu_filter.accelerometer.value = (0.0, 0.0, 9.8)
+    imu_filter.gyroscope.value = (0.001, -1e-3, 1e-4)
+    imu_filter.imu_quat.recompute(i);
+    print imu_filter.imu_quat.value
\ No newline at end of file