Thiết Kế và Điều Khiển Robot Kawasaki RS005N

Tài liệu nghiên cứu Điều khiển robot, tổng hợp lý thuyết và thực hành, cung cấp kiến thức chuyên sâu về ., phục vụ nghiên cứu và ứng dụng thực tiễn

• Đề tài: Thiết Kế và Điều Khiển Robot Kawasaki RS005N

Mục lục chi tiết

• LỜI MỞ ĐẦU
• 1. CHƯƠNG 1: TỔNG QUAN VỀ ROBOT KAWASAKI RS005N
• 1.1. Tổng quan về robot RS005N
• 1.1.1. Tổng quan chung về robot Kawasaki RS005N
• 1.1.2. Thông số kĩ thuật
• 2. CHƯƠNG 2: TÍNH TOÁN ĐỘNG HỌC
• 2.1. Lập bảng DH và tính toán các ma trận
• 2.1.1. Từ bảng DH ta có ma trận thuần nhất của các khâu
• 2.2. Lập phương trình động lực học
• 2.2.1. Kết quả code Matlab
• 3. CHƯƠNG 3: CHỌN ĐỘNG CƠ
• 3.1. Động cơ khâu 1: 130ST-M07725
• 3.2. Động cơ khâu 2: 130ST-M07725
• 3.3. Động cơ khâu 3: 130ST-M07725
• 3.4. Động cơ khâu 4: 130ST-M07725
• 3.5. Động cơ khâu 5: 130ST-M07725
• 3.6. Động cơ khâu 6: 130ST-M07725
• 4. CHƯƠNG 4: THIẾT KẾ BỘ ĐIỀU KHIỂN
• 4.1. Phương trình động lực học robot cho từng khâu
• 4.2. Thiết kế bộ điều khiển PD và PID
• 4.2.1. Phương trình khâu 1
• 4.2.2. Phương trình khâu 2
• 4.2.3. Phương trình khâu 3
• 4.2.4. Phương trình khâu 4
• 4.2.5. Phương trình khâu 5
• 4.2.6. Phương trình khâu 6
• KẾT LUẬN CHUNG
• NHẬN XÉT, ĐÁNH GIÁ CỦA GIẢNG VIÊN
• TÀI LIỆU THAM KHẢO

Tóm tắt

I. Khám Phá Robot Kawasaki RS005N Tổng Quan Toàn Diện

Robot Kawasaki RS005N là một trong những mô hình robot công nghiệp nổi bật thuộc dòng R-series của Kawasaki, được thiết kế cho các ứng dụng yêu cầu tốc độ cao và tải trọng thấp. Việc thiết kế và điều khiển Robot Kawasaki RS005N đòi hỏi sự hiểu biết sâu sắc về cấu trúc cơ khí, động học và động lực học. Mô hình này thiết lập một tiêu chuẩn mới cho các robot công nghiệp cỡ nhỏ và vừa, nổi bật với thiết kế nhỏ gọn nhưng vẫn đảm bảo phạm vi làm việc và tầm với vượt trội. Cấu trúc của robot bao gồm 6 bậc tự do, cho phép thực hiện các chuyển động phức tạp với độ chính xác cao. Việc phân tích kỹ lưỡng các thông số kỹ thuật và đặc tính vật liệu của từng khâu là bước đầu tiên và quan trọng nhất trong quá trình xây dựng mô hình điều khiển hiệu quả, đáp ứng các yêu cầu khắt khe trong sản xuất công nghiệp hiện đại.

1.1. Phân tích thông số kỹ thuật và cấu trúc của RS005N

Robot Kawasaki RS005N là một cánh tay robot đa năng với 6 bậc tự do (6 khâu, 5 khớp), được chế tạo để đáp ứng các nhiệm vụ xử lý vật liệu và lắp ráp quy mô nhỏ. Theo tài liệu nghiên cứu, robot có tầm với tối đa là 705 mm và có thể được lắp đặt trên sàn hoặc trên trần. Một trong những thông số ấn tượng nhất là độ lặp lại vị trí, đạt ±0,02 mm, tuân thủ tiêu chuẩn ISO 9283, đảm bảo độ chính xác cao trong các thao tác lặp đi lặp lại. Cấu trúc cơ khí của robot được chế tạo từ các vật liệu bền bỉ. Cụ thể, đế robot làm từ gang xám với khối lượng 4,844 kg, trong khi các khâu còn lại chủ yếu sử dụng thép carbon để cân bằng giữa độ cứng và khối lượng. Ví dụ, khâu 1 có khối lượng 5,4798 kg và khâu 2 là 5,0639 kg. Các thông số này, bao gồm khối lượng và tọa độ trọng tâm của từng khâu, là dữ liệu đầu vào không thể thiếu cho các bước tính toán động học và động lực học sau này.

1.2. Các ứng dụng công nghiệp phổ biến của Robot Kawasaki

Với thiết kế linh hoạt và hiệu suất cao, Robot Kawasaki RS005N được ứng dụng rộng rãi trong nhiều lĩnh vực công nghiệp. Các ứng dụng chính bao gồm lắp ráp linh kiện điện tử, xử lý vật liệu trong các dây chuyền sản xuất, và bảo dưỡng máy móc. Khả năng mang tải trọng thấp (dưới 5kg) làm cho nó trở thành lựa chọn lý tưởng cho các nhiệm vụ đòi hỏi sự khéo léo và tốc độ hơn là sức mạnh. Ngoài ra, RS005N còn được sử dụng trong các quy trình hàn kín, phân phối chất kết dính, và loại bỏ vật liệu thừa. Sự kết hợp giữa tốc độ, độ chính xác và phạm vi làm việc rộng giúp tối ưu hóa hiệu quả sản xuất, giảm thiểu lỗi do con người và tăng năng suất cho các nhà máy. Việc mô hình hóa và điều khiển chính xác robot cho phép triển khai các ứng dụng này một cách an toàn và hiệu quả.

II. Thách Thức Điều Khiển RS005N Bài Toán Động Học Robot

Để điều khiển chính xác vị trí và hướng của điểm tác động cuối (end-effector), việc xây dựng một mô hình toán học mô tả mối quan hệ giữa các biến khớp và không gian làm việc là bắt buộc. Đây chính là bài toán động học robot, một trong những thách thức cốt lõi trong thiết kế và điều khiển Robot Kawasaki RS005N. Mô hình này bao gồm hai bài toán chính: động học thuận và động học ngược. Động học thuận xác định vị trí của công cụ từ các góc khớp đã biết, trong khi động học ngược tìm ra các góc khớp cần thiết để đạt được vị trí mong muốn. Giải quyết thành công bài toán này là tiền đề để lập trình quỹ đạo chuyển động và thiết kế các bộ điều khiển cấp cao, đảm bảo robot hoạt động đúng theo yêu cầu nhiệm vụ.

