Chuyển tiếp Kinematics cho Baxter

Question

Tôi đã đặt cùng hàm Chuyển tiếp Kinematics này cho rô bốt cánh tay Baxter dựa trên hardware specs và các trục khớp sau: Các vị trí chung cho phép chuyển tiếp sau không khớp với tọa độ Descartes tương ứng, Tôi làm sai ở đây?

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, vì vậy tôi đã xem xét điều này và kiểm tra mã của bạn. Mã này là tốt và làm việc với chuỗi động học được xác định của bạn với các biến đổi từ cơ sở đến cuối cánh tay robot.

(H0 * H1 * H2 * H3 * H4 * H5 * H6) là chuỗi động học chính xác trong đó mỗi đại diện cho một phép biến đổi từ một mối nối tiếp theo trong chuỗi bắt đầu tại chân cánh tay.

Vấn đề là việc biến đổi của bạn sai. Biểu diễn của bạn từ H0 đến H6 là không đúng và đó là những con số trong các ma trận này làm cho các phép biến đổi của bạn không khớp với các biến đổi thực sự diễn ra. Bạn cần phải từ các biến đổi chính xác từ cơ sở tất cả các cách để kết thúc của cánh tay. Khác hơn thế, cách tiếp cận của bạn là chính xác.

Dường như bạn đang sử dụng thông số DH bình thường cho ma trận chuyển đổi của mình. Giá trị của bạn cho a và d (và alpha không được hiển thị trong mã của bạn) bị tắt và làm cho các biến đổi được biểu thị không chính xác. Các thông số DH được nhìn thấy trong https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters.

Tôi đã tìm thấy hướng dẫn chính xác về động học chuyển tiếp của Baxter để trợ giúp sau khi trải qua bảng DH sửa đổi để thiết lập các phép biến đổi. Tôi sẽ xem xét các thông số DH đã sửa đổi ở cuối bài viết trên wiki vì hướng dẫn sử dụng nó.

Baxter Forward Kinematic Hướng dẫn:https://www.ohio.edu/mechanical-faculty/williams/html/pdf/BaxterKinematics.pdf

Trong bài báo này, tác giả, Robert Williams, thiết lập các thông số DH cho cánh tay robot Baxter và được giá trị khác với những gì bạn có (tôi biết bạn đang sử dụng các thông số DH bình thường, nhưng tôi sẽ xem xét sử dụng các thông số đã sửa đổi). bảng của mình là:

Với độ dài của:

Và sử dụng ma trận DH sửa đổi:

Bây giờ bạn có thể tính toán ma trận H0 qua H6 và nếu bạn muốn bạn cũng có thể thêm hình học hiệu ứng cuối nếu bạn ave rằng cho một H7 bổ sung. Một khi bạn nhiều chúng lại với nhau, bạn sẽ nhận được sự chuyển đổi động học thích hợp về phía trước (xem bài báo cho tài nguyên bổ sung). Cả hai cánh tay trái và phải đều có cùng một động học.

Khi bạn nhân tất cả lại với nhau, bạn sẽ nhận được biểu thức cho tọa độ x7, y7 và z7 từ chân của cánh tay là hàm của phép quay của khớp và hình dạng của cánh tay robot. Xem trang giấy ở trang 17 cho các biểu thức cho x7, y7 và z7. Cũng xem trang 14 cho các phép biến đổi riêng lẻ.

Cũng đừng quên thể hiện các góc theo radian vì mã của bạn sử dụng các chức năng trig thông thường.

Cập nhật lần cuối: Tôi chỉ nhớ rằng tôi dễ dàng nghĩ về bản dịch trung gian và các bước quay từng người một (thay vì nhảy thẳng vào ma trận DH). Hai cách tiếp cận sẽ là tương đương, nhưng tôi thích nghĩ về từng bước riêng lẻ mà nó cần để có được từ một khung xoay đến khung tiếp theo.

Đối với điều này, bạn có thể sử dụng các khối xây dựng này.

dịch tinh khiết:

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

đâu u là khoảng cách từ khung trước với khung mới đo từ trục khung x trước.

Trong đó v là khoảng cách từ khung trước đó đến khung mới được đo từ trục khung y trước đó.

Trong đó w là khoảng cách từ khung trước đến khung mới được đo từ trục khung z trước đó.

Rotation về trục z bởi theta tùy ý: này đại diện cho xoay khớp robot để một theta tùy ý.

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

Sự kết hợp của phép quay xung quanh khung hình trung gian để có được vị trí khung hình hoàn chỉnh: (những góc độ thường sẽ trong từng bước của pi/2 hoặc pi để có thể đến được với các định hướng chính thức) có thể sử dụng một vòng quay về trục x trung gian, trục y hoặc trục z được hiển thị bên dưới.

(Rotation về trục x bởi alpha)

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

(Rotation về trục y bằng beta)

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

(Rotation xung quanh trục z bởi gamma):

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

Vì vậy, với các khối xây dựng này, bạn có thể xây dựng chuỗi các bước đi từ khung này sang khung khác (về cơ bản, bất kỳ ma trận H nào cũng có thể được phân tách thành các bước này). Chuỗi sẽ giống như sau:

[H] (chuyển từ khung trước sang khung tiếp theo) = [Dịch thuần túy từ khớp nối trước sang khớp mới được biểu diễn trong khung khớp trước đó] * [Xoay về trục z của khung trước đó bởi theta (cho khớp) (vì khớp có nhiều vị trí, theta còn lại là biểu tượng)] * [Tất cả các phép quay trung gian khác để đến hướng khung hình mới được thể hiện như xoay về khung trục trung gian]

Đó là về cơ bản những gì các thông số DH giúp bạn làm, nhưng tôi muốn nghĩ về các bước riêng lẻ để có được từ một khung đến khung tiếp theo thay vì nhảy ở đó với các tham số DH.

Với các phép biến đổi H0 đến H6 đã sửa, cách tiếp cận của bạn là chính xác. Chỉ cần thay đổi các định nghĩa của H0 đến H6 trong mã của bạn.