[Project][Conveyor Tracking] 5. Conveyor - Robot Motion Overlap

순서

I. Overlap의 의미
II. Overlap 방식
III. 구현

 

---

I. Overlap의 필요성

 

Conveyor Tracking은 로봇이 컨베이어의 속도를 똑같이 따라가는 것을 의미한다.

이 컨베이어 좌표 위에서 로봇이 특정 작업을 진행 할 때
즉, 컨베이어 위에서 움직이고 있는 물체에 대한 작업을 진행한다면

Tracking 알고리즘 + 일반적으로 사용자가 정의한 로봇의 모션이 동기화가 되어야한다.

예를들면, 원래 바닥과 직각으로 내려가는 Linear모션이였다면
컨베이어 위에서는 대각선으로 움직일 것이다.

우리는 Conveyor Motion에 대해 Conveyor 위치 증분을 앞서 계산하였고
이를 로봇에 적용해 로봇 모션과 합치는 작업을 진행 할 것이다.

 

---

II. Overlap 방식

 

사용할 Overlap 방식은 Global Frame Moving (가칭)이다.

Conveyor Coordinate를 Global Frame으로 정의하고
Conveyor의 이동은 Global Frame 자체가 이동 하는 것으로 구현한다.

그렇다면, Global Frame 한 단계 안에 있는 로봇 좌표 기준의 Target은
정지해있지만, Global이 움직이므로 실제로 로봇은 움직일 것이다.

이는, 버스(Conveyor = Global Frame)는 움직일 때
버스 안에 있는 사람(Robot Target)은 그대로 있지만,
바깥에서 볼 때는 움직이고 있는(Robot Actual) 상태이다.

 

물론 Motion Target 자체에 컨베이어를 추가한 Target을 적용 시키는 방법도 있다.
(예전에 했던 방법인데 Motion Planning 예외처리에 오류가 너무 많아서 위 처럼 변경..)

---

III. 구현

 

내가 해야 할 작업은

Conveyor Motion Class에서
Conveyor Frame으로 계산된 Vector6d (Conveyor 1Step 이동량)를
현재 Global Frame 좌표 기준으로 변환하여 이동량을 누적 시켜주는 것이다.

Global Coordinate -> Base(User) Coordinate -> Conveyor Coordinate
의 Depth로 이루어져 있다.

적용 방식

 

---

1. Convert 1

 

우리는 Conveyor Frame 기준 Increase Vector를 가지고 있다.

그리고 세팅에서 Base Frame 과 Conveyor Frame의 Calibration을 진행했고,
이는 Base -> Conveyor의 Frame Calibration 이다.

따라서 1차적으로 Base Frame 기준의 Increase Vector를 구하려면

이미 Calibration을 통해 알고 있는 Base -> Conveyor Inverse에 Vector을 계산해준다.

 

2. Convert 2

최종적으로 Global Frame 기준의 Increase Vector를 원하니 1과 같은 방식으로

Global -> Base Inverse에 Base기준 Increase Vector를 계산해준다.

로봇에서는 Base(User Coordinate)이고 User 세팅이 안 되어 있을 때 default base이다.

 

3. Translate

현재 Global Frame의 Matrix를 가져와 Global 기준 Increase Vector를 누적해준다.

Pose X Y Z 는 누적해서 더해주고
Pose Rx Ry Rz는 Rotation Matrix에 곱해 얻을 수 있다.

 

---

해당 알고리즘을 적용하면

로봇에 따로 임의 명령을 주지 않으면 Conveyor만 따라 움직이고

임의 명령을 주었을 때, 해당 명령 + 컨베이어 방향으로의 Motion이 합쳐져서 진행되었다.

즉, 기타 Robot Motion Planning + Trajectory Generation과 독립적으로 작동한다.