2.1. Phương pháp Denavit Hartenberg DH trong phân tích

Phương pháp Denavit-Hartenberg, hay còn gọi là bảng DH, là một công cụ toán học tiêu chuẩn được sử dụng để gán hệ tọa độ một cách có hệ thống cho các khâu của một cánh tay robot. Phương pháp này giúp đơn giản hóa việc tính toán các ma trận biến đổi thuần nhất giữa các hệ tọa độ liền kề. Mỗi phép biến đổi được mô tả bởi bốn tham số: aᵢ (chiều dài khâu), αᵢ (góc xoắn khâu), dᵢ (khoảng cách khớp), và θᵢ (góc khớp). Việc thiết lập chính xác các tham số này trong bảng DH cho phép mô tả toàn bộ cấu trúc hình học của robot RS005N một cách ngắn gọn và rõ ràng. Đây là bước nền tảng để xây dựng ma trận thuần nhất tổng thể, liên kết hệ tọa độ của đế với hệ tọa độ của điểm tác động cuối, từ đó giải quyết bài toán động học thuận.

2.2. Xây dựng ma trận thuần nhất cho các khâu robot RS005N

Từ các tham số trong bảng DH đã được thiết lập, ma trận biến đổi thuần nhất Aᵢ giữa khâu thứ i-1 và khâu thứ i được tính toán. Ma trận này là một ma trận 4x4, kết hợp cả phép quay và phép tịnh tiến, mô tả mối quan hệ không gian giữa hai hệ tọa độ liên tiếp. Bằng cách nhân tuần tự các ma trận này, ta có thể thu được ma trận biến đổi tổng thể T₀ⁿ từ hệ tọa độ gốc (đế) đến hệ tọa độ của khâu cuối cùng. Tài liệu nghiên cứu đã chỉ ra việc tính toán chi tiết các ma trận từ T₀¹ đến T₀⁶. Ví dụ, T01=[cos(q1) -sin(q1) 0 0; sin(q1) cos(q1) 0 0; 0 0 1 0.35; 0 0 0 1]. Ma trận tổng thể này cung cấp một phương trình tường minh, cho phép xác định vị trí và hướng của dụng cụ cuối cùng dựa trên một bộ giá trị góc khớp (q1 đến q6), hoàn thành bài toán động học thuận cho Robot Kawasaki RS005N.

III. Hướng Dẫn Lập Phương Trình Động Lực Học Cho Robot RS005N

Sau khi hoàn thành mô hình động học, bước tiếp theo là xây dựng mô hình động lực học. Mô hình này mô tả mối quan hệ giữa lực/momen tác động lên các khớp và chuyển động (vận tốc, gia tốc) của robot. Việc lập phương trình động lực học là cực kỳ quan trọng, vì nó là cơ sở để thiết kế các bộ điều khiển hiệu quả, mô phỏng hoạt động của robot và tính toán năng lượng tiêu thụ. Đối với một hệ thống phức tạp như Robot Kawasaki RS005N 6 bậc tự do, phương pháp Lagrange loại II thường được ưu tiên sử dụng do tính hệ thống và khả năng áp dụng cho các cơ cấu đa vật thể. Phương pháp này dựa trên việc tính toán năng lượng (động năng và thế năng) của toàn bộ hệ thống thay vì phân tích lực và momen trên từng vật riêng lẻ.

3.1. Áp dụng phương trình Lagrange để mô hình hóa hệ thống

Phương pháp Lagrange dựa trên nguyên lý tính toán động năng (K) và thế năng (P) của toàn bộ cánh tay robot. Lực tổng quát τᵢ tác động lên khớp thứ i được xác định thông qua phương trình: τᵢ = d/dt(∂L/∂q̇ᵢ) - ∂L/∂qᵢ, trong đó L = K - P là hàm Lagrange, qᵢ là biến khớp và q̇ᵢ là vận tốc khớp. Để áp dụng phương trình này, cần tính toán chi tiết động năng và thế năng của từng khâu, có tính đến khối lượng, momen quán tính, và vị trí trọng tâm. Tài liệu nghiên cứu cung cấp đầy đủ các giá trị momen quán tính tại trọng tâm cho cả 6 khâu (ví dụ, Ixx1 = 0,0793 kg.m²) và các vector tọa độ trọng tâm. Các tính toán này rất phức tạp và thường được hỗ trợ bởi các công cụ phần mềm chuyên dụng để đảm bảo độ chính xác.

3.2. Vai trò của ma trận quán tính và vector trọng trường

Kết quả cuối cùng của việc áp dụng phương trình Lagrange là một phương trình động lực học ở dạng ma trận: M(q)q̈ + C(q,q̇)q̇ + G(q) = τ. Trong đó, M(q) là ma trận quán tính của robot, đối xứng và xác định dương; C(q,q̇)q̇ biểu diễn các lực Coriolis và ly tâm; G(q) là vector lực/momen do trọng trường gây ra; và τ là vector momen tác động tại các khớp. Việc xác định chính xác các ma trận này là nền tảng cho việc thiết kế và điều khiển Robot Kawasaki RS005N. Tài liệu đã sử dụng mô phỏng Matlab để tính toán và khai triển chi tiết các thành phần của phương trình này, cho thấy sự phức tạp và liên kết phi tuyến tính giữa các khớp robot. Đoạn mã Matlab được cung cấp trong nghiên cứu là một minh chứng rõ ràng cho quá trình tính toán này.

IV. Bí Quyết Thiết Kế Bộ Điều Khiển PID Cho Robot Kawasaki

Từ mô hình động lực học đã xây dựng, nhiệm vụ tiếp theo là thiết kế một bộ điều khiển có khả năng bám theo quỹ đạo mong muốn một cách chính xác và ổn định. Bộ điều khiển PID (Proportional-Integral-Derivative) là một trong những giải pháp phổ biến và hiệu quả nhất trong điều khiển robot công nghiệp. Nó hoạt động dựa trên việc tính toán sai lệch (error) giữa giá trị đặt (setpoint) và giá trị thực tế của biến khớp. Tín hiệu điều khiển được tạo ra từ ba thành phần: thành phần tỉ lệ (P) giúp giảm sai lệch, thành phần tích phân (I) giúp khử sai lệch tĩnh, và thành phần vi phân (D) giúp cải thiện đáp ứng quá độ và tăng độ ổn định của hệ thống. Việc lựa chọn thông số Kp, Ki, Kd phù hợp là chìa khóa để đạt được hiệu suất điều khiển tối ưu cho Robot Kawasaki RS005N.

