[로봇소프트웨어] 역기구학(입체) 4link 직선구간 직접교시(꺽인 용접 공구)

[고구려태권V]

(이미지 출처 : robotico)  https://robotico.tistory.com/16

위 이미지처럼 말단장치로 부착된 용접도구는 꺽인 모양으로 되어 있습니다. 지난 게시글인 『역기구학(입체) 3link 직선구간 여기에서 저기까지 용접해라(직접교시)』에서는 말단장치가 없는 것으로 가정하여 3번째 link의 끝점에 대상물에 직접 접촉하는 것으로 작업을 해 보았었습니다(아래 그림).

이번에는 말단장치를 부착한 형태로 역기구학을 검토해 보겠습니다.

로봇 기구의 원점 좌표와 작업대상 위치를 그려 보았습니다.

공구의 끝 부분부터 그려 보았습니다.

추종점(follow point)은 6개(5등분)를 찍었기 때문에 공구를 6개 그렸습니다.

코딩 사례입니다.

    p7x = p8x - L8 * np.cos(alpha) * np.cos(beta)

    p7y = p8y - L8 * np.cos(alpha) * np.sin(beta)

    p7z = p8z + L8 * np.sin(alpha)

p8은 목적물 좌표이고, p7은 공구의 위쪽입니다. L8은 공구 길이, alpha는 45도 입니다.

위 방향(z축)으로 45도로 정확히 접근 되었는지 그래프 방향을 돌려 보겠습니다.

시각적으로 볼 때 잘 된 것 같습니다.

공구를 완성해 보았습니다.

공구가 꺽인 각도는 90도로 설정해 보았습니다.

세번째 link의 끝점(4번째 link의 시작점) 좌표 P3을 확정했습니다.

4번째 link와 공구는 직각인 것으로 가정했습니다.

역기구학이니 작업 대상으로부터 기구 원점으로 거꾸로 접근하고 있습니다.

공구의 배치를 어떤 형태로 할 것인지 부터 시작해서 P3(x,y,z) 좌표를 그림을 그려가며 확정했습니다. 이제부터는 원점P0(0,0,0)에서 P3(x,y,z)까지의 link 각도를 역기구학 공식들을 이용해 구하면 됩니다.

이것은 지난 게시글들에서 다루었던 것이니 과정은 생략하고 결과를 보겠습니다.

th1=79.3,  th2=43.5,  th3=-103.3

th1=73.2,  th2=42.6,  th3=-100.7

th1=67.4,  th2=41.3,  th3=-96.9

th1=62.1,  th2=39.6,  th3=-92.0

th1=57.3,  th2=37.4,  th3=-85.9

th1=52.9,  th2=34.6,  th3=-78.6

마지막으로 4번째 link를 연결해 보겠습니다.

보통은 작업 대상을 바닥에 평평하게 배치를 하게 되고 이번 사례의 경우 x축에 평행하게 배치한 것으로 작업해 보았습니다. 현실에서는 다양한 자세가 나올 수 있어 작업 대상의 x,y 좌표를 달리해 보겠습니다.

z축으로도 기울여 보겠습니다.

관절이 많을 때 역기구학으로는 해를 찾기가 매우 어렵기 때문에, 공구 끝부터 자세를 확정해 나가서 변수를 줄여 나가는 방식으로 접근해 보았습니다.