Forward Kinematyka dla Baxtera

Question

I już ułożyła tę funkcję Forward kinematyka ARM Baxter robota na podstawie jego hardware specs a oś następujących stawów: Wspólnego stanowiska dla następujących naprzód kinematyka nie są dopasowane odpowiednie współrzędne kartezjańskie, co ja Robię źle tutaj?

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

Ok, więc patrzyłem na to i sprawdziłem twój kod. Kod jest dobry i działa ze zdefiniowanym łańcuchem kinematycznym z transformacjami od podstawy do końca ramienia robota.

(H0 * H1 * H2 * H3 * H4 * H5 * H6) jest prawidłowym łańcuchem kinematycznym, w którym każdy reprezentuje przekształcenie z jednego połączenia do drugiego w łańcuchu rozpoczynającym się od podstawy ramienia.

Problem w tym, że twoje transformacje są złe. Twoja reprezentacja od H0 do H6 jest nieprawidłowa i liczby w tych macierzach powodują, że twoje przekształcenia nie pasują do rzeczywistych transformacji, które mają miejsce. Musisz od poprawnych transformacji od podstawy do końca ramienia. Poza tym twoje podejście jest poprawne.

Wygląda na to, że używasz normalnych parametrów DH dla swoich macierzy transformacji. Twoje wartości dla aib (i alfa, które nie są pokazane w kodzie) są wyłączone i powodują niepoprawne wyrażenie transformacji. Parametry DH są widoczne w https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters.

Znalazłem dokładny przewodnik dla kinematiki do przodu firmy Baxter, aby pomóc po przejściu przez zmodyfikowaną tabelę DH, aby skonfigurować transformacje. Spojrzałbym na zmodyfikowane parametry DH na końcu artykułu wiki powyżej, ponieważ przewodnik używa tego.

Baxter proste zadanie kinematyki Przewodnik:https://www.ohio.edu/mechanical-faculty/williams/html/pdf/BaxterKinematics.pdf

W niniejszym artykule autor, Robert Williams, ustawia parametry DH dla ramienia robota Baxter i pobiera wartości różne od tego, co masz (wiem używasz normalne parametry DH, ale chciałbym przyjrzeć się używaniu zmodyfikowanych). Jego tabela:

Przy długościach:

i stosując zmodyfikowaną macierz DH:

Teraz można obliczyć macierze H0 przez H6 a jeśli chcesz, możesz również dodać geometrię efektora końcowego, jeśli h ave to dla dodatkowego H7. Po ich wielokrotnym scaleniu powinieneś uzyskać odpowiednią transformację kinematyczną do przodu (patrz dokument dotyczący dodatkowego zasobu). Zarówno lewa, jak i prawa ręka mają tę samą kinematykę.

Po pomnożeniu wszystkich razem otrzymujesz wyrażenia dla współrzędnych x7, y7 i z7 z podstawy ramienia, które są funkcjami obrotu stawów i geometrii ramienia robota. Zobacz wyrazy na stronie 17 dla wyrażeń dla x7, y7 i z7. Zobacz także str. 14 dla poszczególnych transformacji.

Nie zapomnij również wyrazić kątów w radianach, ponieważ twój kod używa zwykłych funkcji trygonometrycznych.

Ostatnia zmiana: po prostu pamiętać, że łatwiej jest mi myśleć o tłumaczeniu pośredniej i krokach obrotowych jeden po drugim (zamiast skakać prosto do macierzy DH). Te dwa podejścia będą równoważne, ale lubię myśleć o każdym kroku, który trzeba wykonać, aby przejść od jednej klatki rotacji do następnej.

W tym celu można użyć tych klocków.

Czysty Tłumaczenie:

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

gdzie U jest odległością od poprzedniej ramki nowej ramce mierzony od poprzedniego osi x ramki.

Gdzie v jest odległością od poprzedniej klatki do nowej klatki mierzonej od poprzedniej osi ramki y.

gdzie w jest odległością od poprzedniej klatki do nowej klatki, mierzonej od poprzedniej osi ramki Z.

obrotowo wokół osi za pomocą dowolnego teta: to stanowi robota wspólnego obrotu z dowolnego theta.

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

Połączenie obrotów wokół ramek pośrednich się do końcowego położenia ramy (te kąty będą zazwyczaj znajdować się w odstępach od Pi/2 lub pi, aby móc się w położeniu końcowym) można użyć obrót wokół pośredniej osi x, osi y lub osi z pokazanej poniżej.

(Obrót wokół osi x Alpha)

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

(obracanie się wokół osi y przez beta)

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

(obrót wokół osi Z pod gamma):

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

Tak więc z tymi blokami można zbudować sekwencję kroków, aby przejść z jednej ramki do drugiej (zasadniczo każda macierz H może zostać rozłożona na te kroki). Łańcuch byłby następujący:

[H] (przejście z poprzedniej klatki do następnej klatki) = [Czyste tłumaczenie z poprzedniego połączenia na nowe połączenie wyrażone w ramie poprzedniego złącza] * [Obrót wokół osi z poprzedniej ramki przez theta (dla połączenia) (ponieważ złącze ma wiele pozycji, theta jest pozostawiony jako symboliczny)] * [Wszystkie inne pośrednie obroty w celu uzyskania nowej orientacji wspólnego stawu wyrażonej jako obroty wokół ramek z pośrednimi osiami]

To jest w zasadzie to, co pomagają parametry DH, ale lubię myśleć o poszczególnych krokach, aby przejść z jednej ramki do drugiej, zamiast skakać tam z parametrami DH.

Przy skorygowanych przekształceniach H0 do H6 Twoje podejście jest poprawne. Wystarczy zmienić definicje od H0 do H6 w kodzie.