4.1. Xây dựng phương trình điều khiển PD và PID cho từng khớp

Nghiên cứu tập trung vào việc thiết kế bộ điều khiển PD và PID cho từng khớp của robot. Phương trình động lực học robot cho từng khâu đã được tách riêng để thuận tiện cho việc thiết kế. Ví dụ, đối với khâu 1, phương trình có dạng τ₁ = M₁₁q̈₁ + C₁ + G₁. Dựa trên đó, luật điều khiển PID được thiết kế để tạo ra momen τ₁ cần thiết. Tín hiệu điều khiển được tính toán dựa trên sai số e(t) = q_d(t) - q(t), trong đó q_d là vị trí mong muốn và q là vị trí thực tế. Bộ điều khiển PID sẽ tính toán momen khớp dựa trên sai số này và các đạo hàm, tích phân của nó. Mục tiêu là làm cho sai số e(t) tiến về 0, đảm bảo khớp robot bám chính xác quỹ đạo đã định trước. Quy trình này được lặp lại cho cả sáu khớp của robot.

4.2. Tầm quan trọng của việc lựa chọn động cơ servo phù hợp

Hiệu suất của bộ điều khiển phụ thuộc rất lớn vào khả năng của cơ cấu chấp hành, tức là các động cơ tại mỗi khớp. Việc lựa chọn động cơ phải dựa trên yêu cầu về momen, tốc độ và độ chính xác. Tài liệu nghiên cứu đã đề xuất sử dụng cùng một loại động cơ servo cho cả sáu khâu: model 130ST-M07725. Đây là một quyết định thiết kế quan trọng, có thể nhằm mục đích đồng bộ hóa và đơn giản hóa việc bảo trì. Động cơ servo 130ST-M07725 phải có khả năng cung cấp đủ momen xoắn để thắng được momen quán tính, lực Coriolis, và ảnh hưởng của trọng trường đã được tính toán trong phương trình động lực học. Việc lựa chọn một động cơ mạnh mẽ và có đáp ứng nhanh là yếu tố quyết định để bộ điều khiển PID có thể hoạt động hiệu quả và hệ thống đạt được hiệu suất mong muốn.

V. Kết Quả Mô Phỏng Và Ứng Dụng Thực Tiễn Của Robot RS005N

Quá trình thiết kế và điều khiển Robot Kawasaki RS005N không chỉ dừng lại ở lý thuyết mà cần được xác minh thông qua mô phỏng và triển khai thực tế. Mô phỏng đóng vai trò quan trọng trong việc kiểm tra tính đúng đắn của mô hình động học, động lực học và đánh giá hiệu suất của bộ điều khiển được thiết kế. Bằng cách sử dụng các công cụ như Matlab/Simulink, có thể tạo ra một môi trường ảo để robot thực hiện các nhiệm vụ theo quỹ đạo đặt trước. Kết quả từ mô phỏng cung cấp những thông tin chi tiết về đáp ứng của hệ thống, sai số bám quỹ đạo, và sự ổn định của bộ điều khiển trước khi áp dụng vào robot vật lý, giúp tiết kiệm thời gian, chi phí và giảm thiểu rủi ro.

5.1. Đánh giá đáp ứng hệ thống thông qua mô phỏng Matlab

Sử dụng mô phỏng Matlab, nghiên cứu có thể kiểm chứng hiệu quả của bộ điều khiển PID đã thiết kế. Quá trình mô phỏng bao gồm việc cung cấp một quỹ đạo tham chiếu (ví dụ, một hàm sin hoặc một đường cong phức tạp) cho từng khớp robot. Sau đó, mô hình động lực học và bộ điều khiển sẽ tính toán chuyển động thực tế của robot. Kết quả mô phỏng thường được biểu diễn dưới dạng đồ thị, so sánh giữa tín hiệu đặt và tín hiệu đáp ứng của vị trí khớp theo thời gian. Từ đó, có thể đánh giá các chỉ số quan trọng như thời gian xác lập, độ vọt lố, và sai số ở trạng thái xác lập. Các kết quả này cho phép tinh chỉnh lại các thông số của bộ điều khiển để đạt được hiệu suất tối ưu, đảm bảo robot hoạt động mượt mà và chính xác.

5.2. Tiềm năng tích hợp vào dây chuyền sản xuất tự động

Một mô hình điều khiển chính xác và ổn định cho Robot Kawasaki RS005N mở ra nhiều tiềm năng ứng dụng trong các dây chuyền sản xuất tự động. Robot có thể được lập trình để thực hiện các nhiệm vụ như gắp và đặt sản phẩm (pick-and-place) với tốc độ cao, lắp ráp các chi tiết nhỏ đòi hỏi sự khéo léo, hoặc thực hiện kiểm tra chất lượng sản phẩm bằng hệ thống thị giác máy. Việc tích hợp cánh tay robot vào dây chuyền giúp tăng đáng kể năng suất, cải thiện tính nhất quán của sản phẩm và giải phóng con người khỏi các công việc lặp đi lặp lại hoặc nguy hiểm. Sự thành công của việc thiết kế và điều khiển Robot Kawasaki RS005N chính là nền tảng để hiện thực hóa những nhà máy thông minh và linh hoạt trong tương lai.

Trích đoạn nội dung tài liệu

