집밖은 위험해

이제 관성 센서로 받는

가속도, 자이로계 값들을 실시간 플로팅 예제

from zumi.zumi import Zumi
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation




"""
common functions
"""
def get_mpu_val(zumi):
    mpus = zumi.get_all_mpu_data()
    acc_x = mpus[0]
    acc_y = mpus[1]
    acc_z = mpus[2]
    gyro_x = mpus[3]
    gyro_y = mpus[4]
    gyro_z = mpus[5]
    mpu_val = np.array([[acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z]])
    return mpu_val


def new_data_insert(mpu, mpu_val):
    #push
    mpu = np.append(mpu, mpu_val, axis=0)
    #pop
    mpu = np.delete(mpu, 0, 0)


    return mpu




"""
initialize mpu data
"""
def init():
    idx = 0
    mpu = np.zeros((100, 6))
    t = np.linspace(0, 1, 100)

    while idx < 100:
        mpu_val = get_mpu_val(zumi)
        mpu = new_data_insert(mpu, mpu_val)
        idx = idx + 1
    return t, mpu





"""
functions for animation
"""
def ln_set_data(mpu):
    ln0.set_data(t, mpu[:,0])
    ln1.set_data(t, mpu[:,1])
    ln2.set_data(t, mpu[:,2])
    ln3.set_data(t, mpu[:,3])
    ln4.set_data(t, mpu[:,4])
    ln5.set_data(t, mpu[:,5])


# change y limit dynamically
def change_ylim(ax, mpu):
    ax[0, 0].set_ylim(min(mpu[:,0]), max(mpu[:,0]))
    ax[1, 0].set_ylim(min(mpu[:,1]), max(mpu[:,1]))
    ax[2, 0].set_ylim(min(mpu[:,2]), max(mpu[:,2]))
    ax[0, 1].set_ylim(min(mpu[:,3]), max(mpu[:,3]))
    ax[1, 1].set_ylim(min(mpu[:,4]), max(mpu[:,4]))
    ax[2, 1].set_ylim(min(mpu[:,5]), max(mpu[:,5]))


def mpu_init_plot(ax, mpu):
    ln0, = ax[0, 0].plot(t,mpu[:,0], 'r')
    ax[0, 0].grid(True)
    ax[0, 0].set_title("acc x")
    ln1, = ax[1, 0].plot(t,mpu[:,1], 'g')
    ax[1, 0].grid(True)
    ax[1, 0].set_title("acc y")
    ln2, = ax[2, 0].plot(t,mpu[:,2], 'b')
    ax[2, 0].grid(True)
    ax[2, 0].set_title("acc z")
    ln3, = ax[0, 1].plot(t,mpu[:,3], 'r')
    ax[0, 1].grid(True)
    ax[0, 1].set_title("gyro x")
    ln4, = ax[1, 1].plot(t,mpu[:,4], 'g')
    ax[1, 1].grid(True)
    ax[1, 1].set_title("gyro y")
    ln5, = ax[2, 1].plot(t,mpu[:,5], 'b')
    ax[2, 1].grid(True)
    ax[2, 1].set_title("gyro z")
    return ln0, ln1, ln2, ln3, ln4, ln5



def update(i):
    global mpu
    mpu_val = get_mpu_val(zumi)
    mpu = new_data_insert(mpu, mpu_val)

    ln_set_data(mpu)
    change_ylim(ax, mpu)
    return ln0, ln1, ln2, ln3, ln4, ln5




#initialize
zumi = Zumi()
t, mpu = init()

#plot
fig, ax = plt.subplots(3,2)
ln0, ln1, ln2, ln3, ln4, ln5 = mpu_init_plot(ax, mpu)

#animation
ani = FuncAnimation(fig, update, frames=t, blit=True)
plt.show()

다음엔 필터링

평균, 평균이동, 저주파 통과, 칼만 필터 순으로

타공판

볼트너트

센서 홀더

모터 홀더

진자(사용안함)

이전에 로터리 엔코더를 사용한 역 도립진자는

조금 더 비싼 로터리 엔코더를 사용해야하다보니 구현이 어렵게 되었다.

대신 아두이노를 이용한 밸런싱 로봇을 생각해보게 되었다.

기본 타공판, 볼트, 너트, 고정대 등은 만들어 두었으니

출력후 조립만 하면 비슷하게 완성될것 같다.

더 나아가 가능하다면

3층으로 늘려 파이와 카메라도 놓을수 있으면 좋겠다.

ref : https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_imgproc/py_gradients/py_gradients.html

라플라시안 영상, 소벨 x/ 소벨 y 영상

import numpy as np
import cv2

cap = cv2.VideoCapture(0)
while(True):
    ret, frame = cap.read()
    frame = cv2.flip(frame, 0)
    rsz = cv2.resize(frame, dsize=(320,240))
    gray = cv2.cvtColor(rsz, cv2.COLOR_BGR2GRAY)

    laplacian = cv2.Laplacian(gray,cv2.CV_64F)
    sobelx = cv2.Sobel(gray,cv2.CV_64F,1,0,ksize=5)
    sobely = cv2.Sobel(gray,cv2.CV_64F,0,1,ksize=5)

    res = np.concatenate((laplacian, sobelx, sobely), axis=1)
    cv2.imshow('res',res)
    if cv2.waitKey(20) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()

주미 충전시키는 동안 잠시 영상을 진행

리사이즈 후 흑백 채널로 변경

import numpy as np
import cv2

