MoveL Test (check possibility)

RoboDK의 내장 함수 중,

moveLTest가 있다.

시작점, 목표 SE3, step거리. 이렇게 3개의 input을 받고, 가능 여부에 대해 int를 반환하는 함수다.

 

지금까지 Robotics에서 다룬 내용으로 collision은 확인할 수 없지만, 나름대로 구현해보려 한다.

따라서 틀린 내용 있을 수 있으니 유의해주시고 지적 환영합니다.

 

---

Move

초기 위치에서 목표 자세로 이동할 때 중요하게 고려할 사항에 대해 생각해보자.

• 최소 유클리드 거리
• 최소 에너지 소모
• 최소 맨해튼 거리
• 관절 가동 범위
• 충돌 여부
• ...

다양한 사항이 있을 수 있는데, 이동 거리가 짧더라도 시작과 끝의 해의 분기가 달라진다면 각 관절이 이동하는 각도가 커질것이며, 로봇 자체가 움직이는 반경도 커질 것이다.

 

따라서, 좋은 움직임을 하기 위해 시작과 목표지점의 자세는 같은 해의 분기를 갖는 것이 유리하다 볼 수 있다.

---

Joint Branch

Schreiber, L. T., & Gosselin, C. (2022). Determination of the inverse kinematics branches of solution based on joint coordinates for universal robots-like serial robot architecture. Journal of Mechanisms and Robotics, 14(3), 034501.

기구학적으로 한 SE3에 대해 16개의 해가 나올 수 있다.

위 세가지 분기 2×2×2 = 8에, 추가적으로 wrist에 오른손 좌표계 & 왼손 좌표계로 × 2하여
로봇 종류에 따라 8개 혹은 16개의 해를 구할 수 있다.

 

따라서 위 figure의 3 종류로 해의 분기를 따질 것이다.

Schreiber는 i, j, k의 세 parameter로 UR5의 해의 분기를 결정했다.

• 인덱스 i는 base joint solution의 인덱스에 해당하며, shoulder 방향을 나타낸다.
• 인덱스 j는 wrist의 (오른손/왼손 법칙)을 나타낸다.
• 인덱스 (k) 는 팔꿈치 방향과 관련되지만, 어깨 방향 (i) 과 결합되어 팔꿈치 방향(위/아래) g 가 다음과 같이 주어진다.
  $$ g = i \cdot k $$
• 손목 방향(안쪽/바깥쪽) 인덱스 (w)는 손목의 손 법칙(j)와  어깨 방향(i)에 영향을 받아 다음과 같이 결정된다.
  $$ w = i \cdot j $$
  • w = -1이라면, 손목이 안으로 향한다.

인덱스에 따른 joint branch table은 다음과 같다.

 

이제 자세에 따라 i, j, k를 구하는 방법을 구해보면

• $i = \text{signum} \left( a_2 \cos(\theta_3) + a_3 \cos(\theta_3 + \theta_4) + d_5 \sin(\theta_3 + \theta_4 + \theta_5) \right)$
• $j = \text{signum} (\sin\theta_5)$
• $k = \text{signum} (\sin\theta_3)$

따라서, 

1. Shoulder

if (i == 1){
	shoulder = left
}
else if (i == -1){
	shoulder = right
}

2. Elbow

int g = i * k;
if (g == 1){
	elbow = up
}
else if (g == -1){
	elbow = down
}

3. Wrist

int w = i * j;
if (w == 1){
	wrist = rh
}
else if (w == -1){
	wrist = lh
}

 

 

이제 Joint가 주어지면, 해당 자세가 어떤 joint branch에 속하는 지 알 수 있다.

---

Interpolation

moveL은 다행스럽게도 직선 궤적이라 시작점과 끝점에 대해 interpolation하기 수월한 편이다.

RoboDK에서는 거리단위로 step을 조절하지만, 필자는 일단 경로점 수로 interpolation하고자 한다.

 

우리는 지수 매핑과 로그 매핑을 통한 SE3과 se3변환에 대해 알고 있으므로, start와 target SE3 사이의 직선 경로 생성에 응용할 수 있다.

SE3에서 se3으로 로그 매핑 후, 다시 지수 매핑 하여 SE3을 구하는 것인데,

se3에 ratio를 곱해 interpolation 하는 것이다.

단순히 linear하게 interpolation하는 것 보다 리소스는 더 소요되지만, 자연스럽고 가능한 SO3가 생성된다.

이때, position에 대해서 매핑과정에 직선이 아닌 스크류 운동을 할 수 있으므로 회전행렬 SO3에 대해 로그 매핑과 지수 매핑을 실행하고

position에 대해서는 linear하게 쪼개 직선 운동을 내분하여 경로점을 생성할 것이다.

std::vector<Eigen::Matrix4d> geodesicSO3Interpolation(const Eigen::Matrix4d start, const Eigen::Matrix4d goal, int step) {
    std::vector<Eigen::Matrix4d> interpolationMats;
    interpolationMats.reserve(step + 1);

    Eigen::Matrix4d diff = SE3Inverse(start) * goal;

    auto diffRp = se3ToRp(diff);

    // SO(3) geodesic
    auto logDiffR = SO3Toso3(diffRp.first);
    double dt = 1.0 / double(step);
    for (int i = 0; i <= step; ++i) {
        double t = dt * i;
        Eigen::Matrix4d interpolationMat = start * RpToSE3(so3ToSO3(t * logDiffR), t * diffRp.second);
        interpolationMats.emplace_back(interpolationMat);
    }

    return interpolationMats;
}

 

이제 moveL_test을 구현해 볼 수 있을 것이다.

---

Algorithm

 

종합하여 알고리듬 순서도를 작성해보면 다음과 같다.

 

시작
  │
  ▼
Start Joint → Forward Kinematics → Start SE3 계산
  │
  ▼
Start SE3 → Target SE3 까지의 직선 경로 생성 (Step 수에 맞게 경로점 생성)
  │
  ▼
경로점[i]에서 경로점[i+1]로 이동 시도
  │
  ├──> Inverse Kinematics 풀기
  │      ├──> 실패 시 → Return (종료)
  │      └──> 성공 시 → Joint 값 얻기
  │
  ▼
Joint Branch 비교
  │
  ├──> 동일하면 계속 진행
  │
  ├──> 다르면 → Return (종료)
  │
  ▼
경로점[i+1]을 새로운 시작점으로 설정
  │
  ├──> 다음 경로점이 존재하면 3 ~ 4 반복
  │
  └──> 모든 경로점을 통과하면 MoveL 성공
  │
  ▼
끝

---

아쉬운 점은 정역학만 사용하고 충돌 등 다양한 조건에 대해 고려하지 않으므로 원시적인 moveL test에 해당할 것이다.

구현만 하고 실제 로봇에 작동은 아직 하지 않아 많은 테스트가 필요한 포스팅이다.

 

moveJ에 대해서도 고민했지만, 관절 경로는 정역학만으로는 부족한 것 같아 공부하고 있다. (SE3에 대해 interpolation하면 스크류운동이 되어 moveJ 궤적과 흡사할 것으로 예상되지만 실제로는 각 관절의 limit와 에너지, 가속도, 속도 등 다양한 고려가 필요)

3차나 5차함수로 궤적 생성이 필요하고, 로봇 회사마다 moveJ에 들어가는 한계값이 있어 재미있는 도전이 될 듯 하다.