CHƯƠNG 1. TỔNG QUAN VỀ ROBOT KAWASAKI RS005N 1.Tổng quan về robot RS005N 1.Tổng quan chung về robot Kawasaki RS005N Các rô-bốt sê-ri R đang thiết lập tiêu chuẩn cho tất cả các rô-bốt công nghiệp vừa và nhỏ. Với tốc độ cao, tải trọng thấp, hiệu suất cao Thiết kế nhỏ gọn, cùng với tốc độ, phạm vi tiếp cận và phạm vi làm việc hàng đầu trong ngành khiến rô-bốt dòng R trở nên lý tưởng cho nhiều ứng dụng trong vô số ngành công nghiệp đa dạng. Model RS005N là một trong những robot dòng R nhỏ nhất, lý tưởng cho các ứng dụng xử lý vật liệu quy mô nhỏ yêu cầu tải trọng thấp.1: Mô hình cánh tay robot 6 1.Thông số kĩ thuật Loại người máy: Rô bốt đa năng (tải trọng 3-80 kg) Chiều cao tối đa với tới: 705 mm Gắn kết: gắn trên sàn nhà, trần Dòng người máy: dòng R Bậc tự do: 6 khâu, 5 khớp Độ lặp lại vị trí : ±0,02 mm (bề mặt bích cổ tay) tuân theo tiêu chuẩn ISO 9283 1.Ứng dụng Lắp ráp , Xử lý vật liệu , Bảo dưỡng máy , Niêm phong / Phân phối , Loại bỏ vật liệu 1.Đế Robot  Vật liệu: gang xám.

 Khối lượng: 4,844 (kg) 7 Hình 1.Khâu 1  vật liệu: thép cabon.  Khối lượng: 5,4798 (kg) 8 Hình 1.3: Khâu 1 của robot.Khâu 2  vật liệu: thép cabon.  Khối lượng: 5,0639 (kg) 9 Hình 1.4: Khâu 2 của robot.Khâu 3  vật liệu: thép cabon.  Khối lượng: 5,0639 (kg) 10 Hình 1.5: Khâu 3 của robot.Khâu 4  vật liệu: thép cabon.

 Khối lượng: 1,0215 (kg) 11 Hình 1.6: Khâu 4 của robot.Khâu 5  vật liệu: thép cabon.  Khối lượng: 0,348 (kg) 12 Hình 1.7: Khâu 5 của robot.Khâu 6  vật liệu: thép cabon.  Khối lượng: 0,0145 (kg) 13 Hình 1.8: Khâu 6 của robot. CHƯƠNG 2 : TÍNH TOÁN ĐỘNG HỌC 2.Lập bảng DH và tính toán các ma trận 14 Hình 2.1: Gán hệ trục tọa độ cho cánh tay robot.2: Lược đồ cánh tay robot Bảng DH: Khâu i-1 i-1 di i 1 0 0 1 1 2 0 90˚ 0 90˚[phone]˚ 5 4 5 0 -90˚ 6 5 6 0 90˚ 7 6 16 Từ bảng DH ta có ma trận thuần nhất của các khâu:  Vecto tọa độ trọng tâm (bỏ qua khâu đế) Trọng tâm khâu 1: x1= 0,0385; y1= 0,0009; z1= 0,0763 (m) Trọng tâm khâu 2: x2 = 0,1113; y2 = -0,0001; z2= 0,0068 (m).

Trọng tâm khâu 3: x3= 0,1113; y3= -0,0001; z3= 0,0068 (m). 17 Trọng tâm khâu 4: x4= 0; y4= 0,0008; z4= 0,0865 (m). Trọng tâm khâu 5: x5= 0; y5= -0,0167; z5= 0 (m). Trọng tâm khâu 6: x6= 0,0002; y6= 0; z6= -0,0067 (m).

 Vecto gia tốc trọng trường  Momen quán tính tại trọng tâm (kg.m2) Khâu 1: Khâu 2: Khâu 3: Ixx1= 0,0793 Ixx2= 0,0324 Ixx3= 0,0324 Iyx1= 0,0015 Iyx2= 0,0018 Iyx3= 0,0018 Izx1= 0,0263 Izx2= 0,0042 Izx3= 0,0042 Ixy1= 0,0015 Ixy2= 0,0018 Ixy3= 0,0018 Iyy1= 0,0784 Iyy2= 0,157 Iyy3= 0,157 Izy1= 0,0011 Izy2= 0,0001 Izy3= 0,0001 Ixz1= 0,0263 Izx2= 0,0042 Ixz3= 0,0042 Iyz1= 0,0011 Izy2= 0,0001 Iyz3= 0,0001 Izz1= 0,067 Izz2= 0,1374 Izz3= 0,1374 Khâu 4: Khâu 5: Khâu 6: Ixx4= 0,0121 Ixx5= 0,0005 Ixx6= 0 Iyx4= 0 Iyx5= 0 Iyx6= 0 Izx4= 0 Izx5= 0 Izx6= 0 18 Ixy4= 0 Ixy5= 0 Ixy6= 0 Iyy4= 0,0113 Iyy5= 0,0002 Iyy6= 0 Izy4= 0,0001 Izy5= 0 Izy6= 0 Ixz4= 0 Ixz5= 0 Ixz6= 0 Iyz4= 0,0001 Iyz5= 0 Iyz6= 0 Izz4= 0,0017 Izz5= 0,0005 Izz6= 0 ● Ma trận giả quán tính được tính theo công thức: 2.Lập phương trình động lực học. Ta có lực tổng quát của khâu i: Áp dụng phương trình ĐLH cho Robot 6 bậc tự do ( n=6) và khai triển chi tiết ta được: 19 Code Matlab: clc ; syms q1 q2 q3 q4 q5 q6 qd1 qd2 qd3 qd4 qd5 qd6 qdd1 qdd2 qdd3 qdd4 qdd5 qdd6 ; Vg=[0; 0; -9.0067; 1 ]; dx1=0; dy1=0; dz1=0; dx2=0; dy2=0 ; dz2=0; dx3=0; dy3=0 ; dz3=0; dx4=0 ; dy4=0 ; dz4=0; dx5=0 ; dy5=0 ; dz5=0; dx6=0 ; dy6=0 ; dz6=0; m1=5.0121; Ixy4 = 0; Ixz4 = 0; Iyx4 = 0; Iyy4 = 0.0005; Ixy5 = 0; Ixz5 = 0; Iyx5 = 0; Iyy5 = 0.0002; Iyz5 = 0; Izx5 = 0; Izy5 = 0; Izz5 = 0.0005; %khau6 Ixx6 = 0; Ixy6 = 0; Ixz6 = 0; Iyx6 = 0; Iyy6 = 0; Iyz6 = 0; Izx6 = 0; Izy6 = 0; Izz6 = 0; %Tinh J J1=[(-Ixx1+Iyy1+Izz1)/2+m1*dx1*dx1 Ixy1+m1*dx1*dy1 Ixz1+m1*dx1*dz1 -m1*dx1; Ixy1+m1*dx1*dy1 (Ixx1- Iyy1+Izz1)/2+m1*dy1*dy1 Iyz1+m1*dy1*dz1 -m1*dy1; Ixz1+m1*dx1*dz1 Iyz1+m1*dy1*dz1 (Ixx1+Iyy1- Izz1)/2+m1*dz1*dz1 -m1*dz1; -m1*dx1 -m1*dy1 -m1*dz1 m1] J2=[(-Ixx2+Iyy2+Izz2)/2+m2*dx2*dx2 Ixy2+m2*dx2*dy2 Ixz2+m2*dx2*dz2 -m2*dx2; Ixy2+m2*dx2*dy2 (Ixx2- Iyy2+Izz2)/2+m2*dy2*dy2 Iyz2+m2*dy2*dz2 -m2*dy2; Ixz2+m2*dx2*dz2 Iyz2+m2*dy2*dz2 (Ixx2+Iyy2- Izz2)/2+m2*dz2*dz2 -m2*dz2; -m2*dx2 -m2*dy2 -m2*dz2 m2] J3=[(-Ixx3+Iyy3+Izz3)/2+m3*dx3*dx3 Ixy3+m3*dx3*dy3 Ixz3+m3*dx3*dz3 -m3*dx3; Ixy3+m3*dx3*dy3 (Ixx3- Iyy3+Izz3)/2+m3*dy3*dy3 Iyz3+m3*dy3*dz3 -m3*dy3; Ixz3+m3*dx3*dz3 Iyz3+m3*dy3*dz3 (Ixx3+Iyy3- Izz3)/2+m3*dz3*dz3 -m3*dz3; -m3*dx3 -m3*dy3 -m3*dz3 m3] J4=[(-Ixx4+Iyy4+Izz4)/2+m4*dx4*dx4 Ixy4+m4*dx4*dy4 Ixz4+m4*dx4*dz4 -m4*dx4; Ixy4+m4*dx4*dy4 (Ixx4- Iyy4+Izz4)/2+m4*dy4*dy4 Iyz4+m4*dy4*dz4 -m4*dy4; Ixz4+m4*dx4*dz4 Iyz4+m4*dy4*dz4 (Ixx4+Iyy4- Izz4)/2+m4*dz4*dz4 -m4*dz4; -m4*dx4 -m4*dy4 -m4*dz4 m4] 22 J5=[(-Ixx5+Iyy5+Izz5)/2+m5*dx5*dx5 Ixy5+m5*dx5*dy5 Ixz5+m5*dx5*dz5 -m5*dx5; Ixy5+m5*dx5*dy5 (Ixx5- Iyy5+Izz5)/2+m5*dy5*dy5 Iyz5+m5*dy5*dz5 -m5*dy5; Ixz5+m5*dx5*dz5 Iyz5+m5*dy5*dz5 (Ixx5+Iyy5- Izz5)/2+m5*dz5*dz5 -m5*dz5; -m5*dx5 -m5*dy5 -m5*dz5 m5] J6=[(-Ixx6+Iyy6+Izz6)/2+m6*dx6*dx6 Ixy6+m6*dx6*dy6 Ixz6+m6*dx6*dz6 -m6*dx6; Ixy6+m6*dx6*dy6 (Ixx6- Iyy6+Izz6)/2+m6*dy6*dy6 Iyz6+m6*dy6*dz6 -m6*dy6; Ixz6+m6*dx6*dz6 Iyz6+m6*dy6*dz6 (Ixx6+Iyy6- Izz6)/2+m6*dz6*dz6 -m6*dz6; -m6*dx6 -m6*dy6 -m6*dz6 m6] %ma tran bien doi thuan nhat T01=[cos(q1) -sin(q1) 0 0;sin(q1) cos(q1) 0 0; 0 0 1 0. trace(diff(diff(T01,q1),q1)*J1*transpose(diff(T01,q1)))*qd1*qd1+. trace(diff(diff(T02,q1),q1)*J2*transpose(diff(T02,q1)))*qd1*qd1+.

2*trace(diff(diff(T02,q1),q2)*J2*transpose(diff(T02,q1)))*qd1*qd2+. trace(diff(diff(T02,q2),q2)*J2*transpose(diff(T02,q1)))*qd2*qd2+. trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q1)))*qd1*qd1+. 2*trace(diff(diff(T03,q1),q2)*J3*transpose(diff(T03,q1)))*qd1*qd2+.

