라즈베리 파이에서 모터의 시험 구동이 성공하였으니,
보행 모션을 구현하기 위해 Inverse Kinematics 모델을 제작해야 한다.
최종적으로는 ROS2 내부에서 계산해야 하지만, 테스트 용도로 OpenRB-150 sketch로 작성해보자.
Inverse Kinematics 이론에 대해서는
2DOF Inverse Kinematics 에서 다루었다.
사용한 제품은 Robotis의 XL330-M288-T 모터와, OpenRB-150이다.
OpenRB-150 공식 e-manuel
XL330-M288-T 공식 e-manuel
1. 링크 기본 자세 설정 & 좌표축 설정
XL330-M288-T 모터는 position 값이 0~4095 (2048일 때 0)이고,
CCW 방향으로 회전할 때 position 값이 증가한다.
또한, IK를 진행할 좌표의 x축을 아래방향으로 설정하여야 훨씬 편한데, 1번 링크의 각도와 을 완전히 동기화 시키기 위해서이다.
-
position 모드에서, 가동범위는 를 기준으로 이다. 기준을 맞추지 않는다면, 모터의 position 값을 계산할 때, 특별히 예외를 두어야 한다.
-
발이 아랫방향으로 쭉 펴졌을 때 모든 각도를 로 둔다면, IK로 구한 각도를 추가적인 처리 없이 모터의 각도에 바로 사용할 수 있다. radian을 poisition 값으로 치환만 해주면 된다.
-
대칭되어 모터의 position 값 증가 방향이 반대일 경우, 단순히 각도의 부호만 바꾸어 주면 된다.
2. 보행 알고리즘 제작 & foot 궤적 설정
How Walk?
4족 로봇의 보행 메커니즘을 고려해보았을 때, walk를 구현하기 위해서는 총 3가지 모션이 필요하다.
가장 중요한 포인트는 한 발이 앞으로 나갈 때, 나머지 한 발은 뒤로 가야 한다는 것이다.
예를 들어, 2족 보행에서 오른발로 땅을 딛고, 왼발을 들어 앞으로 전진한다고 생각해보자.
그림에서, 왼발과 오른발 사이 간격은 d 이다.
그림과 같이, 바디가 앞으로 d 만큼 전진하려면,
L 발이 공중에 뜬 상태로 앞으로 2d,
R 발은 바닥에 고정되어 있어야 한다.
이제 바디에 대한 각 발의 각도 변화를 살펴보면, 바디에 대한 발의 위치변화를 알 수 있다.
L 발은 공중에 떠서 앞으로 d 만큼 이동하며,
R 발은 바닥에 고정된 상태로 뒤로 d 만큼 이동한다.
foot 이동 궤적 모델링(포물선)
제작한 바퀴의 링크1의 길이
링크2의 길이 이다.
따라서 다리의 최대 길이는 라고 할 수 있다.
발을 앞 또는 뒤로 뻗는 최대 거리를 로 설정하고, 궤적을 만들어 보자.
x축과 y축을 이용해 익숙한 포물선 궤적 방정식을 작성하고,
앞에서 설정한 IK 좌표축으로 변환하였다.
들어올려 앞으로 전진하는 발은 의 방정식 위를 움직이고,
바닥을 딛는 발은 위에서 움직인다. 조금만 생각해보면 3족 보행 이상도 같은 방정식을 쓸 수 있다는 사실을 알 수 있는데,
4족 보행의 경우, 바닥을 딛는 발의 궤적을 으로 삼등분 해주면 된다.
한 발을 디딜 때마다 삼등분만큼 움직이면, 디디고 있던 발이 앞으로 갈 차례가 됐을 때 정확히 60 의 위치에 도달한다.
우선 간단한 테스트를 위해 2족 보행으로 코드를 작성한다. 이 경우,
forward :
backward :
3. OpenRB-150 sketch 작성
알고리즘 정리
한 주기(2초)동안 다리 하나가 앞으로 전진하며, 나머지 다리는 뒤로 후진한다.
사전에 정의된 네 가지의 궤적이 있다.
0번: 후진 선운동 (y는 -60 ~ 60)
1번: 전진 포물선 (y는 60 ~ -60)
2번: 중립시 전진 포물선 (y는 0 ~ -60)
3번: 중립시 후진 선운동 (y는 0 ~ 60)
처음 구동을 시작할 때는 발이 중립(80,0)에 위치한다.
첫 전진을 할 때는 전진하는 발과 후진하는 발이 모두 원래 이동의 절반만 가야 하므로, 따로 2번 3번 모션으로 만들어주었다.
이후에는 무한 루프로, 전진할 다리를 계속 바꿔주면서 나머지 다리는 후진하도록 하면 된다.
코드
다음은 OpenRB-150에 주입한 sketch 코드이다.
사용한 클래스는 Dynamixel2Arduino 내장 클래스들이다.
Dynamixel2Arduino.h 클래스 목록
XL330-M288-T 모터의 공식 e-manuel
Arduino IDE 세팅은 위 링크에 자세히 설명되어 있다.
모터가 위치 명령을 받아오는 방법을 이해하기 위해 위 두 링크를 모두 자세히 읽어야 한다.
OpenRB-150 sketch || walk_test.c
#include <Dynamixel2Arduino.h>
#define USB_SERIAL Serial
#define DXL_SERIAL Serial1
const float PROTOCOL_VERSION = 2.0;
const int start = 1;
const int stop = 0;
Dynamixel2Arduino dxl(DXL_SERIAL, 57600);
float theta_H = 0.0; // 휠의 모터 각도
float theta_L = 0.0; // 다리의 모터 각도
float x = 0.0; // 다리 x좌표 계산을 위한 전역변수
float y = 0.0; // 다리 y좌표 계산을 위한 전역변수
float L1 = 35; // 링크1 길이 (mm)
float L2 = 85; // 링크2 길이 (mm)
float dt = 0.05; // dt = 0.05초
float t = 0.0; // 시간 변수
float tstep = 2.0; // 시간 스텝 길이 (2초)
// 아두이노 작동 시작 세팅
// 모든 모터 작동모드를 OP_POSITION으로 설정하고, 토크를 킨다
// 모든 모터의 포지션을 2048(0도)로 설정
// mode2 한 번 구동
void setup() {
USB_SERIAL.begin(57600); // 통신속도 설정
dxl.begin(57600); // 다이나믹셀 통신 시작
dxl.setPortProtocolVersion(PROTOCOL_VERSION); // 프로토콜 버전 설정
for (int i = 1; i <= 2; i++) { // 모든 모터 포지션모드로 설정 || 다리 개수는 2개
dxl.torqueOff(ids[i]);
dxl.torqueOff(ids[i+1]);
dxl.setOperatingMode(ids[i], OP_POSITION);
dxl.setOperatingMode(ids[i+1], OP_POSITION);
dxl.torqueOn(ids[i]);
dxl.torqueOn(ids[i+1]);
}
Serial.println("✅ OpenRB-150 펌웨어 시작됨!");
Erection(); // 직립 자세로 설정
delay(5000); // 5초 대기
while() {
computeLegIK(2,3); //2번(init 포물선) 모드, 3번 다리 || 왼다리 전진. 오른다리 후진.
computeLegIK(3,1); //3번(init line) 모드, 1번 다리
t += dt;
if(t > tstep){ //tstep까지 작동하면 초기화 및 break
t = 0.0;
break;
}
}
delay(5000); //5초 대기
}
// -------------------- 루프 --------------------
void loop(){
int walk_index = 1; // 이번 tstep에 전진시킬 다리 번호. walk이므로 하나의 다리가 전진할 때 나머지 다리는 후진한다. 1번 다리부터 전진한다.
while(walk_index <= 3){ // 다리 번호는 1, 3만 사용한다. 1, 3은 각각 오른다리와 왼다리를 의미한다.
while(){
for(int i=1; i <= 7; i += 2){ // 코드에 적용할 다리 번호 변수 i를 1, 3로 바꿔가면서 walk_index와 일치하는 지 확인
if(i == walk_index ){ // i가 walk_index와 같으면 || 해당 다리는 mode 1로 전진
computeLegIK(1, i);
}
else{
computeLegIK(0, i); // i가 walk_index와 다르면 || 해당 다리는 mode 0으로 후진
}
}
t += dt; // 시간 증가
if(t > tstep){ // tstep까지 작동하면 초기화 및 break
t = 0.0;
break;
}
delay(50); // 50ms 대기(dt만큼 대기)
}
walk_index += 2; // 전진할 다리 번호 변경. 다름 loop에 반영됨
}
}
//Command position to motor
void computeLegIK(int mode, int i) {
Parabola(mode); // compute x,y of Leg
InvKin(); // IK -> theta_H theta_L
if(i ==3 || i == 5){ // 왼쪽 다리일 경우, theta_M을 반전시킴
theta_H = -theta_H;
theta_L = -theta_L;
}
float pos_H = 2048 + (theta_H * 2048 / PI); // theta_H(rad) -> pos_H(0~4095) || 0도는 2048
float pos_L = 2048 + (theta_L * 2048 / PI); // theta_L(rad) -> pos_L(0~4095) || 0도는 2048
// 값 제한 (실제 모터 가동 범위 초과 방지)
pos_H = constrain(pos_H, 0, 4095);
pos_L = constrain(pos_L, 0, 4095);
dxl.setGoalPosition(i, pos_H);
dxl.setGoalPosition(i+1, pos_L);
}
//comtpute IK
void InvKin() {
float k1, k2, cos2, sin2; // k1, k2는 링크 길이와 관련된 변수
cos2 = (x*x + y*y - L1*L1 - L2*L2) / (2*L1*L2); // cos(theta_L) 계산
if(cos2*cos2>1){
println("cos(theta_L) > 1.0"); // cos(theta_L) 값이 1보다 크면 오류 발생
return;
}
sin2 = sqrt(1 - cos2*cos2); // sin(theta_L) 계산 || 무릎을 앞으로 굽힐 것이므로, sin(theta_L)은 양수로 설정
k1 = L1 + L2*cos2; // k1, k2 계산
k2 = L2*sin2; // k2는 항상 양수로 설정
theta_L = atan2(cos2, sin2); // theta_L 계산(rad)
theta_H = atan2(y, x) - atan2(k2, k1); // theta_H 계산(rad)
}
//현재 t와 mode에 대해 x,y좌표를 계산하는 함수
void Parabola(int mode) {
switch (mode)
{
case 0: // line
y = -60.0 + 60.0*t; // y좌표가 -60 ~ 60
x = 80.0;
break;
case 1: // parabola
y = 60.0 - 60.0*t; // y 좌표가 60 ~ -60
x = 1.0/120.0*y*y + 50.0;
break;
case 2: // init parabola
y = -30.0*t; // y 좌표가 0 ~ -60
x = 1.0/30.0*(y+30.0)*(y+30.0) + 50.0;
break;
case 3: // init line
y = 30.0*t; // y 좌표가 0 ~ 60
x = 80.0;
default:
Serial.println("parabola error");
break;
}
}
// 다리 위치를 직립 초기화. 내장 딜레이는 없음.
void Erection(){
t = tstep/2; //0번 모드 중간 위치로 발을 이동시키기 위해 t를 tstep/2로 설정
computeLegIK(0,1); //1번 다리 직립 위치
computeLegIK(0,3); //3번 다리 직립 위치
t = 0.0; // t 초기화
}추후 4족으로 코드를 수정해야 하기 때문에, 코드의 확장성을 위해 기능들을 최대한 별개의 함수로 분리하였다
틀은,
(무슨 모드로, 어떤 다리를) 명령을 내리면,
시간 스텝 동안 해당 모드의 실시간 발 좌표에 따라, 정해진 다리의 각도를 IK로 계산하고, 움직이는 것이다.
바퀴가 좌우대칭으로 각도 부호가 달라지기 때문에, 해당 되는 모터들의 부호를 뒤집어 주는 방법으로 해결하였다.
다리의 움직임이 너무 뻣뻣한 모습을 보이는데, PID 제어를 도입하거나, 속도를 군데군데서 바꿔주는 방법 등이 있을 것으로 생각된다.
또한 2번 링크가 반달 형태여서, 땅에 닿는 부분이 일정하지 않다. 이 또한 추가적인 계산이 필요하다.
4. ROS2와의 통신?
이 부분이 가장 큰 고민이다.
단순히 serial 통신을 사용하기에는, print 모니터링과 데이터 통신을 동시에 할 수 없다는 치명적인 문제가 있다.
serial 통신 한문장에 모든 데이터를 담기에는 메시지가 너무 복잡해지고, 파싱도 지나치게 디테일하게 해야 한다.
micro ROS라는 시스템으로 Arduino와 ROS2가 직접 ROS2의 topic 등과 같은 통신을 할 수 있게 한다던데, OpenRB-150 이 활용 가능한 스펙인지 테스트 해 볼 필요가 있다.
