[Spot Dynamixel - 6] Kinematic Motion 코드 설명

 

🦵🏼

Kinematic Motion 코드 설명

class TrottingGait

gait란 걸음걸이를 뜻합니다. 곧, 이 클래스는 로봇을 걷게 하기 위한 구현입니다.

def __init__(self):
        self.step_gain = 0.8
        self.maxSl=2
        self.bodyPos=(0,100,0)
        self.bodyRot=(0,0,0)
        self.t0=300 #0 #300 # senseless i guess
        self.t1=1200 #1200
        self.t2=300 #0 #300
        self.t3=350 #200
        self.Sl=0
        self.Sw=0
        self.Sh=70 #100
        self.Sa=0
        self.Spf=87
        self.Spr=77
        self.Fo=120
        self.Ro=50

        self.Rc=[-50,0,0,1] # rotation center

각 매개변수에 대한 설명은 다음과 같습니다.

• Sl : 한걸음을 걸을때 전후 방향으로 내딛는 길이

• Sw : 한걸음을 걸을때 좌우방향 내딛는 길이

• Sh : 한걸음을 걸을때 발을 들어올리는 높이

• Sa : 대각선으로 걸을때 지상으로부터 수직한 축을 기준으로 회전하는 각도(Pitch)

• 정지상태일때 각 다리의 위치설정과 관련된 변수들⇒ 각 다리의 끝점 (x, y, z) 좌표를 나타냅니다.2번 다리: (Fo, -100, -Spf )4번 다리: (Ro, -100, -Spr )
• 3번 다리: (Ro, -100, Spr )
• 1번 다리: (Fo, -100, Spf )

 

😉다음으로 주요 인스턴스 함수들을 살펴보겠습니다.

 

calcLeg

현재의 leg point (x, y, z)와 주어진 시간 t를 통해 이동시킬 leg point를 계산해주는 함수입니다.

def calcLeg(self,t,x,y,z):
        startLp=np.array([x-self.Sl/2.0,y,z-self.Sw,1])
        endY=0 #-0.8 # delta y to jump a bit before lifting legs
        endLp=np.array([x+self.Sl/2,y+endY,z+self.Sw,1])
        
        if(t<self.t0): # TODO: remove t0 and t2 - not practical
            return startLp
        elif(t<self.t0+self.t1): # drag foot over ground

            td=t-self.t0
            tp=1/(self.t1/td)
            diffLp=endLp-startLp
            curLp=startLp+diffLp*tp
            psi=-((math.pi/180*self.Sa)/2)+(math.pi/180*self.Sa)*tp
            Ry = np.array([[np.cos(psi),0,np.sin(psi),0],
                    [0,1,0,0],
                    [-np.sin(psi),0,np.cos(psi),0],[0,0,0,1]])
            #Tlm = np.array([[0,0,0,-self.Rc[0]],[0,0,0,-self.Rc[1]],[0,0,0,-self.Rc[2]],[0,0,0,0]])
            curLp=Ry.dot(curLp)
            return curLp
        elif(t<self.t0+self.t1+self.t2):
            return endLp
        elif(t<self.t0+self.t1+self.t2+self.t3): # Lift foot
            td=t-(self.t0+self.t1+self.t2)
            tp=1/(self.t3/td)
            diffLp=startLp-endLp
            curLp=endLp+diffLp*tp
            curLp[1]+=self.Sh*math.sin(math.pi*tp)
            return curLp

• psi와 Ry가 등장한 이유는, 구체적인 계산식은 큰 의미는 없고, 로봇이 대각선 방향으로 움직이기 위해 pitch 방향의 전환이 필요하기 때문입니다.

• 로봇이 걸어가기 위해서는 반복적인 움직임들이 연속됩니다. 이 반복되는 메커니즘을 여러 부분으로 나누고, 이에 따라 Leg point를 제어하는 알고리즘이라 할 수 있습니다.

 

💡

각 상황에 대한 구체적인 설명은 다음 영상으로 대체하겠습니다.

https://www.youtube.com/watch?v=LH_wL2gKRJ4&list=PLDAyxeaWPd36ol0wfXV4KST0x8v7KWFwO&index=21

 

positions

키보드 입력을 받아 로봇을 이동시킬 수 있도록 구현된 인터페이스입니다.

def positions(self,t,kb_offset={}):
        spf=self.Spf
        spr=self.Spr
        # self.Sh=60.0

        if list(kb_offset.values()) == [0.0, 0.0, 0.0]:
            self.Sl=0.0
            self.Sw=0.0
            self.Sa=0.0
        else:
            self.Sl=kb_offset['IDstepLength']
            self.Sw=kb_offset['IDstepWidth']
            self.Sa=kb_offset['IDstepAlpha']

        Tt=(self.t0+self.t1+self.t2+self.t3)
        Tt2=Tt/2
        rd=0 # rear delta - unused - maybe stupid
        td=(t*1000)%Tt
        t2=(t*1000-Tt2)%Tt
        rtd=(t*1000-rd)%Tt # rear time delta
        rt2=(t*1000-Tt2-rd)%Tt
        Fx=self.Fo
        Rx=-1*self.Ro
        Fy=-100
        Ry=-100
        r=np.array([self.calcLeg(td,Fx,Fy,spf),self.calcLeg(t2,Fx,Fy,-spf),self.calcLeg(rt2,Rx,Ry,spr),self.calcLeg(rtd,Rx,Ry,-spr)])
        #print(r)
        return r

• 키보드 입력에 따라 Sl, Sw, Sa 값을 변경함으로 로봇의 전후좌우, 대각선 방향으로의 진행 제어가 가능합니다.