2*trace(diff(diff(T03,q1),q3)*J3*transpose(diff(T03,q1)))*qd1*qd3+. trace(diff(diff(T03,q2),q2)*J3*transpose(diff(T03,q1)))*qd2*qd1+. 2*trace(diff(diff(T03,q2),q3)*J3*transpose(diff(T03,q1)))*qd2*qd3+. trace(diff(diff(T03,q3),q3)*J3*transpose(diff(T03,q1)))*qd3*qd3+.

trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q1)))*qd1*qd1+. 2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q1)))*qd1*qd2+. 2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q1)))*qd1*qd3+. 2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q1)))*qd1*qd4+.

trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q1)))*qd2*qd2+. 2*trace(diff(diff(T04,q2),q3)*J4*transpose(diff(T04,q1)))*qd2*qd3+. 2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q1)))*qd2*qd4+. trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q1)))*qd3*qd3+.

2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q1)))*qd3*qd4+. trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q1)))*qd4*qd4+. 24 trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q1)))*qd1*qd1+. 2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q1)))*qd1*qd2+.

2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q1)))*qd1*qd3+. 2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q1)))*qd1*qd4+. 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q1)))*qd1*qd5+. trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q1)))*qd2*qd2+.

2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q1)))*qd2*qd3+. 2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q1)))*qd2*qd4+. 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q1)))*qd2*qd5+. trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q1)))*qd3*qd3+.

2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q1)))*qd3*qd4+. 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q1)))*qd3*qd5+. trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q1)))*qd4*qd4+. 2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q1)))*qd4*qd5+.

trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q1)))*qd5*qd5+. trace(diff(diff(T06,q1),q1)*J6*transpose(diff(T06,q1)))*qd1*qd1+. 2*trace(diff(diff(T06,q1),q2)*J6*transpose(diff(T06,q1)))*qd1*qd2+. 2*trace(diff(diff(T06,q1),q3)*J6*transpose(diff(T06,q1)))*qd1*qd3+.

2*trace(diff(diff(T06,q1),q4)*J6*transpose(diff(T06,q1)))*qd1*qd4+. 2*trace(diff(diff(T06,q1),q5)*J6*transpose(diff(T06,q1)))*qd1*qd5+. 2*trace(diff(diff(T06,q1),q6)*J6*transpose(diff(T06,q1)))*qd1*qd6+. trace(diff(diff(T06,q2),q2)*J6*transpose(diff(T06,q1)))*qd2*qd2+.

2*trace(diff(diff(T06,q2),q3)*J6*transpose(diff(T06,q1)))*qd2*qd3+. 2*trace(diff(diff(T06,q2),q4)*J6*transpose(diff(T06,q1)))*qd2*qd4+. 2*trace(diff(diff(T06,q2),q5)*J6*transpose(diff(T06,q1)))*qd2*qd5+. 2*trace(diff(diff(T06,q2),q6)*J6*transpose(diff(T06,q1)))*qd2*qd6+.

