SpotMicro Jetson의 각도체계 이해하기

각도 단위 맞추기

Inverse Kinematics 에서 사용하는 단위 - radian

모터 제어에 입력하는 단위 - degree

theta = radian*180/np.pi

모터 방향을 고려하기

• 모터의 축에서 밖으로 나가는 방향을 기준으로 하는 회전을 가진다. 각도가 커지면 반시계 방향으로 회전!
• 조립된 다리를 기준으로 회전축이 가리키는 방향은 오른쪽 그림과 같다.

Inverse Kinematics 값을 모터에 출력하기

출처: https://github.com/FlorianWilk/SpotMicroAI

Kinematics 모델에서 얻어오는 각도는 위의 왼쪽 사진의 축을 기준으로 계산한 각도이다. 이 각도를 모터에 바로 집어넣어도 될까? 모터의 관점에서 입력을 다시 가공할 필요가 있다!

output = offset +(-) theta

self._servo_offsets = [180, 90, 90, 1, 90, 90, 180, 90, 90, 1, 90, 90]

# Angle mapping from radian to servo angles
def angleToServo(self, La):
		self.getDegreeAngles(La)
		
		#FL Lower
		self._val_list[0] = self._servo_offsets[0] - self._thetas[0][2]
		#FL Upper
		self._val_list[1] = self._servo_offsets[1] - self._thetas[0][1]    
		#FL Shoulder
		self._val_list[2] = self._servo_offsets[2] + self._thetas[0][0]
		
		#FR Lower
		self._val_list[3] = self._servo_offsets[3] + self._thetas[1][2]
		#FR Upper
		self._val_list[4] = self._servo_offsets[4] + self._thetas[1][1]    
		#FR Shoulder
		self._val_list[5] = self._servo_offsets[5] - self._thetas[1][0]
		
		#BL Lower
		self._val_list[6] = self._servo_offsets[6] - self._thetas[2][2]
		#BL Upper
		self._val_list[7] = self._servo_offsets[7] - self._thetas[2][1]    
		#BL Shoulder, Formula flipped from the front
		self._val_list[8] = self._servo_offsets[8] - self._thetas[2][0]
		
		#BR Lower.
		self._val_list[9] = self._servo_offsets[9] + self._thetas[3][2]
		#BR Upper
		self._val_list[10] = self._servo_offsets[10] + self._thetas[3][1]    
		#BR Shoulder, Formula flipped from the front
		self._val_list[11] = self._servo_offsets[11] + self._thetas[3][0]