Python MadgwickAHRSの例

コード例 #1

ファイル: create_entities_utils.py プロジェクト: cmastalli/sot-torque-control

def create_imu_filter(robot, dt):
    from dynamic_graph.sot.torque_control.madgwickahrs import MadgwickAHRS
    imu_filter = MadgwickAHRS('imu_filter');
    imu_filter.init(dt);
    plug(robot.imu_offset_compensation.accelerometer_out, imu_filter.accelerometer);
    plug(robot.imu_offset_compensation.gyrometer_out,     imu_filter.gyroscope);
    return imu_filter;

コード例 #2

# -*- 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)