trace(diff(diff(T06,q3),q3)*J6*transpose(diff(T06,q1)))*qd3*qd3+. 2*trace(diff(diff(T06,q3),q4)*J6*transpose(diff(T06,q1)))*qd3*qd4+. 2*trace(diff(diff(T06,q3),q5)*J6*transpose(diff(T06,q1)))*qd3*qd5+. 2*trace(diff(diff(T06,q3),q6)*J6*transpose(diff(T06,q1)))*qd3*qd6+.

trace(diff(diff(T06,q4),q4)*J6*transpose(diff(T06,q1)))*qd4*qd4+. 25 2*trace(diff(diff(T06,q4),q5)*J6*transpose(diff(T06,q1)))*qd4*qd5+. 2*trace(diff(diff(T06,q4),q6)*J6*transpose(diff(T06,q1)))*qd4*qd6+. trace(diff(diff(T06,q5),q5)*J6*transpose(diff(T06,q1)))*qd5*qd5+.

2*trace(diff(diff(T06,q5),q6)*J6*transpose(diff(T06,q1)))*qd5*qd6+. trace(diff(diff(T06,q6),q6)*J6*transpose(diff(T06,q1)))*qd6*qd6-. trace(diff(diff(T02,q1),q1)*J2*transpose(diff(T02,q2)))*qd1*qd1+. 2*trace(diff(diff(T02,q1),q2)*J2*transpose(diff(T02,q2)))*qd1*qd2+.

trace(diff(diff(T02,q2),q2)*J2*transpose(diff(T02,q2)))*qd2*qd2+. trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q2)))*qd1*qd1+. 2*trace(diff(diff(T03,q1),q2)*J3*transpose(diff(T03,q2)))*qd1*qd2+. 2*trace(diff(diff(T03,q1),q3)*J3*transpose(diff(T03,q2)))*qd1*qd3+.

trace(diff(diff(T03,q2),q2)*J3*transpose(diff(T03,q2)))*qd2*qd1+. 2*trace(diff(diff(T03,q2),q3)*J3*transpose(diff(T03,q2)))*qd2*qd3+. trace(diff(diff(T03,q3),q3)*J3*transpose(diff(T03,q2)))*qd3*qd3+. trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q2)))*qd1*qd1+.

2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q2)))*qd1*qd2+. 2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q2)))*qd1*qd3+. 2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q2)))*qd1*qd4+. trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q2)))*qd2*qd2+.

2*trace(diff(diff(T04,q2),q3)*J4*transpose(diff(T04,q2)))*qd2*qd3+. 2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q2)))*qd2*qd4+. trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q2)))*qd3*qd3+. 2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q2)))*qd3*qd4+.

trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q2)))*qd4*qd4+. trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q2)))*qd1*qd1+. 2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q2)))*qd1*qd2+. 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q2)))*qd1*qd3+.

2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q2)))*qd1*qd4+. 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q2)))*qd1*qd5+. trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q2)))*qd2*qd2+. 2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q2)))*qd2*qd3+.

2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q2)))*qd2*qd4+. 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q2)))*qd2*qd5+. trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q2)))*qd3*qd3+. 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q2)))*qd3*qd4+.

27 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q2)))*qd3*qd5+. trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q2)))*qd4*qd4+. 2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q2)))*qd4*qd5+. trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q2)))*qd5*qd5+.

trace(diff(diff(T06,q1),q1)*J6*transpose(diff(T06,q2)))*qd1*qd1+. 2*trace(diff(diff(T06,q1),q2)*J6*transpose(diff(T06,q2)))*qd1*qd2+. 2*trace(diff(diff(T06,q1),q3)*J6*transpose(diff(T06,q2)))*qd1*qd3+. 2*trace(diff(diff(T06,q1),q4)*J6*transpose(diff(T06,q2)))*qd1*qd4+.

2*trace(diff(diff(T06,q1),q5)*J6*transpose(diff(T06,q2)))*qd1*qd5+. 2*trace(diff(diff(T06,q1),q6)*J6*transpose(diff(T06,q2)))*qd1*qd6+. trace(diff(diff(T06,q2),q2)*J6*transpose(diff(T06,q2)))*qd2*qd2+. 2*trace(diff(diff(T06,q2),q3)*J6*transpose(diff(T06,q2)))*qd2*qd3+.

2*trace(diff(diff(T06,q2),q4)*J6*transpose(diff(T06,q2)))*qd2*qd4+. 2*trace(diff(diff(T06,q2),q5)*J6*transpose(diff(T06,q2)))*qd2*qd5+. 2*trace(diff(diff(T06,q2),q6)*J6*transpose(diff(T06,q2)))*qd2*qd6+. trace(diff(diff(T06,q3),q3)*J6*transpose(diff(T06,q2)))*qd3*qd3+.

2*trace(diff(diff(T06,q3),q4)*J6*transpose(diff(T06,q2)))*qd3*qd4+. 2*trace(diff(diff(T06,q3),q5)*J6*transpose(diff(T06,q2)))*qd3*qd5+. 2*trace(diff(diff(T06,q3),q6)*J6*transpose(diff(T06,q2)))*qd3*qd6+. trace(diff(diff(T06,q4),q4)*J6*transpose(diff(T06,q2)))*qd4*qd4+.

2*trace(diff(diff(T06,q4),q5)*J6*transpose(diff(T06,q2)))*qd4*qd5+. 2*trace(diff(diff(T06,q4),q6)*J6*transpose(diff(T06,q2)))*qd4*qd6+. trace(diff(diff(T06,q5),q5)*J6*transpose(diff(T06,q2)))*qd5*qd5+. 2*trace(diff(diff(T06,q5),q6)*J6*transpose(diff(T06,q2)))*qd5*qd6+.

trace(diff(diff(T06,q6),q6)*J6*transpose(diff(T06,q2)))*qd6*qd6-. trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q3)))*qd1*qd1+. 2*trace(diff(diff(T03,q1),q2)*J3*transpose(diff(T03,q3)))*qd1*qd2+. 2*trace(diff(diff(T03,q1),q3)*J3*transpose(diff(T03,q3)))*qd1*qd3+.

trace(diff(diff(T03,q2),q2)*J3*transpose(diff(T03,q3)))*qd2*qd1+. 2*trace(diff(diff(T03,q2),q3)*J3*transpose(diff(T03,q3)))*qd2*qd3+. trace(diff(diff(T03,q3),q3)*J3*transpose(diff(T03,q3)))*qd3*qd3+. trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q3)))*qd1*qd1+.