cap = cv2.VideoCapture(0)
while(True):
    ret, frame = cap.read()
    frame = cv2.flip(frame, 0)
    rsz = cv2.resize(frame, dsize=(320,240))

    res = cv2.cvtColor(rsz, cv2.COLOR_BGR2GRAY)
    cv2.imshow('res',res)
    if cv2.waitKey(20) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()

ref : https://matplotlib.org/api/_as_gen/matplotlib.pyplot.subplots.html

1. 1 x 2 열 subplot

import numpy as np
import matplotlib.pyplot as plt

# First create some toy data:
x = np.linspace(0, 2*np.pi, 400)
y = np.sin(x**2)

# Create two subplots and unpack the output array immediately
f, (ax1, ax2) = plt.subplots(1, 2, sharey=True)
ax1.plot(x, y)
ax1.set_title('Sharing Y axis')
ax2.scatter(x, y)

plt.show()

2. 2x2 서브플롯 생성

import numpy as np
import matplotlib.pyplot as plt

# First create some toy data:
x = np.linspace(0, 2*np.pi, 400)
y = np.sin(x**2)

# Create four4 axes and access them through the returned array
fig, axs = plt.subplots(2, 2)
axs[0, 0].plot(x, y)
axs[1, 1].scatter(x, y)

plt.show()

가장 단순한 plt.subplots()

여러개 플롯 생성 없이 사용.

import numpy as np
import matplotlib.pyplot as plt

x = np.linspace(0, 2*np.pi, 400)
y = np.sin(x**2)

# Create just a figure and only one subplot
fig, ax = plt.subplots()
ax.plot(x, y)
ax.set_title('Simple plot')

plt.show()

지금까지 플로팅, mpu 전체 데이터 받기 등 마쳤습니다.

이번에는 전체 MPU raw 데이터를 플로팅 하는 예제를 정리하였습니다.

그 동안 뒤죽박죽 이던 코드를 함수로 나눠서 조금 정리했습니다.

from zumi.zumi import Zumi
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation




"""
common functions
"""
def get_mpu_val(zumi):
    mpus = zumi.get_all_mpu_data()
    acc_x = mpus[0]
    acc_y = mpus[1]
    acc_z = mpus[2]
    gyro_x = mpus[3]
    gyro_y = mpus[4]
    gyro_z = mpus[5]
    mpu_val = np.array([[acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z]])
    return mpu_val


def new_data_insert(mpu, mpu_val):
    #push
    mpu = np.append(mpu, mpu_val, axis=0)
    #pop
    mpu = np.delete(mpu, 0, 0)

    return mpu




"""
initialize mpu data
"""
def init():
    idx = 0
    mpu = np.zeros((100, 6))
    t = np.linspace(0, 1, 100)

    while idx < 100:
        mpu_val = get_mpu_val(zumi)
        mpu = new_data_insert(mpu, mpu_val)
        idx = idx + 1
    return t, mpu





"""
functions for animation
"""
def mpu_init_plot(ax, mpu):
    ln0, = ax[0].plot(t,mpu[:,0], 'r')
    ax[0].grid(True)
    ax[0].set_title("acc x")
    ln1, = ax[1].plot(t,mpu[:,1], 'g')
    ax[1].grid(True)
    ax[1].set_title("acc y")
    ln2, = ax[2].plot(t,mpu[:,2], 'b')
    ax[2].grid(True)
    ax[2].set_title("acc z")
    ln3, = ax[3].plot(t,mpu[:,3], 'r')
    ax[3].grid(True)
    ax[3].set_title("gyro x")
    ln4, = ax[4].plot(t,mpu[:,4], 'g')
    ax[4].grid(True)
    ax[4].set_title("gyro y")
    ln5, = ax[5].plot(t,mpu[:,5], 'b')
    ax[5].grid(True)
    ax[5].set_title("gyro z")
    return ln0, ln1, ln2, ln3, ln4, ln5



#initialize
zumi = Zumi()
t, mpu = init()

#plot
fig, ax = plt.subplots(6,1)
ln0, ln1, ln2, ln3, ln4, ln5 = mpu_init_plot(ax, mpu)
plt.show()

다음에는 전체 데이터 animation을 하겠습니다.

주미 lib에서는

각도를 반환해주는 update_angles() 함수가 있지만

raw mpu데이터가 아니라 필터 처리된

조금 다른 값들을 반환해주고 있습니다.

그래서 지난번과 마찬가지로

주미 스트립트를 조금 고치겠습니다.

    def get_all_mpu_data(self):
        self.mpu_list = self.mpu.read_all_MPU_data()
        x_acc = self.mpu_list[0]
        y_acc = self.mpu_list[1]
        z_acc = self.mpu_list[2]
        x_gyro = self.mpu_list[3]
        y_gyro = self.mpu_list[4]
        z_gyro = self.mpu_list[5]
        return [x_acc, y_acc, z_acc, x_gyro, y_gyro, z_gyro]

지난번에 추가한 get_acc 함수 밑에 넣어주면 이제 가속도, 자이로 전체 값을 사용할수 있게 됩니다.

얼마전에 

SLAM 관련 강의를 해오시던 시릴 교수님이

SLAM 분야에 사용되는 주요 컨샙들을 5분 정도 시간으로 설명해주는 유튜브 영상을 올리셨더라

칼만/파티클 필터, 점유 격자 지도, bag of words, 좌표계, 란삭, SLAM, ICP, 이진 특징 등

5 Minutes with Cyrill 

나중에 개념들을 간단하게 정리하는데 많이 도움될거같다.

플레이리스트

https://www.youtube.com/playlist?list=PLgnQpQtFTOGSO8HC48K9sPuNliY1qxzV9