보행 코드 분석

SD_automatic_gait_test.py

실질적으로 로봇을 움직이는 최종 코드를 분석해 봅니다.

라이브러리 import 및 초기 세팅을 진행합니다.

from os import system, name 
import sys, os
sys.path.append("..")
sys.path.append(os.path.dirname(os.path.abspath(os.path.dirname(__file__))))
import matplotlib.animation as animation
import numpy as np
import time
import math
import datetime as dt
import keyboard
import random
import board
import busio

import Kinematics.kinematics as kn
import spotmicroai
from SD_servo_controller_test import Dynamixel_Controllers

from multiprocessing import Process
from Common.multiprocess_kb import KeyInterrupt
from Kinematics.kinematicMotion import KinematicMotion, TrottingGait

rtime=time.time()

def reset():
    global rtime
    rtime=time.time()

spotmicroai 는 지금까지 우리 살펴본 로봇의 Kinematics, 보행에 대한 모든 내용을 취합하여 추상화한 클래스입니다. 앞으로 우리는 이를 통해 로봇을 제어하는 main loop를 실행시킬 것입니다.

robot=spotmicroai.Robot(False,False,reset)

IMU 센서의 초기 세팅을 진행합니다.

💡

실행전, 연결 여부 확인을 위해 예제 코드가 실행되는지 반드시 확인해보세요!

import adafruit_bno055
# BNO055 ( IMU )
i2c_bus0 = busio.I2C(board.SCL_1, board.SDA_1)
sensor = adafruit_bno055.BNO055_I2C(i2c_bus0)

Dynamixel 모터 초기 세팅을 진행합니다.

위 작업을 진행하기 위해 USB 포트에 권한을 설정해 주어야 합니다.

$ cd /dev
$ sudo chmod 775 /dev/ttyUSB0

from dynamixel_sdk import *
# Dynamixel address
ADDR_MX_TORQUE_ENABLE      = 24               
ADDR_MX_GOAL_POSITION      = 30
ADDR_MX_PRESENT_POSITION   = 36

PROTOCOL_VERSION            = 1.0

BAUDRATE                    = 1000000            
DEVICENAME                  = '/dev/ttyUSB0'

TORQUE_ENABLE               = 1                
TORQUE_DISABLE              = 0

# setting port and protocol version
portHandler = PortHandler(DEVICENAME)
packetHandler = PacketHandler(PROTOCOL_VERSION)


DXL_controller = Dynamixel_Controllers()

# setting the DXL_Motor
DXL_controller.DynamixelSetting()

💡

여기까지 정상적으로 실행이 되면 다이나믹셀 모터에 토크가 걸려 고정된 상태여야 합니다.

spurWidth=robot.W/2+50
stepLength=0
stepHeight=72

# Initial End point X Value for Front legs 
iXf=120

walk=False

로봇의 보행과 관련된 매개변수를 설정합니다.

spurWidth : 좌우 발 사이의 너비에 해당하는 매개변수입니다.

stepHeight : 보행 시 발을 들어올리는 높이입니다.

iXf : 로봇 앞발의 초기 x 좌표로, 무게 중심에 따른 안정성을 위해 최적화 시 사용합니다.

각각의 Leg Point (x, y, z 좌표)를 지정합니다.

Lp = np.array([[iXf, -170, spurWidth, 1], [iXf, -170, -spurWidth, 1],
							[-120, -170, spurWidth, 1], [-120, -170, -spurWidth, 1]])

보행을 위한 클래스, TrottingGait를 선언하는 모습입니다.

motion=KinematicMotion(Lp) 
resetPose()

trotting=TrottingGait()

Main Loop

대부분의 로봇은 무한 while loop 안에서 지속적으로 피드백, 구동을 반복합니다.

지금의 경우 우리 로봇은

IMU로 부터 위치 데이터를 센싱하여

로봇이 넘어지지 않게 끊임없이 유지하는 로직이 구동되고 있습니다.

이러한 관점으로 main loop를 분석해 보겠습니다. 😉

loop가 반복될 때마다 구동 시간이 업데이트 됩니다.

def main(id, command_status):
    jointAngles = []
    while True:
        xr = 0.0
        yr = 0.0

        # Reset when robot pose become strange
        # robot.resetBody()

        ir = xr/(math.pi/180)

        d = time.time()-rtime

        # robot height
        height = 30  # 40

키보드 입력을 통해 로봇의 경로를 제어하는 부분입니다.

				# calculate robot step command from keyboard inputs
        result_dict = command_status.get()
        print(result_dict)
        command_status.put(result_dict)

        # wait 3 seconds to start
        if result_dict['StartStepping']:
            currentLp = trotting.positions(d-3, result_dict)
            robot.feetPosition(currentLp)
        else:
            robot.feetPosition(Lp)