2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q3)))*qd1*qd2+. 2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q3)))*qd1*qd3+. 2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q3)))*qd1*qd4+. trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q3)))*qd2*qd2+.

2*trace(diff(diff(T04,q2),q3)*J4*transpose(diff(T04,q3)))*qd2*qd3+. 2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q3)))*qd2*qd4+. 29 trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q3)))*qd3*qd3+. 2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q3)))*qd3*qd4+.

trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q3)))*qd4*qd4+. trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q3)))*qd1*qd1+. 2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q3)))*qd1*qd2+. 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q3)))*qd1*qd3+.

2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q3)))*qd1*qd4+. 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q3)))*qd1*qd5+. trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q3)))*qd2*qd2+. 2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q3)))*qd2*qd3+.

2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q3)))*qd2*qd4+. 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q3)))*qd2*qd5+. trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q3)))*qd3*qd3+. 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q3)))*qd3*qd4+.

2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q3)))*qd3*qd5+. trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q3)))*qd4*qd4+. 2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q3)))*qd4*qd5+. trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q3)))*qd5*qd5+.

trace(diff(diff(T06,q1),q1)*J6*transpose(diff(T06,q3)))*qd1*qd1+. 2*trace(diff(diff(T06,q1),q2)*J6*transpose(diff(T06,q3)))*qd1*qd2+. 2*trace(diff(diff(T06,q1),q3)*J6*transpose(diff(T06,q3)))*qd1*qd3+. 2*trace(diff(diff(T06,q1),q4)*J6*transpose(diff(T06,q3)))*qd1*qd4+.

2*trace(diff(diff(T06,q1),q5)*J6*transpose(diff(T06,q3)))*qd1*qd5+. 2*trace(diff(diff(T06,q1),q6)*J6*transpose(diff(T06,q3)))*qd1*qd6+. trace(diff(diff(T06,q2),q2)*J6*transpose(diff(T06,q3)))*qd2*qd2+. 2*trace(diff(diff(T06,q2),q3)*J6*transpose(diff(T06,q3)))*qd2*qd3+.

2*trace(diff(diff(T06,q2),q4)*J6*transpose(diff(T06,q3)))*qd2*qd4+. 2*trace(diff(diff(T06,q2),q5)*J6*transpose(diff(T06,q3)))*qd2*qd5+. 2*trace(diff(diff(T06,q2),q6)*J6*transpose(diff(T06,q3)))*qd2*qd6+. trace(diff(diff(T06,q3),q3)*J6*transpose(diff(T06,q3)))*qd3*qd3+.

2*trace(diff(diff(T06,q3),q4)*J6*transpose(diff(T06,q3)))*qd3*qd4+. 30 2*trace(diff(diff(T06,q3),q5)*J6*transpose(diff(T06,q3)))*qd3*qd5+. 2*trace(diff(diff(T06,q3),q6)*J6*transpose(diff(T06,q3)))*qd3*qd6+. trace(diff(diff(T06,q4),q4)*J6*transpose(diff(T06,q3)))*qd4*qd4+.

2*trace(diff(diff(T06,q4),q5)*J6*transpose(diff(T06,q3)))*qd4*qd5+. 2*trace(diff(diff(T06,q4),q6)*J6*transpose(diff(T06,q3)))*qd4*qd6+. trace(diff(diff(T06,q5),q5)*J6*transpose(diff(T06,q3)))*qd5*qd5+. 2*trace(diff(diff(T06,q5),q6)*J6*transpose(diff(T06,q3)))*qd5*qd6+.

trace(diff(diff(T06,q6),q6)*J6*transpose(diff(T06,q3)))*qd6*qd6-. trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q4)))*qd1*qd1+. 2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q4)))*qd1*qd2+. 2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q4)))*qd1*qd3+.

31 2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q4)))*qd1*qd4+. trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q4)))*qd2*qd2+. 2*trace(diff(diff(T04,q2),q3)*J4*transpose(diff(T04,q4)))*qd2*qd3+. 2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q4)))*qd2*qd4+.

trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q4)))*qd3*qd3+. 2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q4)))*qd3*qd4+. trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q4)))*qd4*qd4+. trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q4)))*qd1*qd1+.

2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q4)))*qd1*qd2+. 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q4)))*qd1*qd3+. 2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q4)))*qd1*qd4+. 2*trace(diff(diff(T05,q1),5)*J5*transpose(diff(T05,q4)))*qd1*qd5+.

trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q4)))*qd2*qd2+. 2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q4)))*qd2*qd3+. 2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q4)))*qd2*qd4+. 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q4)))*qd2*qd5+.

trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q4)))*qd3*qd3+. 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q4)))*qd3*qd4+. 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q4)))*qd3*qd5+. trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q4)))*qd4*qd4+.

2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q4)))*qd4*qd5+. trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q4)))*qd5*qd5+. trace(diff(diff(T06,q1),q1)*J6*transpose(diff(T06,q4)))*qd1*qd1+. 2*trace(diff(diff(T06,q1),q2)*J6*transpose(diff(T06,q4)))*qd1*qd2+.

2*trace(diff(diff(T06,q1),q3)*J6*transpose(diff(T06,q4)))*qd1*qd3+. 2*trace(diff(diff(T06,q1),q4)*J6*transpose(diff(T06,q4)))*qd1*qd4+. 2*trace(diff(diff(T06,q1),q5)*J6*transpose(diff(T06,q4)))*qd1*qd5+. 2*trace(diff(diff(T06,q1),q6)*J6*transpose(diff(T06,q4)))*qd1*qd6+.

trace(diff(diff(T06,q2),q2)*J6*transpose(diff(T06,q4)))*qd2*qd2+. 2*trace(diff(diff(T06,q2),q3)*J6*transpose(diff(T06,q4)))*qd2*qd3+. 2*trace(diff(diff(T06,q2),q4)*J6*transpose(diff(T06,q4)))*qd2*qd4+. 32 2*trace(diff(diff(T06,q2),q5)*J6*transpose(diff(T06,q4)))*qd2*qd5+.

2*trace(diff(diff(T06,q2),q6)*J6*transpose(diff(T06,q4)))*qd2*qd6+. trace(diff(diff(T06,q3),q3)*J6*transpose(diff(T06,q4)))*qd3*qd3+. 2*trace(diff(diff(T06,q3),q4)*J6*transpose(diff(T06,q4)))*qd3*qd4+. 2*trace(diff(diff(T06,q3),q5)*J6*transpose(diff(T06,q4)))*qd3*qd5+.

