앞으로 운동학은 박스터

Question

위해 나는 그것의 hardware specs 다음과 같은 관절 축을 기준으로 박스터 로봇이 앞으로 운동 학적 기능을 함께 넣었습니다 내가 여기서 잘못하고있는거야?

def FK_function_2(joints): 
    def yaw(theta): #(rotation around z) 
     y = np.array([[np.cos(theta), -np.sin(theta), 0], 
         [np.sin(theta), np.cos(theta), 0], 
         [0, 0, 1] ]) 
     return y 

    R01 = yaw(joints[0]).dot(np.array([[-1,  0, 0], 
             [0,  0, 1], 
             [0,  1, 0]])) 
    R12 = yaw(joints[1]).dot(np.array([[0,  0, -1], 
             [-1,  0, 0], 
             [0,  1, 0]])) 
    R23 = yaw(joints[2]).dot(np.array([[-1,  0, 0], 
             [0,  0, 1], 
             [0,  1, 0]])) 
    R34 = yaw(joints[3]).dot(np.array([[-1,  0, 0], 
             [0,  0, 1], 
             [0,  1, 0]])) 
    R45 = yaw(joints[4]).dot(np.array([[-1,  0, 0], 
             [0,  0, 1], 
             [0,  1, 0]])) 
    R56 = yaw(joints[5]).dot(np.array([[-1,  0, 0], 
             [0,  0, 1], 
             [0,  1, 0]])) 
    R67 = yaw(joints[6]).dot(np.array([[1,  0, 0], 
             [0,  1, 0], 
             [0,  0, 1]])) 

    d = np.array([0.27035, 0, 0.36435, 0, 0.37429, 0, 0.229525]) 
    a = np.array([0.069, 0, 0.069, 0, 0.010, 0, 0]) 

    l1 = np.array([a[0]*np.cos(joints[0]), a[0]*np.sin(joints[0]), d[0]]); 
    l2 = np.array([a[1]*np.cos(joints[1]), a[1]*np.sin(joints[1]), d[1]]); 
    l3 = np.array([a[2]*np.cos(joints[2]), a[2]*np.sin(joints[2]), d[2]]); 
    l4 = np.array([a[3]*np.cos(joints[3]), a[3]*np.sin(joints[3]), d[3]]); 
    l5 = np.array([a[4]*np.cos(joints[4]), a[4]*np.sin(joints[4]), d[4]]); 
    l6 = np.array([a[5]*np.cos(joints[5]), a[5]*np.sin(joints[5]), d[5]]); 
    l7 = np.array([a[6]*np.cos(joints[6]), a[6]*np.sin(joints[6]), d[6]]); 

    unit = np.array([0, 0, 0, 1]) 
    H0 = np.concatenate((np.concatenate((R01, l1.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0) 
    H1 = np.concatenate((np.concatenate((R12, l2.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0) 
    H2 = np.concatenate((np.concatenate((R23, l3.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0) 
    H3 = np.concatenate((np.concatenate((R34, l4.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0) 
    H4 = np.concatenate((np.concatenate((R45, l5.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0) 
    H5 = np.concatenate((np.concatenate((R56, l6.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0) 
    H6 = np.concatenate((np.concatenate((R67, l7.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0) 


    T = H0.dot(H1).dot(H2).dot(H3).dot(H4).dot(H5).dot(H6) 

    return T[0:3, 3] 

Answer 1

좋아, 그래서 이걸보고 너의 코드를 확인했다. 코드는 좋으며 로봇 암의 끝에서 끝까지 변형 된 정의 된기구 학적 체인과 함께 작동합니다.

(H0 * H1 * H2 * H3 * H4 * H5 * H6)은 올바른 운동 학적 체인이며 각은 팔의베이스에서 시작하여 체인에서 한 조인트에서 다음 조인트로의 변환을 나타냅니다.

변환 문제가 잘못되었습니다. H0에서 H6까지의 표현은 옳지 않으며이 행렬에서 변환이 실제 변환과 일치하지 않게하는 숫자입니다. 기본에서부터 팔 끝까지 정확한 변환을해야합니다. 그 외에는 당신의 접근 방식이 옳습니다.

변환 행렬에 일반 DH 매개 변수를 사용하는 것 같습니다. a 및 d (및 코드에 표시되지 않는 알파)에 대한 값이 꺼져서 변환이 잘못 표시됩니다. DH 매개 변수는 https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters에 나와 있습니다.

변환을 설정하기 위해 수정 된 DH 테이블을 살펴본 후 Baxter의 forward kinematics에 대한 정확한 가이드를 찾았습니다. 위의 위키 기사의 끝 부분에서 수정 된 DH 매개 변수를 살펴볼 것입니다.

박스터 앞으로 운동 학적 가이드 : 본 논문에서는https://www.ohio.edu/mechanical-faculty/williams/html/pdf/BaxterKinematics.pdf

은, 저자, 로버트 윌리엄스는 박스터 로봇 팔에 대한 DH 매개 변수를 설정하고 당신이 무엇을 (내가 사용하는 아는 것보다 다른 값을 얻는다 정상적인 DH 매개 변수,하지만 수정 된 것들을 사용하여 볼 것이다). 그의 표는 다음과 같습니다

그리고 수정 된 DH 매트릭스 사용 :

지금 당신이 H6를 통해 매트릭스 H0를 계산할 수를 길이로

원하는 경우 엔드 이펙터 형상을 추가 할 수도 있습니다. ave 그 추가 H7. 이들을 모두 여러 번 합치면 적절한 순 기구학 변형 (Forward kinematic transformation)을 얻어야합니다 (추가 자료는 종이 참조). 왼쪽 팔과 오른쪽 팔은 같은 운동학을 가지고 있습니다.

이 값을 모두 곱하면 관절 회전 및 로봇 팔의 기하학 기능인 팔 밑에서 x7, y7 및 z7 좌표에 대한 표현식을 얻을 수 있습니다. x7, y7 및 z7에 대한 표현은 17 페이지의 논문을 참조하십시오. 개별 변환에 대해서는 14 페이지를 참조하십시오.

또한 코드가 일반 trig 함수를 사용하므로 각도를 라디안으로 표시하는 것을 잊지 마십시오.

마지막 업데이트 : 난 그냥 내가 (대신 DH 매트릭스로 바로 점프의) 중간 변환 및 회전 단계를 하나씩 생각하는 것이 쉽다는 것을 기억했다. 두 가지 접근 방식은 동일하지만, 한 회전 프레임에서 다음 프레임으로 이동하는 데 필요한 개별 단계를 생각하고 있습니다.

이렇게하려면이 빌딩 블록을 사용할 수 있습니다.

순수 번역 : u는 이전 프레임의 X 축으로부터 측정 된 새로운 프레임은 이전 프레임으로부터의 거리

[1 0 0 u; 
0 1 0 v; 
0 0 1 w; 
0 0 0 1] 

.

여기서 v는 이전 y 프레임 축에서 측정 된 이전 프레임에서 새 프레임까지의 거리입니다.

여기서 w는 이전 z 프레임 축에서 측정 한 이전 프레임에서 새 프레임까지의 거리입니다. 임의의 세타에 의해 Z 축 주위

회전 : 이 임의의 세타에 로봇 조인트의 회전을 나타낸다. 중간 프레임 주위에 회전

[cos(theta) -sin(theta)  0 0; 
sin(theta)  cos(theta)   0 0; 
0     0    1 0; 
0     0    0 1] 

조합 최종 프레임 위치에 도착 (최종 방향을 얻을 수 있도록 이들 각도는 일반적 PI/2 PI 씩 것) 하여 사용할 수 아래에 표시된 중간 x 축, y 축 또는 z 축 주위의 회전.

(회전 알파에 의해 X 축 주위)

R_y(beta) = [ cos(beta)  0  sin(beta) 0; 
        0   1   0  0; 
       -sin(beta) 0  cos(beta) 0; 
        0   0   0  1]; 

(회전 감마에서 Z 축 주위)

R_x(alpha) =   [1    0   0    0; 
         0   cos(alpha) -sin(alpha)  0; 
         0   sin(alpha) cos(alpha)   0; 
         0   0   0    1]; 

(베타에 의해 Y 축에 대한 회전) :

[cos(gamma) -sin(gamma)  0  0; 
sin(gamma) cos(gamma)  0  0; 
     0   0   1  0; 
     0   0   0  1] 

이러한 빌딩 블록을 사용하면 한 프레임에서 다른 프레임으로 이동하는 순서를 만들 수 있습니다. 본질적으로 H 매트릭스는이 단계로 분해 될 수 있습니다. 체인은 다음과 같습니다 :

[이전 프레임에서 다음 프레임으로의 이전 조인트에서의 변환] = [이전 조인트의 프레임에서 표현 된 새로운 조인트로의 순수 변환] * [이전 프레임의 z 축에 대한 회전 (joint가 많은 위치를 가지고 있기 때문에 theta는 symbolic으로 남음)] * [중간 축 프레임을 중심으로 회전으로 표현 된 새로운 조인트 프레임 방향에 도달하는 모든 다른 중간 회전]

즉 근본적으로 DH 매개 변수가 도움이되는 것이지만 DH 매개 변수로 점프하는 대신 한 프레임에서 다음 프레임으로 이동하는 개별 단계를 생각하고 싶습니다.

수정 된 H0에서 H6으로 변환하면 접근 방법이 정확합니다. 코드에서 H0에서 H6까지의 정의를 변경하십시오.