💡

‼️ 중요 로봇이 기울어져도 수평을 유지할 수 있도록 해주는 Pose 제어 부분입니다 ‼️

				pitch = -((sensor.euler[1]*math.pi)/180)
        yaw = -((sensor.euler[0]*math.pi)/180)
        roll = -((sensor.euler[2]*math.pi)/180)
        robot.bodyRotation((roll,yaw,pitch))
        robot.bodyPosition((0, 0, 0))

IMU로부터 몸체의 각도를 전달받고, 이를 spotmicroai robot class의 bodyRotation 함수에 전달합니다.

def bodyRotation(self,rot):
        self.rot=rot


def calcIK(self,Lp,angles,center):
        (omega,phi,psi)=angles
        (xm,ym,zm)=center
				...

bodyRotation 함수는 rot 변수에 몸체 각도 정보를 저장하여 추후 모터 각도를 계산할때

(calcIK함수에서 rot 변수를 사용합니다.) 로봇 몸체가 기운 것에 대한 보상을 할 수 있도록 합니다.

즉, 바닥이 기울어져 로봇의 몸체가 기울었을때 다리의 자세를 변경하여 몸체의 각도가 외부의 좌표계 입장에서 일정하게 유지할 수 있도록 합니다.

https://s3-us-west-2.amazonaws.com/secure.notion-static.com/28f36d67-6f69-41fc-ab36-dc9f4ea15c79/Spotmicro_Dynamixel_Pitch.mp4

https://s3-us-west-2.amazonaws.com/secure.notion-static.com/e54adf08-f96a-45d8-96c4-e7c58662f2db/Spotmicro_Dynamixel_roll.mp4

다음으로, 로봇의 모터를 제어하기 위한 각도를 받아와 LaDian 변수에 저장하는 부분입니다.

⇒ 디버깅을 위해 이 부분이 콘솔에 출력됩니다.

				LaDian = robot.getAngle()
        print(LaDian)

...

# spotmicroai.py
def step(self):

        if (self.useRealTime):
            self.t = time.time() - self.ref_time
        else:
            self.t = self.t + self.fixedTimeStep

        # print(self.t)

        kp=self.IDkp # p.readUserDebugParameter(self.IDkp)
        kd=self.IDkd #p.readUserDebugParameter(self.IDkd)
        maxForce=self.IDmaxForce #p.readUserDebugParameter(self.IDmaxForce)

        # Calculate Angles with the input of FeetPos,BodyRotation and BodyPosition
        self.angles = self.kin.calcIK(self.Lp, self.rot, self.pos)
				...

이 angle은 spotmicroai.py 파일 안의 step 함수에서 이루어집니다.

몸체의 위치, 각도, 발의 위치 정보를 입력하면 calIK 함수를 통해 이 동작을 구현하기 위한 각 관절 모터의 각도가 계산되어 갱신됩니다.

받아온 모터 각도를 각 다이나믹셀에 전달하는 부분입니다.

그러면 최종적으로 보행 패턴을 따라 모터가 구동이 되어 걷는 동작을 수행하게 됩니다. 🐕

if len(LaDian):
     
            # ladian to degree
            thetas = DXL_controller.LadianToAngles(LaDian)

            # adjust robot servo
            Goal_Degree = DXL_controller.AngleToServo(DXLMotor_N)

            # degree to dynamixel value
            Goal_Position_Value = DXL_controller.DegreeToDXLValue(Goal_Degree)


            # Real Actuators
            DXL_controller.WriteMotor(DXLMotor_N)
            print(" **** Goal Degree **** ")
            print(Goal_Degree)
        
            
            
            # # Plot Robot Pose into Matplotlib for Debugging
            # TODO: Matplotplib animation
            # kn.initFK(jointAngles)
            # kn.plotKinematics()

        robot.step()
        consoleClear()

키보드 조종

메인 코드를 실행한 뒤, 실제 로봇을 전후좌우로 이동시키기 위한 방법입니다.

W A S D : 게임과 같이 전후좌우로 로봇을 움직이게 하며, 한 번 누를 때마다 해당 방향으로의 속도가 증가하는 방식입니다.

Q E : 시계방향, 반시계방향으로 로봇을 회전 + 이동시키게 됩니다.

예를 들어

W 10번 + S 5번을 누르면 W 5번 만큼의 전진 속도를 갖고 로봇이 움직입니다.

저작자표시 비영리 동일조건

Swiming_Lab

[Spot Dynamixel - 7] 보행 코드 분석