2*trace(diff(diff(T06,q3),q6)*J6*transpose(diff(T06,q4)))*qd3*qd6+. trace(diff(diff(T06,q4),q4)*J6*transpose(diff(T06,q4)))*qd4*qd4+. 2*trace(diff(diff(T06,q4),q5)*J6*transpose(diff(T06,q4)))*qd4*qd5+. 2*trace(diff(diff(T06,q4),q6)*J6*transpose(diff(T06,q4)))*qd4*qd6+.

trace(diff(diff(T06,q5),q5)*J6*transpose(diff(T06,q4)))*qd5*qd5+. 2*trace(diff(diff(T06,q5),q6)*J6*transpose(diff(T06,q4)))*qd5*qd6+. trace(diff(diff(T06,q6),q6)*J6*transpose(diff(T06,q4)))*qd6*qd6-. trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q5)))*qd1*qd1+.

2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q5)))*qd1*qd2+. 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q5)))*qd1*qd3+. 2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q5)))*qd1*qd4+. 33 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q5)))*qd1*qd5+.

trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q5)))*qd2*qd2+. 2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q5)))*qd2*qd3+. 2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q5)))*qd2*qd4+. 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q5)))*qd2*qd5+.

trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q5)))*qd3*qd3+. 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q5)))*qd3*qd4+. 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q5)))*qd3*qd5+. trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q5)))*qd4*qd4+.

2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q5)))*qd4*qd5+. trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q5)))*qd5*qd5+. trace(diff(diff(T06,q1),q1)*J6*transpose(diff(T06,q5)))*qd1*qd1+. 2*trace(diff(diff(T06,q1),q2)*J6*transpose(diff(T06,q5)))*qd1*qd2+.

2*trace(diff(diff(T06,q1),q3)*J6*transpose(diff(T06,q5)))*qd1*qd3+. 2*trace(diff(diff(T06,q1),q4)*J6*transpose(diff(T06,q5)))*qd1*qd4+. 2*trace(diff(diff(T06,q1),q5)*J6*transpose(diff(T06,q5)))*qd1*qd5+. 2*trace(diff(diff(T06,q1),q6)*J6*transpose(diff(T06,q4)))*qd1*qd6+.

trace(diff(diff(T06,q2),q2)*J6*transpose(diff(T06,q5)))*qd2*qd2+. 2*trace(diff(diff(T06,q2),q3)*J6*transpose(diff(T06,q5)))*qd2*qd3+. 2*trace(diff(diff(T06,q2),q4)*J6*transpose(diff(T06,q5)))*qd2*qd4+. 2*trace(diff(diff(T06,q2),q5)*J6*transpose(diff(T06,q5)))*qd2*qd5+.

2*trace(diff(diff(T06,q2),q6)*J6*transpose(diff(T06,q5)))*qd2*qd6+. trace(diff(diff(T06,q3),q3)*J6*transpose(diff(T06,q5)))*qd3*qd3+. 2*trace(diff(diff(T06,q3),q4)*J6*transpose(diff(T06,q5)))*qd3*qd4+. 2*trace(diff(diff(T06,q3),q5)*J6*transpose(diff(T06,q5)))*qd3*qd5+.

2*trace(diff(diff(T06,q3),q6)*J6*transpose(diff(T06,q5)))*qd3*qd6+. trace(diff(diff(T06,q4),q4)*J6*transpose(diff(T06,q5)))*qd4*qd4+. 2*trace(diff(diff(T06,q4),q5)*J6*transpose(diff(T06,q5)))*qd4*qd5+. 2*trace(diff(diff(T06,q4),q6)*J6*transpose(diff(T06,q5)))*qd4*qd6+.

trace(diff(diff(T06,q5),q5)*J6*transpose(diff(T06,q5)))*qd5*qd5+. 2*trace(diff(diff(T06,q5),q6)*J6*transpose(diff(T06,q5)))*qd5*qd6+. 34 trace(diff(diff(T06,q6),q6)*J6*transpose(diff(T06,q5)))*qd6*qd6-. trace(diff(diff(T06,q1),q1)*J6*transpose(diff(T06,q6)))*qd1*qd1+.

2*trace(diff(diff(T06,q1),q2)*J6*transpose(diff(T06,q6)))*qd1*qd2+. 2*trace(diff(diff(T06,q1),q3)*J6*transpose(diff(T06,q6)))*qd1*qd3+. 2*trace(diff(diff(T06,q1),q4)*J6*transpose(diff(T06,q6)))*qd1*qd4+. 2*trace(diff(diff(T06,q1),q5)*J6*transpose(diff(T06,q6)))*qd1*qd5+.

2*trace(diff(diff(T06,q1),q6)*J6*transpose(diff(T06,q6)))*qd1*qd6+. trace(diff(diff(T06,q2),q2)*J6*transpose(diff(T06,q6)))*qd2*qd2+. 2*trace(diff(diff(T06,q2),q3)*J6*transpose(diff(T06,q6)))*qd2*qd3+. 2*trace(diff(diff(T06,q2),q4)*J6*transpose(diff(T06,q6)))*qd2*qd4+.

2*trace(diff(diff(T06,q2),q5)*J6*transpose(diff(T06,q6)))*qd2*qd5+. 2*trace(diff(diff(T06,q2),q6)*J6*transpose(diff(T06,q6)))*qd2*qd6+. trace(diff(diff(T06,q3),q3)*J6*transpose(diff(T06,q6)))*qd3*qd3+. 2*trace(diff(diff(T06,q3),q4)*J6*transpose(diff(T06,q6)))*qd3*qd4+.

2*trace(diff(diff(T06,q3),q5)*J6*transpose(diff(T06,q6)))*qd3*qd5+. 2*trace(diff(diff(T06,q3),q6)*J6*transpose(diff(T06,q6)))*qd3*qd6+. trace(diff(diff(T06,q4),q4)*J6*transpose(diff(T06,q6)))*qd4*qd4+. 2*trace(diff(diff(T06,q4),q5)*J6*transpose(diff(T06,q6)))*qd4*qd5+.

2*trace(diff(diff(T06,q4),q6)*J6*transpose(diff(T06,q6)))*qd4*qd6+. trace(diff(diff(T06,q5),q5)*J6*transpose(diff(T06,q6)))*qd5*qd5+. 2*trace(diff(diff(T06,q5),q6)*J6*transpose(diff(T06,q6)))*qd5*qd6+. trace(diff(diff(T06,q6),q6)*J6*transpose(diff(T06,q6)))*qd6*qd6-.