UR5 역기구학 코드 작성(step1 정기구학 코드 작성)

학교에서 그냥 일하면 재미가 없으니까 일하면서 

글써보겠습니다 ㅎ

아래 PDF를 참고했습니다.

https://tianyusong.com/wp-content/uploads/2017/12/ur5_inverse_kinematics.pdf

UR5 역기구학 코드 작성하기 

이렇게 4가지 단계로 코드 작성해보겠습니다

1. 정기구학 코드

정기구학 코드는 역기구학 코드의 답이 맞나 확인할 수 있어야 하니까 필요합니다. 

2. 역기구학 코드 

역기구학 코드는 geometrical method로 계산된 수식 이용해서 짤 겁니다. 

analytical method라고도 합니다. 

3. 솔루션 스페이스 정하기

역기구학은 해(solution)가 유일하지 않습니다. 

나온 해중에 어떤 해를 사용할 것인지 정해줘야해요

4. 싱귤러리티 체크

싱귤러리티는 특이점이라고 하는데  

특이점 근처에서는 로봇의 동작이 너무 빨라지고 특이점에 도달하면 로봇의 동작계획의 해를 찾을 수 없기 때문에 항상 체크를 해줘야해요

정기구학 코드 작성

UR5의 DH parameter를 입력하기 위해 코드를 작성해줍니다.

theta는 사용자 입력 값입니다.

이후 T matrix, Q matrix(homogenous Transformation matrix)를 만들어서 기구학 계산을 합니다.

get_rotation_matrix라는 함수는 어떤 축에 대한 회전행렬을 반환하는 함수 입니다. 

 

 

 

 

 

 

왼쪽 사진의 행렬과 비슷하지만 3x3이 아닌 4x4 행렬을 만들어 줍니다.

다음으로는 T행렬을 연속으로 곱하여 Q 행렬를 얻습니다. T 행렬은 로봇의 인접한 관절과의 관계정보를 나타낸다면 이 T행렬을 순차적으로 전부 곱하게 되면 로봇의 베이스 좌표계로 부터 로봇의 end-effector 좌표계를 얻을 수 있게 됩니다.

 

 Q행렬을 전부 곱하여 실제 UR5 로봇의 값과 비교해봅니다.

아래 사진과 같은 코드를 이용하여 각 로봇관절의 회전 각을 통하여 end-effector의 위치를 알수 있습니다.

첫번째 케이스로 shoulder가 -90도, wirst1이 -90도 나머지는 0도일때 실험입니다.

 

코드의 결과값과 로봇의 티치펜던트에서 계산한 값이 거의 동일한 것을 확인할 수 있습니다. 

두 번째 케이스로 로봇 관절각도가 전부 0도일 때 실험입니다.

이번에도 관절각 오차를 생각했을 때 거의 맞는 값을 계산한 것으로 보입니다. 

회전행렬을  euler angle로 표현하는 코드까지 작성하면 정기구학 코드는 완성인데요 

회전행렬을 euler angle로 바꾸는 건 공부할 수 있는 자료가 많아서  설명은 패스하겠습니다.

 

UR5 뿐만아니라 DH 파라미터를 알고 있다면 사용할 수 있도록 python으로 코드작성했습니다. 

코드 필요하시면 댓글 부탁드려요