BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG HỌC VIỆN KỸ THUẬT QUÂN SỰ BÙI VĂN TRẦM NGHIÊN CỨU ĐỘNG LỰC HỌC THIẾT BỊ KHOAN XOAY ĐẬP LẮP TRÊN MÁY ĐÀO PHỤC VỤ THI CÔNG HẦM KHẨU ĐỘ VỪA VÀ NHỎ LUẬN ÁN TIẾN SĨ KỸ THUẬT HÀ NỘI - NĂM 2019 luan an BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG HỌC VIỆN KỸ THUẬT QUÂN SỰ BÙI VĂN TRẦM NGHIÊN CỨU ĐỘNG LỰC HỌC THIẾT BỊ KHOAN XOAY ĐẬP LẮP TRÊN MÁY ĐÀO PHỤC VỤ THI CÔNG HẦM KHẨU ĐỘ VỪA VÀ NHỎ Chuyên ngành : Kỹ thuật Cơ khí Động lực Mã số : 52 01 16 LUẬN ÁN TIẾN SĨ KỸ THUẬT NGƯỜI HƯỚNG DẪN KHOA HỌC: GS TS Chu Văn Đạt TS Nguyễn Lâm Khánh HÀ NỘI - NĂM 2019 luan an i LỜI CAM ĐOAN Tôi xin cam đoan Luận án cơng trình nghiên cứu Các số liệu, kết luận án trung thực chưa công bố cơng trình khác./ NCS Bùi Văn Trầm luan an ii LỜI CẢM ƠN Tôi xin trân trọng cảm ơn NGƯT.GS.TS Chu Văn Đạt TS Nguyễn Lâm Khánh tận tình hướng dẫn tơi hồn thành luận án Trân trọng cảm ơn Thầy cô giáo Bộ môn Xe máy Công binh - Học viện Kỹ thuật Quân sự; Thầy cô giáo Bộ môn Máy xây dựng & Xếp dỡ - Trường Đại học Giao thông vận tải; đồng nghiệp Bộ môn Cơ học ứng dụng - Trường Đại học Bách khoa Hà Nội; Thầy cô giáo, nhà khoa học Học viện Kỹ thuật Quân sự, Trường Đại học Giao thông vận tải, Trường Đại học Xây dựng, Học viện Nông nghiệp Việt Nam, Trường Đại học Bách khoa Hà Nội, đồng nghiệp Trường Đại học Công nghệ GTVT, … góp ý, giúp đỡ tơi trình thực luận án Xin trân trọng cảm ơn Lãnh đạo Trường Đại học Công nghệ Giao thông vận tải, lãnh đạo đơn vị chức Học viện Kỹ thuật Quân (Phòng Đào tạo Sau đại học, Khoa Động lực) tạo điều kiện, giúp đỡ tơi q trình nghiên cứu Trân trọng cảm ơn Lữ đoàn 131 Hải quân; trân trọng cảm ơn Ban lãnh đạo nhà máy Z49; trân trọng cảm ơn cán phịng thí nghiệm Bộ mơn Xe máy Công binh - Học viện KTQS, cảm ơn cán phịng thí nghiệm chun ngành xây dựng giao thơng LAS-XD72 tận tình hỗ trợ giúp đỡ tơi q trình thực luận án Đặc biệt, Tơi trân trọng cảm ơn người thân gia đình bạn bè, đồng nghiệp Trường Đại học Công nghệ GTVT ln động viên, hỗ trợ tơi hồn thành luận án Trân trọng cảm ơn./ NCS Bùi Văn Trầm luan an iii MỤC LỤC Trang LỜI CAM ĐOAN i LỜI CẢM ƠN ii MỤC LỤC iii DANH MỤC CÁC KÝ HIỆU vii DANH MỤC CÁC CHỮ VIẾT TẮT xi DANH MỤC CÁC BẢNG BIỂU xii DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ xiii MỞ ĐẦU Chương TỔNG QUAN VẤN ĐỀ NGHIÊN CỨU 1.1 Đường hầm đường hầm quân 1.1.1 Công trình ngầm đường hầm 1.1.2 Đường hầm quân hộ chiếu gương khoan 1.2 Quy trình thi cơng đường hầm phương pháp khoan nổ 1.3 Đặc điểm môi trường đất đá đường hầm quân 1.4 Đặc điểm thiết bị khoan lỗ nổ mìn thi công đường hầm quân 11 1.4.1 Điều kiện thi công đường hầm quân 11 1.4.2 Thiết bị khoan xoay - đập 12 1.5 Các yếu tố ảnh hưởng đến độ xác lỗ khoan 14 1.5.1 Tiêu chuẩn áp dụng khoan nổ mìn 14 1.5.2 Các yếu tố ảnh hưởng đến độ xác lỗ khoan 15 1.6 Tình hình nghiên cứu hệ giá đỡ thiết bị khoan lỗ nổ mìn 16 1.6.1 Tương tác mũi khoan với đá 16 1.6.2 Động học động lực học hệ liên quan 21 1.7 Mục tiêu nhiệm vụ luận án 33 luan an iv 1.7.1 Mục tiêu luận án 33 1.7.2 Nhiệm vụ luận án 33 KẾT LUẬN CHƯƠNG 34 Chương XÁC ĐỊNH KHÔNG GIAN LÀM VIỆC CỦA MÁY KHOAN MK-Z49 VÀ MÔ PHỎNG CHUYỂN ĐỘNG 36 2.1 Cơ sở lý thuyết 36 2.1.1 Phương pháp ma trận Denavit - Hartenberg 36 2.1.2 Phương trình xác định vị trí hướng khâu thao tác 37 2.2 Khảo sát không gian làm việc máy khoan nổ mìn MK-Z49 39 2.2.1 Kết cấu nguyên lý hệ giá đỡ thiết bị khoan MK-Z49 39 2.2.2 Động học thuận hệ giá đỡ máy khoan MK-Z49 41 2.2.3 Không gian thao tác 50 2.3 Nghiên cứu điều chỉnh hệ giá đỡ máy khoan MK-Z49 đáp ứng yêu cầu thi công 53 2.3.1 Thiết lập phương trình động học 54 2.3.2 Giải toán động học ngược 54 2.4 Mô hoạt động máy khoan MK-Z49 sau điều chỉnh kết cấu 63 2.4.1 Xây dựng thư viện mơ hình chi tiết máy phận máy 63 2.4.2 Khoan vị trí điển hình 64 KẾT LUẬN CHƯƠNG 66 Chương ĐỘNG LỰC HỌC HỆ GIÁ ĐỠ THIẾT BỊ KHOAN 67 3.1 Cơ sở lý thuyết 67 3.1.1 Lý sử dụng phương trình Lagrange dạng nhân tử 67 3.1.2 Phương pháp giải phương trình Lagrange dạng nhân tử 68 3.2 Mơ hình động lực học hệ giá đỡ thiết bị khoan 69 3.2.1 Các giả thiết xây dựng mô hình 69 luan an v 3.2.2 Mơ hình khảo sát động lực học hệ 69 3.2.3 Hệ phương trình vi phân chuyển động hệ khảo sát 74 3.3 Lực suy rộng lực hoạt động 78 3.3.1 Các lực hoạt động 78 3.3.2 Xác định lực hoạt động 79 3.3.3 Xác định véc tơ lực suy rộng 89 3.3.4 Hành trình áp suất khoang xy lanh 95 3.4 Mô số 100 3.4.1 Số liệu đầu vào 100 3.4.2 Kết mô số 104 3.5 Sai lệch lỗ khoan 108 3.6 Khảo sát ảnh hưởng số thông số đặc trưng đến rung động giá dẫn hướng thiết bị khoan 111 3.6.1 Ảnh hưởng đường kính xi lanh thủy lực dẫn động 111 3.6.2 Thay đổi tính đối tượng khoan 116 KẾT LUẬN CHƯƠNG 120 Chương NGHIÊN CỨU THỰC NGHIỆM 121 4.1 Mục đích nội dung nghiên cứu 121 4.1.1 Mục đích nghiên cứu 121 4.1.2 Nội dung thực nghiệm 121 4.2 Các thông số đo mục đích sử dụng 121 4.3 Phương tiện thiết bị thực nghiệm 122 4.3.1 Phương tiện thiết bị thực nghiệm 122 4.3.2 Các thiết bị đo, thiết bị nối kết thực địa 123 4.3.3 Thiết bị phịng thí nghiệm 124 4.4 Tổ chức trình thực nghiệm 125 4.4.1 Các bước chuẩn bị thực nghiệm 125 luan an vi 4.4.2 Bố trí đầu đo thiết bị đo 125 4.4.3 Xác định thơng số q tình thực nghiệm 129 4.4.4 Phương pháp xử lý kết thực nghiệm 131 4.5 Kết thực nghiệm 132 4.5.1 Kết thu phịng thí nghiệm 132 4.5.2 Kết thu trường 134 4.6 Đánh giá kết lí thuyết thực nghiệm 134 KẾT LUẬN CHUNG 139 DANH MỤC CÁC CƠNG TRÌNH ĐÃ CƠNG BỐ 141 TÀI LIỆU THAM KHẢO 142 luan an vii DANH MỤC CÁC KÝ HIỆU Ký Tên gọi hiệu Đơn vị h Góc nghiêng đường tâm lỗ với trục hầm n Độ bền nén đất đá theo trục fkp Hệ số độ cứng (độ kiên cố đất đá) Góc nội ma sát Rn Cường độ kháng nén trục cđ Độ cứng đá kđ Độ cản nhớt đá N.s/m G Mô đun đàn hồi trượt N/m2 r Bán kính vùng tiếp xúc thực tế lực ấn vào đá m Hàm dịch chuyển Reissner - a0 Tần suất lực tác động vào đá - Khối lượng riêng đá d Đường kính mũi khoan Lk Chiều sâu lỗ khoan m x Sai lệch lỗ khoan cm Poc Lực dọc trục N Pyđ Năng lượng đập J MB Mô men xoắn B Tốc độ góc đầu khoan F(t) Lực tác dụng vào mũi khoan N D Ngưỡng phá hủy đá - I Xung va đập tức thời búa khoan - f1 , f2 độ kG/cm2 độ kG/cm2 N/m Kg/m3 mm N.m Rad/s Mc Mô men cản đầu mũi khoan N.m Mtđ Mô men ma sát thành mũi khoan với thành đá N.m Mđđ Mô men ma sát đá với đá N.m Mkđ Mô men cản cắt mũi khoan với đá N.m luan an viii Ký Tên gọi hiệu Đơn vị 1 Hệ số ma sát bề mặt mũi khoan với đá db Đường kính bi gắn đầu mũi khoan mm h1 Chiều sâu bi ngập vào đá mm rtb Khoảng cách trung bình đến tâm mũi khoan mm σc Ứng suất cắt đá kPa z Số lượng bi đầu mũi khoan viên Pk Lực đập dọc trục mũi khoan N Qk Lực cắt mũi khoan N M Mô men quay mũi khoan có hai lưỡi cắt R Khoảng cách từ lực cắt Qk đến tâm quay mũi khoan m At Công đập phút J h Chiều sâu phá hủy đá m n Độ bền nén đất đá N/m2 d Góc sắc đầu mũi dao Độ fmx Hệ số ma sát thép đất đá n cd(td) Tần số đập - N.m Lần/phút Độ cứng dầu thủy lực tương dương khoang xi lanh N/m i Góc quay quanh trục zi-1 để trục xi-1 chuyển đến trục xi’ (xi’// xi) Độ di Dịch chuyển tịnh tiến dọc theo trục zi-1 để gốc tọa độ Oi-1 chuyển đến Oi’ m Dịch chuyển tịnh tiến dọc theo trục xi để điểm Oi’ chuyển đến điểm Oi m i Góc quay quanh trục xi cho trục z’i-1 chuyển đến trục zi Độ q1 Góc quay khâu đế quanh điểm liên kết O1 Độ q2 Góc quay cần quanh điểm liên kết O2 Độ q3 Hành trình tịnh tiến khâu dọc khâu mm cd(A), cd(B) luan an 219 (yC6 * m6 * a5 - zC5 * (a5 + xC5) * m5 - xC4 * m4 * d4 + xC4 * (yC4 q3) * m4 - I4xy + I5xz) * dq1 * cos(q2) + 0.2e1 * (-d5 * m6 + (yC5 d5) * m5 - zC4 * m4) * dq3) * cos(q4) / 0.2e1 + sin(q4) * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * dq5 * cos(q6) ^ + (0.2e1 * dq5 * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q6) + (dq6 * yC6 * zC6 + 0.2e1 * dq5 * a5 * (a6 + xC6)) * m6 - dq6 * I6yz) * cos(q6) + ((0.2e1 * dq5 * a5 * zC6 + dq6 * yC6 * (a6 + xC6)) * m6 - dq6 * I6xy) * sin(q6) + dq5 * ((a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + (a5 + xC5 + zC5) * (a5 + xC5 - zC5) * m5 - I5xx + I5zz + I6yy - I6zz)) * cos(q5) ^ + ((0.4e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * dq6 * sin(q5) * cos(q6) ^ + ((-0.2e1 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * dq6 * sin(q6) + (-0.2e1 * dq6 * a5 * zC6 + 0.4e1 * dq5 * yC6 * (a6 + xC6)) * m6 - 0.4e1 * I6xy * dq5) * sin(q5) + 0.2e1 * dq1 * (m6 * yC6 * zC6 - I6yz) * cos(q2) - 0.2e1 * dq3 * m6 * (a6 + xC6)) * cos(q6) + (((0.4e1 * dq5 * yC6 * zC6 - 0.2e1 * dq6 * a5 * (a6 + xC6)) * m6 + 0.4e1 * I6yz * dq5) * sin(q6) + (0.2e1 * dq6 * (a6 + xC6) * zC6 + 0.4e1 * dq5 * a5 * yC6) * m6 - 0.4e1 * dq5 * zC5 * (a5 + xC5) * m5 + 0.4e1 * I5xz * dq5 - 0.2e1 * I6xz * dq6) * sin(q5) + (0.2e1 * (yC6 * (a6 + xC6) * m6 I6xy) * dq1 * cos(q2) + 0.2e1 * dq3 * zC6 * m6) * sin(q6) - 0.2e1 * (yC6 * d5 * m6 + zC5 * (yC5 - d5) * m5 - I5yz) * dq1 * cos(q2) - 0.2e1 * dq3 * (a5 * m6 + m5 * (a5 + xC5))) * sin(q4) + 0.2e1 * ((0.2e1 * zC6 * (a6 + xC6) * m6 - (2 * I6xz)) * cos(q6) ^ + (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + (a5 * zC6 - d5 * (a6 + xC6)) * m6) * cos(q6) + m6 * (d5 * zC6 + a5 * (a6 + xC6)) * sin(q6) + ((-a6 - xC6) * zC6 - a5 * d5) * m6 + (yC5 - d5) * (a5 + xC5) * m5 I5xy + I6xz) * dq2) * cos(q5) / 0.2e1 + (-0.4e1 * cos(q2) * (zC6 * (a6 + xC6) * m6 - I6xz) * dq1 * sin(q5) * cos(q6) ^ + (-0.2e1 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + (a5 * zC6 - d5 * (a6 + xC6)) * m6) * cos(q2) * dq1 * sin(q5) + 0.2e1 * dq1 * zC6 * m6 * (q3 + d4) * cos(q2) - 0.2e1 * dq6 * (m6 * yC6 * zC6 - I6yz)) * cos(q6) + (-0.2e1 * (d5 * zC6 + a5 * (a6 + xC6)) * cos(q2) * dq1 * m6 * sin(q6) - 0.2e1 * dq1 * (((-a6 - xC6) * zC6 - a5 * d5) * m6 + (yC5 d5) * (a5 + xC5) * m5 - I5xy + I6xz) * cos(q2) - 0.2e1 * dq3 * (-m5 * zC5 + m6 * yC6)) * sin(q5) + (0.2e1 * dq1 * m6 * (q3 + d4) * (a6 + xC6) * cos(q2) - 0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq6) * sin(q6) + 0.2e1 * (-d5 * (q3 + d4) * m6 + (yC5 - d5) * (q3 + d4) * m5 - zC4 * m4 * d4 + zC4 * (yC4 - q3) * m4 - I4yz) * dq1 * cos(q2) + 0.2e1 * yC6 ^ * m6 * dq5 + 0.2e1 * zC5 ^ * m5 * dq5 + (I5xx + I5yy - I5zz + I6xx I6yy + I6zz) * dq5 - 0.2e1 * xC4 * m4 * dq3) * sin(q4) / 0.2e1 + dq1 * sin(q2) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + (0.2e1 * dq2 * (m6 * yC6 * zC6 - I6yz) * sin(q5) - 0.4e1 * dq1 * ((zC6 * (a6 + xC6) * m6 - I6xz) * sin(q6) - zC6 * d5 * m6) * sin(q2)) * cos(q6) / 0.2e1 + ((yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) - yC6 * d5 * m6 - zC5 * (yC5 - d5) * m5 + I5yz) * dq2 * sin(q5) + 0.2e1 * dq1 * d5 * sin(q2) * m6 * (a6 + xC6) * sin(q6) - ((a6 ^ + 0.2e1 * a6 * xC6 + d5 ^ + xC6 ^ 2) * m6 + (yC5 - d5) ^ * m5 + zC4 ^ * m4 + I4xx / 0.2e1 + I4yy / 0.2e1 - I4zz / 0.2e1 + I5xx / 0.2e1 - I5yy / 0.2e1 + I5zz / 0.2e1 - I6xx / 0.2e1 + I6yy / 0.2e1 + I6zz / 0.2e1) * dq1 * sin(q2) - dq2 * (zC4 * xC4 * m4 - I4xz); c43 = (dq1 * ((m6 * (a6 + xC6) * cos(q6) - zC6 * sin(q6) * m6 + a5 m6 + m5 * (a5 + xC5)) * cos(q5) + (-m5 * zC5 + m6 * yC6) * sin(q5) xC4 * m4) * sin(q2) + dq2 * (zC6 * m6 * cos(q6) + m6 * (a6 + xC6) sin(q6) - d5 * m6 + (yC5 - d5) * m5 - zC4 * m4)) * cos(q4) - sin(q4) (-dq1 * (zC6 * m6 * cos(q6) + m6 * (a6 + xC6) * sin(q6) - d5 * m6 (yC5 - d5) * m5 - zC4 * m4) * sin(q2) + dq2 * ((m6 * (a6 + xC6) cos(q6) - zC6 * sin(q6) * m6 + a5 * m6 + m5 * (a5 + xC5)) * cos(q5) (-m5 * zC5 + m6 * yC6) * sin(q5) + xC4 * m4)); luan an * + * * + * + 220 c44 = (-0.2e1 * dq6 * (zC6 * (a6 + xC6) * m6 - I6xz) * cos(q6) ^ + (-((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * dq6 * sin(q6) + (0.2e1 * dq5 * yC6 * (a6 + xC6) - dq6 * a5 * zC6) * m6 0.2e1 * I6xy * dq5) * cos(q6) + ((-0.2e1 * dq5 * yC6 * zC6 - dq6 * a5 * (a6 + xC6)) * m6 + 0.2e1 * I6yz * dq5) * sin(q6) + (0.2e1 * dq5 * a5 * yC6 + dq6 * (a6 + xC6) * zC6) * m6 + (-0.2e1 * a5 * m5 * zC5 - 0.2e1 * m5 * xC5 * zC5 + (2 * I5xz)) * dq5 - I6xz * dq6) * cos(q5) ^ - (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * dq5 * cos(q6) ^ + (-0.2e1 * dq5 * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q6) + (dq6 * yC6 * zC6 + 0.2e1 * dq5 * a5 * (a6 + xC6)) * m6 - dq6 * I6yz) * cos(q6) + ((-0.2e1 * dq5 * a5 * zC6 + dq6 * yC6 * (a6 + xC6)) * m6 - dq6 * I6xy) * sin(q6) + dq5 * ((a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + a5 ^ * m5 + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy I6zz)) * sin(q5) * cos(q5) + 0.2e1 * dq6 * (zC6 * (a6 + xC6) * m6 I6xz) * cos(q6) ^ + (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * dq6 * sin(q6) - (a6 + xC6) * (d5 * dq6 + dq5 * yC6) * m6 + I6xy * dq5) * cos(q6) + (zC6 * (d5 * dq6 + dq5 * yC6) * m6 - I6yz * dq5) * sin(q6) + (-dq5 * a5 * yC6 - dq6 * (a6 + xC6) * zC6) * m6 + (a5 * m5 * zC5 + m5 * xC5 * zC5 - I5xz) * dq5 + I6xz * dq6; c45 = (-0.2e1 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * (dq1 * cos(q4) * sin(q2) - dq2 * sin(q4)) * cos(q6) ^ + (0.4e1 * dq1 * ((-zC6 * (a6 + xC6) * m6 + I6xz) * sin(q6) + a5 * m6 * (a6 + xC6)) * sin(q2) * cos(q4) + 0.4e1 * dq2 * ((-zC6 * (a6 + xC6) * m6 + I6xz) * sin(q6) + a5 * m6 * (a6 + xC6)) * sin(q4) + 0.4e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * (dq1 * cos(q2) + dq4)) * cos(q6) - 0.2e1 * dq1 * (-0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + a5 ^ * m5 + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 I5xx + I5zz + I6yy - I6zz) * sin(q2) * cos(q4) + 0.2e1 * dq2 * (-0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + a5 ^ * m5 + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz) * sin(q4) + 0.4e1 * ((-m6 * yC6 * zC6 + I6yz) * sin(q6) + yC6 * m6 * a5 - a5 * m5 * zC5 - m5 * xC5 * zC5 + I5xz) * (dq1 * cos(q2) + dq4)) * cos(q5) ^ / 0.2e1 + ((-0.2e1 * ((xC6 + zC6 + a6) * (xC6 zC6 + a6) * m6 - I6xx + I6zz) * (dq1 * cos(q2) + dq4) * sin(q5) - 0.4e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * (dq1 * sin(q2) * sin(q4) + dq2 * cos(q4) + dq5)) * cos(q6) ^ + ((-0.4e1 * (yC6 * (a6 + xC6) * m6 I6xy) * dq1 * sin(q2) * cos(q4) + 0.4e1 * (yC6 * (a6 + xC6) * m6 I6xy) * dq2 * sin(q4) - 0.4e1 * ((-zC6 * (a6 + xC6) * m6 + I6xz) * sin(q6) + a5 * m6 * (a6 + xC6)) * (dq1 * cos(q2) + dq4)) * sin(q5) 0.2e1 * (dq1 * sin(q2) * sin(q4) + dq2 * cos(q4) + dq5) * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + (a5 * zC6 - d5 * (a6 + xC6)) * m6)) * cos(q6) + (-0.4e1 * dq1 * ((-m6 * yC6 * zC6 + I6yz) * sin(q6) + yC6 * m6 * a5 - a5 * m5 * zC5 - m5 * xC5 * zC5 + I5xz) * sin(q2) * cos(q4) + 0.4e1 * dq2 * ((-m6 * yC6 * zC6 + I6yz) * sin(q6) + yC6 * m6 * a5 - a5 * m5 * zC5 - m5 * xC5 * zC5 + I5xz) * sin(q4) - 0.2e1 * (-0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + a5 ^ * m5 + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz) * (dq1 * cos(q2) + dq4)) * sin(q5) - 0.2e1 * (dq1 * sin(q2) * sin(q4) + dq2 * cos(q4) + dq5) * (m6 * (d5 * zC6 + a5 * (a6 + xC6)) * sin(q6) + ((-a6 - xC6) * zC6 - a5 * d5) * m6 + m5 * (yC5 - d5) * a5 + xC5 * (yC5 - d5) * m5 - I5xy + I6xz)) * cos(q5) / 0.2e1 - ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * dq6 * sin(q5) * cos(q6) ^ + ((-0.2e1 * dq2 * (m6 * yC6 * zC6 - I6yz) * cos(q4) - 0.2e1 * dq1 * sin(q4) * (m6 * yC6 * zC6 - I6yz) * sin(q2) + 0.4e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * dq6 * sin(q6) 0.2e1 * zC6 * (d5 * dq6 + dq5 * yC6) * m6 + 0.2e1 * I6yz * dq5) * luan an 221 sin(q5) - 0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * (dq1 * cos(q2) + dq4)) * cos(q6) / 0.2e1 + (-0.2e1 * ((yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) - yC6 * d5 * m6 - zC5 * (yC5 - d5) * m5 + I5yz) * dq2 * cos(q4) - 0.2e1 * dq1 * ((yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) - yC6 * d5 * m6 - zC5 * (yC5 - d5) * m5 + I5yz) * sin(q4) * sin(q2) + (-0.2e1 * (a6 + xC6) * (d5 * dq6 + dq5 * yC6) * m6 + 0.2e1 * I6xy * dq5) * sin(q6) + (0.2e1 * a6 ^ * dq6 + 0.4e1 * a6 * dq6 * xC6 + 0.2e1 * d5 * dq5 * yC6 + 0.2e1 * dq6 * xC6 ^ 2) * m6 + 0.2e1 * dq5 * zC5 * (yC5 - d5) * m5 0.2e1 * dq5 * I5yz - dq6 * (I6xx - I6yy - I6zz)) * sin(q5) / 0.2e1 dq1 * (yC6 ^ * m6 + zC5 ^ * m5 + I5xx / 0.2e1 + I5yy / 0.2e1 - I5zz / 0.2e1 + I6xx / 0.2e1 - I6yy / 0.2e1 + I6zz / 0.2e1) * sin(q2) * cos(q4) + (yC6 ^ * m6 + zC5 ^ * m5 + I5xx / 0.2e1 + I5yy / 0.2e1 I5zz / 0.2e1 + I6xx / 0.2e1 - I6yy / 0.2e1 + I6zz / 0.2e1) * dq2 * sin(q4) - ((-m6 * yC6 * zC6 + I6yz) * sin(q6) + yC6 * m6 * a5 - a5 * m5 * zC5 - m5 * xC5 * zC5 + I5xz) * (dq1 * cos(q2) + dq4); c46 = ((-0.4e1 * (-dq1 * cos(q4) * sin(q2) + dq2 * sin(q4)) * (zC6 * (a6 + xC6) * m6 - I6xz) * cos(q5) - 0.2e1 * ((xC6 + zC6 + a6) * (xC6 zC6 + a6) * m6 - I6xx + I6zz) * (dq1 * sin(q2) * sin(q4) + dq2 * cos(q4) + dq5)) * sin(q5) - 0.4e1 * (cos(q5) - 0.1e1) * (dq1 * cos(q2) + dq4) * (cos(q5) + 0.1e1) * (zC6 * (a6 + xC6) * m6 - I6xz)) * cos(q6) ^ / 0.2e1 + (((-0.2e1 * (-dq1 * cos(q4) * sin(q2) + dq2 * sin(q4)) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + 0.2e1 * a5 * zC6 * cos(q4) * sin(q2) * m6 * dq1 - 0.2e1 * a5 * dq2 * zC6 * sin(q4) * m6 - 0.2e1 * (m6 * yC6 * zC6 - I6yz) * (dq1 * cos(q2) + dq4)) * cos(q5) + 0.4e1 * (dq1 * sin(q2) * sin(q4) + dq2 * cos(q4) + dq5) * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q6) - 0.2e1 * dq2 * zC6 * cos(q4) * d5 * m6 - 0.2e1 * zC6 * sin(q2) * sin(q4) * d5 * m6 * dq1 0.2e1 * zC6 * (d5 * dq5 - dq6 * yC6) * m6 - 0.2e1 * dq6 * I6yz) * sin(q5) + (-0.2e1 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * (dq1 * cos(q2) + dq4) * sin(q6) - 0.2e1 * sin(q2) * dq1 * (m6 * yC6 * zC6 - I6yz) * cos(q4) + 0.2e1 * dq2 * (m6 * yC6 * zC6 - I6yz) * sin(q4) - 0.2e1 * a5 * zC6 * m6 * (dq1 * cos(q2) + dq4)) * cos(q5) ^ + 0.2e1 * dq6 * m6 * (a5 * zC6 + d5 * a6 + d5 * xC6) * cos(q5) + 0.2e1 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * (dq1 * cos(q2) + dq4) * sin(q6) + 0.2e1 * sin(q2) * dq1 * (m6 * yC6 * zC6 I6yz) * cos(q4) - 0.2e1 * dq2 * (m6 * yC6 * zC6 - I6yz) * sin(q4) 0.2e1 * d5 * m6 * (a6 + xC6) * (dq1 * cos(q2) + dq4)) * cos(q6) / 0.2e1 + (((0.2e1 * a5 * sin(q2) * m6 * dq1 * (a6 + xC6) * cos(q4) - 0.2e1 * dq2 * m6 * a5 * (a6 + xC6) * sin(q4) - 0.2e1 * (yC6 * (a6 + xC6) * m6 I6xy) * (dq1 * cos(q2) + dq4)) * sin(q6) + 0.2e1 * (-dq1 * cos(q4) * sin(q2) + dq2 * sin(q4)) * (zC6 * (a6 + xC6) * m6 - I6xz)) * cos(q5) + (-0.2e1 * dq2 * d5 * m6 * (a6 + xC6) * cos(q4) - 0.2e1 * sin(q4) * d5 * m6 * dq1 * (a6 + xC6) * sin(q2) - 0.2e1 * (a6 + xC6) * (d5 * dq5 - dq6 * yC6) * m6 - 0.2e1 * dq6 * I6xy) * sin(q6) + 0.2e1 * (dq1 * sin(q2) * sin(q4) + dq2 * cos(q4) + dq5) * ((a6 + xC6) ^ * m6 - I6xx / 0.2e1 + I6yy / 0.2e1 + I6zz / 0.2e1)) * sin(q5) / 0.2e1 + ((-0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq1 * sin(q2) * cos(q4) + 0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq2 * sin(q4) - 0.2e1 * a5 * m6 * (a6 + xC6) * (dq1 * cos(q2) + dq4)) * sin(q6) + 0.2e1 * (dq1 * cos(q2) + dq4) * (zC6 * (a6 + xC6) * m6 - I6xz)) * cos(q5) ^ / 0.2e1 + dq6 * sin(q6) * m6 * (a5 * a6 + a5 * xC6 - d5 * zC6) * cos(q5) + (0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq1 * sin(q2) * cos(q4) - 0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq2 * sin(q4) + 0.2e1 * zC6 * d5 * m6 * (dq1 * cos(q2) + dq4)) * sin(q6) / 0.2e1 - (dq1 * cos(q2) + dq4) * (zC6 * (a6 + xC6) * m6 - I6xz); luan an 222 c51 = (0.4e1 * ((-yC6 * (a6 + xC6) * m6 + I6xy) * cos(q6) + (m6 * yC6 * zC6 - I6yz) * sin(q6) + xC5 * zC5 * m5 - a5 * yC6 * m6 + zC5 * m5 * a5 - I5xz) * dq1 * (cos(q4) ^ + 0.1e1) * cos(q2) ^ + (0.4e1 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + ((-0.2e1 * zC6 * (a6 + xC6) * m6 + (2 * I6xz)) * sin(q6) + 0.2e1 * a5 * m6 * (a6 + xC6)) * cos(q6) - 0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz) * dq1 * sin(q2) * cos(q4) - 0.2e1 * sin(q4) * dq2 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + (-0.4e1 * ((-zC6 * (a6 + xC6) * m6 + I6xz) * sin(q6) + a5 * m6 * (a6 + xC6)) * dq2 * sin(q4) - 0.4e1 * dq4 * (yC6 * (a6 + xC6) * m6 - I6xy)) * cos(q6) - 0.2e1 * (-0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy I6zz) * dq2 * sin(q4) + 0.4e1 * ((m6 * yC6 * zC6 - I6yz) * sin(q6) + xC5 * zC5 * m5 - a5 * yC6 * m6 + zC5 * m5 * a5 - I5xz) * dq4) * cos(q2) - 0.4e1 * (((-yC6 * (a6 + xC6) * m6 + I6xy) * cos(q6) + (m6 * yC6 * zC6 - I6yz) * sin(q6) + xC5 * zC5 * m5 - a5 * yC6 * m6 + zC5 * m5 * a5 I5xz) * dq1 * cos(q4) - (dq4 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ / 0.2e1 + (-dq2 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q4) + ((-zC6 * (a6 + xC6) * m6 + I6xz) * sin(q6) + a5 * m6 * (a6 + xC6)) * dq4) * cos(q6) + ((m6 * yC6 * zC6 - I6yz) * sin(q6) + xC5 * zC5 * m5 - a5 * yC6 * m6 + zC5 * m5 * a5 - I5xz) * dq2 * sin(q4) + dq4 * (-0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz) / 0.2e1) * sin(q2)) * cos(q4)) * cos(q5) ^ / 0.2e1 + (0.2e1 * dq1 * (sin(q5) * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + ((-0.2e1 * zC6 * (a6 + xC6) * m6 + (2 * I6xz)) * sin(q6) + 0.2e1 * a5 * m6 * (a6 + xC6)) * cos(q6) - 0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz) * cos(q4) ^ - ((m6 * yC6 * zC6 I6yz) * cos(q6) + (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) - yC6 * d5 * m6 - zC5 * (yC5 - d5) * m5 + I5yz) * sin(q4) * cos(q4) + sin(q5) * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + ((-0.2e1 * zC6 * (a6 + xC6) * m6 + (2 * I6xz)) * sin(q6) + 0.2e1 * a5 * m6 * (a6 + xC6)) * cos(q6) - 0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz) - (d4 + q3) * (m6 * (a6 + xC6) * cos(q6) - zC6 * sin(q6) * m6 + a5 * m6 + m5 * (a5 + xC5))) * cos(q2) ^ + (-0.2e1 * dq1 * ((((-0.4e1 * yC6 * (a6 + xC6) * m6 + 0.4e1 * I6xy) * cos(q6) + (0.4e1 * m6 * yC6 * zC6 - 0.4e1 * I6yz) * sin(q6) + 0.4e1 * xC5 * zC5 * m5 - 0.4e1 * a5 * yC6 * m6 + 0.4e1 * zC5 * m5 * a5 - 0.4e1 * I5xz) * sin(q5) + (d4 + q3) * (-m5 * zC5 + m6 * yC6)) * sin(q2) + a1 * (-m5 * zC5 + m6 * yC6)) * cos(q4) + 0.2e1 * ((0.2e1 * zC6 * (a6 + xC6) * m6 - (2 * I6xz)) * cos(q6) ^ + (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) - m6 * (-a5 * zC6 + d5 * (a6 + xC6))) * cos(q6) + ((a6 + xC6) * a5 + zC6 * d5) * m6 * sin(q6) + (-d5 * a5 - zC6 * (a6 + xC6)) * m6 + m5 * (yC5 - d5) * a5 + xC5 * (yC5 - d5) * m5 - I5xy + I6xz) * dq1 * sin(q4) * sin(q2) + (0.2e1 * dq4 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + (-0.4e1 * dq2 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q4) + 0.4e1 * ((-zC6 * (a6 + xC6) * m6 + I6xz) * sin(q6) + a5 * m6 * (a6 + xC6)) * dq4) * cos(q6) + 0.4e1 * ((m6 * yC6 * zC6 - I6yz) * sin(q6) + xC5 * zC5 * m5 - a5 * yC6 * m6 + zC5 * m5 * a5 - I5xz) * dq2 * sin(q4) + 0.2e1 * dq4 * (-0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ luan an 223 2) * m5 - I5xx + I5zz + I6yy - I6zz)) * sin(q5) + 0.2e1 * dq6 * (yC6 * (a6 + xC6) * m6 - I6xy) * cos(q6) + 0.2e1 * dq2 * (d4 + q3) * (-m5 * zC5 + m6 * yC6) * sin(q4) - 0.2e1 * dq6 * sin(q6) * (m6 * yC6 * zC6 I6yz)) * cos(q2) + (0.2e1 * ((m6 * yC6 * zC6 - I6yz) * cos(q6) + (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) - yC6 * d5 * m6 - zC5 * (yC5 - d5) * m5 + I5yz) * dq2 * sin(q2) - 0.2e1 * sin(q5) * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + ((-0.2e1 * zC6 * (a6 + xC6) * m6 + (2 * I6xz)) * sin(q6) + 0.2e1 * a5 * m6 * (a6 + xC6)) * cos(q6) - 0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz) * dq1) * cos(q4) ^ + (((0.2e1 * sin(q4) * dq2 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + (0.4e1 * ((-zC6 * (a6 + xC6) * m6 + I6xz) * sin(q6) + a5 * m6 * (a6 + xC6)) * dq2 * sin(q4) + 0.4e1 * dq4 * (yC6 * (a6 + xC6) * m6 - I6xy)) * cos(q6) + 0.2e1 * (-0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz) * dq2 * sin(q4) - 0.4e1 * ((m6 * yC6 * zC6 - I6yz) * sin(q6) + xC5 * zC5 * m5 - a5 * yC6 * m6 + zC5 * m5 * a5 - I5xz) * dq4) * sin(q5) 0.2e1 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + ((-0.2e1 * zC6 * (a6 + xC6) * m6 + (2 * I6xz)) * sin(q6) + a5 * m6 * (a6 + xC6)) * cos(q6) - a5 * zC6 * sin(q6) * m6 + m6 * zC6 ^ + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1) * dq6) * sin(q2) + 0.2e1 * dq1 * ((m6 * yC6 * zC6 - I6yz) * cos(q6) + (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) - yC6 * d5 * m6 - zC5 * (yC5 - d5) * m5 + I5yz) * sin(q4)) * cos(q4) + (0.4e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * dq4 * sin(q4) * cos(q6) ^ + (0.2e1 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) - m6 * (-a5 * zC6 + d5 * (a6 + xC6))) * dq4 * sin(q4) + (0.2e1 * a1 * (a6 + xC6) * dq1 - 0.2e1 * dq2 * yC6 * zC6) * m6 + 0.2e1 * I6yz * dq2) * cos(q6) + (0.2e1 * ((a6 + xC6) * a5 + zC6 * d5) * dq4 * m6 * sin(q6) + (-0.2e1 * dq4 * d5 * a5 - 0.2e1 * dq4 * (a6 + xC6) * zC6 + 0.2e1 * dq3 * yC6) * m6 + 0.2e1 * dq4 * m5 * (yC5 - d5) * a5 + (0.2e1 * xC5 * (yC5 - d5) * dq4 - 0.2e1 * dq3 * zC5) * m5 - 0.2e1 * dq4 * (I5xy - I6xz)) * sin(q4) + ((-0.2e1 * a1 * dq1 * zC6 0.2e1 * yC6 * dq2 * (a6 + xC6)) * m6 + 0.2e1 * I6xy * dq2) * sin(q6) + (0.2e1 * a1 * a5 * dq1 + 0.2e1 * d5 * dq2 * yC6) * m6 + 0.2e1 * a1 * m5 * (a5 + xC5) * dq1 + 0.2e1 * (zC5 * (yC5 - d5) * m5 - I5yz) * dq2) * sin(q2) + 0.2e1 * (d4 + q3) * (m6 * (a6 + xC6) * cos(q6) - zC6 * sin(q6) * m6 + a5 * m6 + m5 * (a5 + xC5)) * dq1) * cos(q5) / 0.2e1 + (((yC6 * (a6 + xC6) * m6 - I6xy) * cos(q6) + (-m6 * yC6 * zC6 + I6yz) * sin(q6) - xC5 * zC5 * m5 + a5 * yC6 * m6 - zC5 * m5 * a5 + I5xz) * cos(q4) ^ + sin(q5) * ((0.2e1 * zC6 * (a6 + xC6) * m6 - (2 * I6xz)) * cos(q6) ^ + (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) - m6 * (-a5 * zC6 + d5 * (a6 + xC6))) * cos(q6) + ((a6 + xC6) * a5 + zC6 * d5) * m6 * sin(q6) + (-d5 * a5 - zC6 * (a6 + xC6)) * m6 + m5 * (yC5 - d5) * a5 + xC5 * (yC5 - d5) * m5 - I5xy + I6xz) * sin(q4) * cos(q4) - (d4 + q3) * (-m5 * zC5 + m6 * yC6) * sin(q5) + (yC6 * (a6 + xC6) * m6 - I6xy) * cos(q6) + (-m6 * yC6 * zC6 + I6yz) * sin(q6) - xC5 * zC5 * m5 + a5 * yC6 * m6 - zC5 * m5 * a5 + I5xz) * dq1 * cos(q2) ^ + (0.2e1 * (((d4 + q3) * (m6 * (a6 + xC6) * cos(q6) - zC6 * sin(q6) * m6 + a5 * m6 + m5 * (a5 + xC5)) * sin(q5) + (-(xC6 + zC6 + a6) * (xC6 zC6 + a6) * m6 + I6xx - I6zz) * cos(q6) ^ + ((0.2e1 * zC6 * (a6 + xC6) * m6 - (2 * I6xz)) * sin(q6) - 0.2e1 * a5 * m6 * (a6 + xC6)) * cos(q6) + 0.2e1 * a5 * zC6 * sin(q6) * m6 + (-a5 ^ + yC6 ^ - zC6 ^ 2) * m6 - m5 * a5 ^ - 0.2e1 * xC5 * m5 * a5 + (-xC5 ^ + zC5 ^ 2) * m5 + I5xx - I5zz - I6yy + I6zz) * sin(q2) + sin(q5) * a1 * (m6 * (a6 + xC6) * cos(q6) - zC6 * sin(q6) * m6 + a5 * m6 + m5 * (a5 + xC5))) * dq1 * cos(q4) + 0.2e1 * sin(q5) * dq1 * ((m6 * yC6 * zC6 - I6yz) * cos(q6) luan an 224 + (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) - yC6 * d5 * m6 - zC5 * (yC5 - d5) * m5 + I5yz) * sin(q4) * sin(q2) + (-0.2e1 * dq6 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + (-0.2e1 * dq2 * m6 * (d4 + q3) * (a6 + xC6) * sin(q4) - 0.2e1 * dq6 * ((-0.2e1 * zC6 * (a6 + xC6) * m6 + (2 * I6xz)) * sin(q6) + a5 * m6 * (a6 + xC6))) * cos(q6) - 0.2e1 * (-zC6 * sin(q6) * m6 + a5 * m6 + m5 * (a5 + xC5)) * (d4 + q3) * dq2 * sin(q4) + 0.2e1 * dq6 * (a5 * zC6 * sin(q6) * m6 - m6 * zC6 ^ - I6xx / 0.2e1 - I6yy / 0.2e1 + I6zz / 0.2e1)) * sin(q5) + 0.2e1 * sin(q4) * dq2 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 I6xx + I6zz) * cos(q6) ^ + (0.4e1 * ((-zC6 * (a6 + xC6) * m6 + I6xz) * sin(q6) + a5 * m6 * (a6 + xC6)) * dq2 * sin(q4) + 0.2e1 * dq4 * (yC6 * (a6 + xC6) * m6 - I6xy)) * cos(q6) + 0.2e1 * (-0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ + zC6 ^ 2) * m6 + xC5 ^ * m5 + 0.2e1 * xC5 * m5 * a5 + m5 * a5 ^ - I5xx / 0.2e1 + I5yy / 0.2e1 + I5zz / 0.2e1 + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1) * dq2 * sin(q4) - 0.2e1 * ((m6 * yC6 * zC6 - I6yz) * sin(q6) + xC5 * zC5 * m5 - a5 * yC6 * m6 + zC5 * m5 * a5 - I5xz) * dq4) * cos(q2) / 0.2e1 + (-0.2e1 * sin(q5) * ((0.2e1 * zC6 * (a6 + xC6) * m6 - (2 * I6xz)) * cos(q6) ^ + (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) - m6 * (-a5 * zC6 + d5 * (a6 + xC6))) * cos(q6) + ((a6 + xC6) * a5 + zC6 * d5) * m6 * sin(q6) + (-d5 * a5 - zC6 * (a6 + xC6)) * m6 + m5 * (yC5 - d5) * a5 + xC5 * (yC5 - d5) * m5 - I5xy + I6xz) * dq2 * sin(q2) + 0.2e1 * ((-yC6 * (a6 + xC6) * m6 + I6xy) * cos(q6) + (m6 * yC6 * zC6 - I6yz) * sin(q6) + xC5 * zC5 * m5 - a5 * yC6 * m6 + zC5 * m5 * a5 - I5xz) * dq1) * cos(q4) ^ / 0.2e1 + ((-0.2e1 * dq6 * ((yC6 * (a6 + xC6) * m6 - I6xy) * cos(q6) - (m6 * yC6 * zC6 - I6yz) * sin(q6)) * sin(q5) + 0.2e1 * dq2 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q4) * cos(q6) - 0.2e1 * ((m6 * yC6 * zC6 - I6yz) * sin(q6) + xC5 * zC5 * m5 - a5 * yC6 * m6 + zC5 * m5 * a5 - I5xz) * dq2 * sin(q4) + 0.2e1 * (yC6 ^ * m6 + zC5 ^ * m5 + I5xx / 0.2e1 + I5yy / 0.2e1 - I5zz / 0.2e1 + I6xx / 0.2e1 - I6yy / 0.2e1 + I6zz / 0.2e1) * dq4) * sin(q2) - 0.2e1 * sin(q5) * ((0.2e1 * zC6 * (a6 + xC6) * m6 - (2 * I6xz)) * cos(q6) ^ + (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) - m6 * (-a5 * zC6 + d5 * (a6 + xC6))) * cos(q6) + ((a6 + xC6) * a5 + zC6 * d5) * m6 * sin(q6) + (-d5 * a5 - zC6 * (a6 + xC6)) * m6 + m5 * (yC5 - d5) * a5 + xC5 * (yC5 - d5) * m5 - I5xy + I6xz) * dq1 * sin(q4)) * cos(q4) / 0.2e1 + ((0.4e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * dq2 * cos(q6) ^ + (((0.2e1 * dq4 * yC6 * zC6 - 0.2e1 * dq3 * (a6 + xC6)) * m6 - 0.2e1 * dq4 * I6yz) * sin(q4) + 0.2e1 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 I6xx + I6zz) * sin(q6) - m6 * (-a5 * zC6 + d5 * (a6 + xC6))) * dq2) * cos(q6) + (((0.2e1 * dq3 * zC6 + 0.2e1 * dq4 * yC6 * (a6 + xC6)) * m6 0.2e1 * I6xy * dq4) * sin(q6) + (-0.2e1 * d5 * dq4 * yC6 - 0.2e1 * a5 * dq3) * m6 - 0.2e1 * dq3 * m5 * a5 + (-0.2e1 * zC5 * (yC5 - d5) * dq4 0.2e1 * dq3 * xC5) * m5 + 0.2e1 * dq4 * I5yz) * sin(q4) + 0.2e1 * ((a6 + xC6) * a5 + zC6 * d5) * m6 * dq2 * sin(q6) + (0.2e1 * a1 * dq1 * yC6 - 0.2e1 * (d5 * a5 + zC6 * (a6 + xC6)) * dq2) * m6 - 0.2e1 * dq1 * a1 * m5 * zC5 + 0.2e1 * (m5 * (yC5 - d5) * a5 + xC5 * (yC5 - d5) * m5 - I5xy + I6xz) * dq2) * sin(q5) - 0.2e1 * dq6 * ((0.2e1 * zC6 * (a6 + xC6) * m6 - (2 * I6xz)) * cos(q6) ^ + (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + a5 * zC6 * m6) * cos(q6) + a5 * m6 * (a6 + xC6) * sin(q6) - zC6 * (a6 + xC6) * m6 + I6xz) * sin(q4)) * sin(q2) / 0.2e1 + dq1 * sin(q5) * (d4 + q3) * (-m5 * zC5 + m6 * yC6); c52 = ((-0.4e1 * dq1 * ((yC6 * (a6 + xC6) * m6 - I6xy) * cos(q6) + (m6 * yC6 * zC6 + I6yz) * sin(q6) - xC5 * zC5 * m5 + a5 * yC6 * m6 - zC5 * m5 * a5 + I5xz) * sin(q2) * cos(q4) - 0.2e1 * (dq1 * cos(q2) + dq4) * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + ((-0.2e1 * zC6 * (a6 + xC6) * m6 + (2 * I6xz)) * sin(q6) + 0.2e1 * a5 luan an 225 * m6 * (a6 + xC6)) * cos(q6) - 0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz)) * sin(q4) 0.4e1 * ((yC6 * (a6 + xC6) * m6 - I6xy) * cos(q6) + (-m6 * yC6 * zC6 + I6yz) * sin(q6) - xC5 * zC5 * m5 + a5 * yC6 * m6 - zC5 * m5 * a5 + I5xz) * (cos(q4) - 0.1e1) * dq2 * (cos(q4) + 0.1e1)) * cos(q5) ^ / 0.2e1 + (((0.2e1 * dq1 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 I6xx + I6zz) * cos(q6) ^ + ((-0.2e1 * zC6 * (a6 + xC6) * m6 + (2 * I6xz)) * sin(q6) + 0.2e1 * a5 * m6 * (a6 + xC6)) * cos(q6) - 0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz) * sin(q2) * sin(q5) - 0.2e1 * ((m6 * yC6 * zC6 - I6yz) * cos(q6) + (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) - yC6 * d5 * m6 zC5 * (yC5 - d5) * m5 + I5yz) * dq2) * cos(q4) - 0.4e1 * ((yC6 * (a6 + xC6) * m6 - I6xy) * cos(q6) + (-m6 * yC6 * zC6 + I6yz) * sin(q6) - xC5 * zC5 * m5 + a5 * yC6 * m6 - zC5 * m5 * a5 + I5xz) * (dq1 * cos(q2) + dq4) * sin(q5) + 0.2e1 * dq6 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + 0.2e1 * dq6 * ((-0.2e1 * zC6 * (a6 + xC6) * m6 + (2 * I6xz)) * sin(q6) + a5 * m6 * (a6 + xC6)) * cos(q6) 0.2e1 * sin(q6) * a5 * dq6 * m6 * zC6 + 0.2e1 * dq1 * (d4 + q3) * (-m5 * zC5 + m6 * yC6) * cos(q2) + 0.2e1 * (m6 * zC6 ^ + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1) * dq6) * sin(q4) + (0.2e1 * dq2 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + ((0.2e1 * zC6 * (a6 + xC6) * m6 + (2 * I6xz)) * sin(q6) + 0.2e1 * a5 * m6 * (a6 + xC6)) * cos(q6) - 0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz) * sin(q5) + 0.2e1 * dq1 * ((m6 * yC6 * zC6 - I6yz) * cos(q6) + (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) - yC6 * d5 * m6 - zC5 * (yC5 - d5) * m5 + I5yz) * sin(q2)) * cos(q4) ^ + (0.4e1 * dq4 * (zC6 * (a6 + xC6) * m6 - I6xz) * cos(q6) ^ + 0.2e1 * dq4 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + m6 * (a5 * zC6 - d5 * (a6 + xC6))) * cos(q6) + 0.2e1 * ((a6 + xC6) * a5 + zC6 * d5) * dq4 * m6 * sin(q6) + (-0.2e1 * dq4 * d5 * a5 - 0.2e1 * dq4 * (a6 + xC6) * zC6 + 0.2e1 * dq3 * yC6) * m6 + 0.2e1 * dq4 * m5 * (yC5 - d5) * a5 + (0.2e1 * xC5 * (yC5 - d5) * dq4 0.2e1 * dq3 * zC5) * m5 - 0.2e1 * dq4 * (I5xy - I6xz)) * cos(q4) 0.2e1 * dq2 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + ((-0.2e1 * zC6 * (a6 + xC6) * m6 + (2 * I6xz)) * sin(q6) + 0.2e1 * a5 * m6 * (a6 + xC6)) * cos(q6) - 0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz) * sin(q5) + (-0.2e1 * dq1 * (m6 * yC6 * zC6 - I6yz) * sin(q2) + 0.2e1 * dq2 * m6 * (d4 + q3) * (a6 + xC6)) * cos(q6) - 0.2e1 * dq1 * ((yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) - yC6 * d5 * m6 - zC5 * (yC5 - d5) * m5 + I5yz) * sin(q2) + 0.2e1 * (-zC6 * sin(q6) * m6 + a5 * m6 + m5 * (a5 + xC5)) * (d4 + q3) * dq2) * cos(q5) / 0.2e1 + ((0.2e1 * dq2 * ((0.2e1 * zC6 * (a6 + xC6) * m6 - (2 * I6xz)) * cos(q6) ^ + (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + m6 * (a5 * zC6 - d5 * (a6 + xC6))) * cos(q6) + ((a6 + xC6) * a5 + zC6 * d5) * m6 * sin(q6) + (-d5 * a5 - zC6 * (a6 + xC6)) * m6 + m5 * (yC5 - d5) * a5 + xC5 * (yC5 - d5) * m5 - I5xy + I6xz) * sin(q5) + 0.2e1 * dq1 * ((yC6 * (a6 + xC6) * m6 - I6xy) * cos(q6) + (-m6 * yC6 * zC6 + I6yz) * sin(q6) - xC5 * zC5 * m5 + a5 * yC6 * m6 - zC5 * m5 * a5 + I5xz) * sin(q2)) * cos(q4) + ((-0.2e1 * dq1 * m6 * (d4 + q3) * (a6 + xC6) * cos(q2) + 0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq6) * cos(q6) + (0.2e1 * dq1 * zC6 * m6 * (d4 + q3) * cos(q2) - 0.2e1 * dq6 * (m6 * yC6 * zC6 I6yz)) * sin(q6) - 0.2e1 * dq1 * (d4 + q3) * cos(q2) * (a5 * m6 + m5 * (a5 + xC5))) * sin(q5) + 0.2e1 * dq1 * ((xC6 + zC6 + a6) * (xC6 - zC6 + luan an 226 a6) * m6 - I6xx + I6zz) * cos(q2) * cos(q6) ^ + 0.4e1 * dq1 * cos(q2) * ((-zC6 * (a6 + xC6) * m6 + I6xz) * sin(q6) + a5 * m6 * (a6 + xC6)) * cos(q6) - 0.4e1 * dq1 * cos(q2) * sin(q6) * a5 * m6 * zC6 + 0.2e1 * dq1 * ((a5 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + xC5 ^ * m5 - I5xx / 0.2e1 + I5yy / 0.2e1 + I5zz / 0.2e1 + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1) * cos(q2) - 0.2e1 * (yC6 ^ * m6 + zC5 ^ * m5 + I5xx / 0.2e1 + I5yy / 0.2e1 - I5zz / 0.2e1 + I6xx / 0.2e1 I6yy / 0.2e1 + I6zz / 0.2e1) * dq4) * sin(q4) / 0.2e1 + (-0.2e1 * dq1 * ((0.2e1 * zC6 * (a6 + xC6) * m6 - (2 * I6xz)) * cos(q6) ^ + (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + m6 * (a5 * zC6 - d5 * (a6 + xC6))) * cos(q6) + ((a6 + xC6) * a5 + zC6 * d5) * m6 * sin(q6) + (-d5 * a5 - zC6 * (a6 + xC6)) * m6 + m5 * (yC5 - d5) * a5 + xC5 * (yC5 - d5) * m5 - I5xy + I6xz) * sin(q2) * sin(q5) + 0.2e1 * ((yC6 * (a6 + xC6) * m6 - I6xy) * cos(q6) + (-m6 * yC6 * zC6 + I6yz) * sin(q6) - xC5 * zC5 * m5 + a5 * yC6 * m6 - zC5 * m5 * a5 + I5xz) * dq2) * cos(q4) ^ / 0.2e1 + ((((0.2e1 * dq4 * yC6 * zC6 - 0.2e1 * dq3 * (a6 + xC6)) * m6 - 0.2e1 * dq4 * I6yz) * cos(q6) + ((0.2e1 * dq3 * zC6 + 0.2e1 * dq4 * yC6 * (a6 + xC6)) * m6 - 0.2e1 * I6xy * dq4) * sin(q6) + (-0.2e1 * d5 * dq4 * yC6 - 0.2e1 * a5 * dq3) * m6 - 0.2e1 * dq3 * m5 * a5 + (-0.2e1 * zC5 * (yC5 - d5) * dq4 - 0.2e1 * dq3 * xC5) * m5 + 0.2e1 * dq4 * I5yz) * sin(q5) - 0.2e1 * ((0.2e1 * zC6 * (a6 + xC6) * m6 - (2 * I6xz)) * cos(q6) ^ + (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 I6xx + I6zz) * sin(q6) + a5 * zC6 * m6) * cos(q6) + a5 * m6 * (a6 + xC6) * sin(q6) - zC6 * (a6 + xC6) * m6 + I6xz) * dq6) * cos(q4) / 0.2e1 + (0.4e1 * dq1 * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q2) * cos(q6) ^ + 0.2e1 * dq1 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + m6 * (a5 * zC6 - d5 * (a6 + xC6))) * sin(q2) * cos(q6) + 0.2e1 * dq1 * (((a6 + xC6) * a5 + zC6 * d5) * m6 * sin(q6) + (-d5 * a5 - zC6 * (a6 + xC6)) * m6 + m5 * (yC5 - d5) * a5 + xC5 * (yC5 - d5) * m5 - I5xy + I6xz) * sin(q2) + 0.2e1 * dq2 * (d4 + q3) * (-m5 * zC5 + m6 * yC6)) * sin(q5) / 0.2e1 - ((yC6 * (a6 + xC6) * m6 - I6xy) * cos(q6) + (-m6 * yC6 * zC6 + I6yz) * sin(q6) - xC5 * zC5 * m5 + a5 * yC6 * m6 - zC5 * m5 * a5 + I5xz) * dq2; c53 = -(dq1 * sin(q4) * sin(q2) + cos(q4) * dq2) * ((m6 * (a6 + xC6) * cos(q6) - zC6 * sin(q6) * m6 + a5 * m6 + m5 * (a5 + xC5)) * sin(q5) cos(q5) * (-m5 * zC5 + m6 * yC6)); c54 = (0.2e1 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * (dq1 * cos(q4) * sin(q2) - dq2 * sin(q4)) * cos(q6) ^ + (0.4e1 * dq1 * sin(q2) * ((-zC6 * (a6 + xC6) * m6 + I6xz) * sin(q6) + a5 * m6 * (a6 + xC6)) * cos(q4) - 0.4e1 * ((-zC6 * (a6 + xC6) * m6 + I6xz) * sin(q6) + a5 * m6 * (a6 + xC6)) * dq2 * sin(q4) - 0.4e1 * (dq1 * cos(q2) + dq4) * (yC6 * (a6 + xC6) * m6 - I6xy)) * cos(q6) + 0.2e1 * dq1 * sin(q2) * (-0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz) * cos(q4) - 0.2e1 * (-0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy I6zz) * dq2 * sin(q4) - 0.4e1 * ((-m6 * yC6 * zC6 + I6yz) * sin(q6) + a5 * yC6 * m6 - zC5 * m5 * a5 - xC5 * zC5 * m5 + I5xz) * (dq1 * cos(q2) + dq4)) * cos(q5) ^ / 0.2e1 + ((0.2e1 * (dq1 * cos(q2) + dq4) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q5) + 0.4e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * (dq1 * sin(q4) * sin(q2) + cos(q4) * dq2)) * cos(q6) ^ + ((0.4e1 * dq1 * sin(q2) * (yC6 * (a6 + xC6) * m6 - I6xy) * cos(q4) - 0.4e1 * dq2 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q4) + 0.4e1 * (dq1 * cos(q2) + dq4) * ((-zC6 * (a6 + xC6) * m6 + I6xz) * sin(q6) + a5 * m6 * (a6 + xC6))) * sin(q5) + 0.2e1 * (((xC6 + luan an 227 zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + m6 * (a5 * zC6 - d5 * (a6 + xC6))) * dq2 * cos(q4) + 0.2e1 * dq1 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + m6 * (a5 * zC6 d5 * (a6 + xC6))) * sin(q4) * sin(q2) + 0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq6) * cos(q6) + (0.4e1 * ((-m6 * yC6 * zC6 + I6yz) * sin(q6) + a5 * yC6 * m6 - zC5 * m5 * a5 - xC5 * zC5 * m5 + I5xz) * dq1 * sin(q2) * cos(q4) - 0.4e1 * ((-m6 * yC6 * zC6 + I6yz) * sin(q6) + a5 * yC6 * m6 - zC5 * m5 * a5 - xC5 * zC5 * m5 + I5xz) * dq2 * sin(q4) + 0.2e1 * (-0.2e1 * a5 * zC6 * sin(q6) * m6 + (a5 ^ - yC6 ^ + zC6 ^ 2) * m6 + m5 * a5 ^ + 0.2e1 * xC5 * m5 * a5 + (xC5 ^ - zC5 ^ 2) * m5 - I5xx + I5zz + I6yy - I6zz) * (dq1 * cos(q2) + dq4)) * sin(q5) + 0.2e1 * dq2 * (((a6 + xC6) * a5 + zC6 * d5) * m6 * sin(q6) + (-d5 * a5 - zC6 * (a6 + xC6)) * m6 + m5 * (yC5 - d5) * a5 + xC5 * (yC5 - d5) * m5 - I5xy + I6xz) * cos(q4) + 0.2e1 * dq1 * (((a6 + xC6) * a5 + zC6 * d5) * m6 * sin(q6) + (-d5 * a5 - zC6 * (a6 + xC6)) * m6 + m5 * (yC5 - d5) * a5 + xC5 * (yC5 - d5) * m5 - I5xy + I6xz) * sin(q4) * sin(q2) - 0.2e1 * dq6 * sin(q6) * (m6 * yC6 * zC6 - I6yz)) * cos(q5) / 0.2e1 - dq6 * sin(q5) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q6) ^ + ((0.2e1 * dq2 * (m6 * yC6 * zC6 - I6yz) * cos(q4) + 0.2e1 * dq1 * sin(q4) * (m6 * yC6 * zC6 - I6yz) * sin(q2) - 0.2e1 * dq6 * ((0.2e1 * zC6 * (a6 + xC6) * m6 + 0.2e1 * I6xz) * sin(q6) + a5 * m6 * (a6 + xC6))) * sin(q5) + 0.2e1 * (dq1 * cos(q2) + dq4) * (yC6 * (a6 + xC6) * m6 - I6xy)) * cos(q6) / 0.2e1 + (0.2e1 * dq2 * ((yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) - yC6 * d5 * m6 - zC5 * (yC5 - d5) * m5 + I5yz) * cos(q4) + 0.2e1 * dq1 * ((yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) - yC6 * d5 * m6 - zC5 * (yC5 - d5) * m5 + I5yz) * sin(q4) * sin(q2) + 0.2e1 * dq6 * (a5 * zC6 * sin(q6) * m6 - m6 * zC6 ^ - I6xx / 0.2e1 - I6yy / 0.2e1 + I6zz / 0.2e1)) * sin(q5) / 0.2e1 + dq1 * sin(q2) * (yC6 ^ * m6 + zC5 ^ * m5 + I5xx / 0.2e1 + I5yy / 0.2e1 - I5zz / 0.2e1 + I6xx / 0.2e1 - I6yy / 0.2e1 + I6zz / 0.2e1) * cos(q4) - dq2 * (yC6 ^ * m6 + zC5 ^ * m5 + I5xx / 0.2e1 + I5yy / 0.2e1 - I5zz / 0.2e1 + I6xx / 0.2e1 - I6yy / 0.2e1 + I6zz / 0.2e1) * sin(q4) + ((-m6 * yC6 * zC6 + I6yz) * sin(q6) + a5 * yC6 * m6 - zC5 * m5 * a5 - xC5 * zC5 * m5 + I5xz) * (dq1 * cos(q2) + dq4); c55 = -((2 * zC6 * (a6 + xC6) * m6 - * I6xz) * cos(q6) ^ + (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + (a5 * m6 * zC6)) * cos(q6) + a5 * m6 * (a6 + xC6) * sin(q6) - (zC6 * (a6 + xC6) * m6) + I6xz) * dq6; c56 = (-0.2e1 * (dq1 * cos(q4) * sin(q2) - dq2 * sin(q4)) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q5) - 0.2e1 * (dq1 * cos(q2) + dq4) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 I6xx + I6zz) * sin(q5) - 0.4e1 * (dq1 * sin(q4) * sin(q2) + cos(q4) * dq2 + dq5) * (zC6 * (a6 + xC6) * m6 - I6xz)) * cos(q6) ^ / 0.2e1 + ((0.4e1 * (dq1 * cos(q4) * sin(q2) - dq2 * sin(q4)) * (zC6 * (a6 + xC6) * m6 - I6xz) * cos(q5) + 0.4e1 * (dq1 * cos(q2) + dq4) * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q5) - 0.2e1 * (dq1 * sin(q4) * sin(q2) + cos(q4) * dq2 + dq5) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz)) * sin(q6) + (-0.2e1 * a5 * sin(q2) * m6 * dq1 * (a6 + xC6) * cos(q4) + 0.2e1 * a5 * m6 * dq2 * (a6 + xC6) * sin(q4) + 0.2e1 * (dq1 * cos(q2) + dq4) * (yC6 * (a6 + xC6) * m6 - I6xy)) * cos(q5) + (-0.2e1 * dq1 * sin(q2) * (yC6 * (a6 + xC6) * m6 - I6xy) * cos(q4) + 0.2e1 * dq2 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q4) - 0.2e1 * a5 * m6 * (a6 + xC6) * (dq1 * cos(q2) + dq4)) * sin(q5) - 0.2e1 * a5 * zC6 * m6 * dq2 * cos(q4) - 0.2e1 * a5 * zC6 * sin(q2) * sin(q4) * m6 * dq1 + (-0.2e1 * a5 * dq5 * zC6 - 0.2e1 * dq6 * yC6 * (a6 + xC6)) * m6 + 0.2e1 * dq6 * I6xy) * cos(q6) / 0.2e1 + ((0.2e1 * a5 * zC6 * cos(q4) * sin(q2) * m6 * luan an 228 dq1 - 0.2e1 * a5 * zC6 * sin(q4) * m6 * dq2 - 0.2e1 * (m6 * yC6 * zC6 I6yz) * (dq1 * cos(q2) + dq4)) * cos(q5) + (0.2e1 * sin(q2) * dq1 * (m6 * yC6 * zC6 - I6yz) * cos(q4) - 0.2e1 * dq2 * (m6 * yC6 * zC6 - I6yz) * sin(q4) + 0.2e1 * a5 * zC6 * m6 * (dq1 * cos(q2) + dq4)) * sin(q5) 0.2e1 * a5 * m6 * dq2 * (a6 + xC6) * cos(q4) - 0.2e1 * a5 * sin(q4) * m6 * dq1 * (a6 + xC6) * sin(q2) + (0.2e1 * dq6 * yC6 * zC6 - 0.2e1 * a5 * dq5 * (a6 + xC6)) * m6 - 0.2e1 * dq6 * I6yz) * sin(q6) / 0.2e1 - (dq1 * cos(q4) * sin(q2) - dq2 * sin(q4)) * (m6 * zC6 ^ + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1) * cos(q5) - (dq1 * cos(q2) + dq4) * (m6 * zC6 ^ + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1) * sin(q5) + (dq1 * sin(q4) * sin(q2) + cos(q4) * dq2 + dq5) * (zC6 * (a6 + xC6) * m6 I6xz); c61 = ((0.4e1 * dq1 * (cos(q2) + 0.1e1) * (zC6 * (a6 + xC6) * m6 I6xz) * (cos(q2) - 0.1e1) * cos(q5) ^ + 0.4e1 * dq2 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q2) * cos(q5) + 0.4e1 * dq1 * (cos(q2) + 0.1e1) * (zC6 * (a6 + xC6) * m6 - I6xz) * (cos(q2) 0.1e1)) * cos(q4) ^ + (0.4e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q4) * dq2 * sin(q2) * cos(q5) ^ + (-0.4e1 * dq1 * sin(q4) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q2) ^ 0.8e1 * dq1 * sin(q5) * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q2) * cos(q2) + (-0.4e1 * dq4 * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q5) + 0.2e1 * dq5 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz)) * sin(q2) + 0.4e1 * dq1 * sin(q4) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz)) * cos(q5) + 0.2e1 * (sin(q5) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q2) + 0.2e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q4) * sin(q2)) * dq2) * cos(q4) + 0.4e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * (dq1 * cos(q2) + dq4) * cos(q2) * cos(q5) ^ - 0.2e1 * dq2 * (-0.2e1 * sin(q5) * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q4) * cos(q2) + ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q2)) * cos(q5) - 0.8e1 * dq1 * (zC6 * (a6 + xC6) * m6 - I6xz) * cos(q2) ^ + (0.4e1 * dq1 * sin(q5) * sin(q4) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q2) + 0.2e1 * dq5 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q5) - 0.4e1 * dq4 * (zC6 * (a6 + xC6) * m6 - I6xz)) * cos(q2) + 0.2e1 * sin(q4) * (dq4 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 I6xx + I6zz) * sin(q5) + 0.2e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * dq5) * sin(q2) + 0.4e1 * dq1 * (zC6 * (a6 + xC6) * m6 - I6xz)) * cos(q6) ^ / 0.2e1 + ((0.2e1 * dq1 * (cos(q2) + 0.1e1) * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + a5 * m6 * zC6) * (cos(q2) 0.1e1) * cos(q5) ^ + (0.2e1 * dq1 * sin(q5) * (m6 * yC6 * zC6 - I6yz) * cos(q2) ^ - 0.8e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * dq2 * sin(q2) * sin(q6) + 0.2e1 * ((a6 + xC6) * a5 + zC6 * d5) * m6 * dq2 * sin(q2) 0.2e1 * dq1 * sin(q5) * (m6 * yC6 * zC6 - I6yz)) * cos(q5) + 0.2e1 * dq1 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) - d5 * m6 * (a6 + xC6)) * cos(q2) ^ - 0.2e1 * dq1 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + 0.2e1 * sin(q5) * (yC6 * (a6 + xC6) * m6 - I6xy) * dq2 * sin(q2) + 0.2e1 * dq1 * d5 * m6 * (a6 + xC6)) * cos(q4) ^ + (0.4e1 * (dq1 * (m6 * yC6 * zC6 - I6yz) * cos(q2) + sin(q4) * dq2 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) / 0.2e1 + a5 * zC6 * sin(q4) * m6 * dq2 / 0.2e1 + dq4 * (m6 * yC6 * zC6 - I6yz) / 0.2e1) * sin(q2) * cos(q5) ^ + (0.8e1 * dq1 * ((zC6 * (a6 + xC6) * m6 - I6xz) * sin(q6) - ((a6 + xC6) * a5 + zC6 * d5) * m6 / 0.4e1) * sin(q4) * cos(q2) ^ + (-0.4e1 * dq1 * sin(q5) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 I6xx + I6zz) * sin(q2) * sin(q6) - 0.4e1 * dq1 * zC6 * (a5 * sin(q5) d4 / 0.2e1 - q3 / 0.2e1) * m6 * sin(q2) + (0.2e1 * a1 * dq1 * zC6 0.2e1 * dq2 * yC6 * (a6 + xC6)) * m6 + 0.2e1 * dq2 * I6xy) * cos(q2) + luan an 229 ((-0.2e1 * dq4 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q5) - 0.4e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * dq5) * sin(q2) - 0.8e1 * dq1 * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q4)) * sin(q6) + (0.2e1 * dq2 * sin(q5) * (m6 * yC6 * zC6 - I6yz) * sin(q4) 0.2e1 * (dq4 * zC6 * sin(q5) - dq5 * (a6 + xC6)) * m6 * a5) * sin(q2) + 0.2e1 * dq1 * ((a6 + xC6) * a5 + zC6 * d5) * m6 * sin(q4)) * cos(q5) 0.2e1 * dq1 * sin(q5) * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q4) * cos(q2) ^ + (-0.4e1 * sin(q5) * (zC6 * (a6 + xC6) * m6 - I6xz) * dq2 * sin(q6) - 0.2e1 * dq1 * (m6 * yC6 * zC6 - I6yz) * sin(q2) + 0.2e1 * dq2 * m6 * (a5 * sin(q5) - d4 - q3) * (a6 + xC6)) * cos(q2) + 0.2e1 * sin(q4) * dq2 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q2) * sin(q6) + (-0.2e1 * dq2 * d5 * m6 * (a6 + xC6) * sin(q4) + 0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq5 * sin(q5) + (0.2e1 * dq4 * yC6 * zC6 - 0.2e1 * dq3 * (a6 + xC6)) * m6 + 0.2e1 * dq4 * I6yz) * sin(q2) + 0.2e1 * dq1 * sin(q5) * (yC6 * (a6 + xC6) * m6 I6xy) * sin(q4)) * cos(q4) + 0.2e1 * (dq1 * (((xC6 + zC6 + a6) * (xC6 zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + a5 * m6 * zC6) * cos(q2) + dq4 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) - dq2 * (m6 * yC6 * zC6 - I6yz) * sin(q4) + dq4 * zC6 * a5 * m6) * cos(q2) * cos(q5) ^ + (0.2e1 * dq1 * sin(q5) * (m6 * yC6 * zC6 - I6yz) * cos(q2) ^ + (0.2e1 * sin(q5) * sin(q4) * dq2 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) - 0.2e1 * dq1 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q4) * sin(q2) + 0.2e1 * dq2 * zC6 * m6 * (a5 * sin(q5) - d4 - q3) * sin(q4) + 0.2e1 * dq4 * (m6 * yC6 * zC6 - I6yz) * sin(q5) - 0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq5) * cos(q2) + 0.4e1 * ((zC6 * (a6 + xC6) * m6 - I6xz) * dq2 * sin(q6) - zC6 * m6 * (dq2 * d5 + dq3 * sin(q4)) / 0.2e1) * sin(q2)) * cos(q5) - 0.4e1 * dq1 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) - m6 * (zC6 * (d4 + q3) * sin(q5) - a5 * zC6 + d5 * (a6 + xC6)) / 0.2e1) * cos(q2) ^ + ((-0.8e1 * dq1 * sin(q5) * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q4) * sin(q2) - 0.4e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * dq5 * sin(q5) - 0.2e1 * dq4 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz)) * sin(q6) - 0.2e1 * m6 * (-(((a6 + xC6) * a5 + zC6 * d5) * sin(q5) - (d4 + q3) * (a6 + xC6)) * dq1 * sin(q4) * sin(q2) + (a6 + xC6) * (a1 * dq1 * sin(q4) - dq5 * a5 * sin(q5) - dq4 * d5))) * cos(q2) + (-0.4e1 * (dq4 * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q5) dq5 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) / 0.2e1) * sin(q4) * sin(q2) + 0.2e1 * dq1 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz)) * sin(q6) - 0.2e1 * zC6 * m6 * (((-dq4 * sin(q5) * d5 - a5 * dq5) * sin(q4) + a1 * dq1 * sin(q5)) * sin(q2) dq1 * ((-d4 - q3) * sin(q5) + a5))) * cos(q6) / 0.2e1 + (-0.2e1 * dq1 * (cos(q2) + 0.1e1) * (-a5 * m6 * (a6 + xC6) * sin(q6) + zC6 * (a6 + xC6) * m6 - I6xz) * (cos(q2) - 0.1e1) * cos(q5) ^ + (0.2e1 * dq1 * sin(q5) * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) * cos(q2) ^ + (0.2e1 * m6 * dq2 * (-a5 * zC6 + d5 * (a6 + xC6)) * sin(q2) - 0.2e1 * dq1 * sin(q5) * (yC6 * (a6 + xC6) * m6 - I6xy)) * sin(q6) - 0.2e1 * dq2 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q2)) * cos(q5) 0.2e1 * dq1 * (-zC6 * d5 * m6 * sin(q6) + zC6 * (a6 + xC6) * m6 - I6xz) * cos(q2) ^ + (-0.2e1 * dq2 * sin(q5) * (m6 * yC6 * zC6 - I6yz) * sin(q2) - 0.2e1 * dq1 * zC6 * d5 * m6) * sin(q6) + 0.2e1 * dq1 * (zC6 * (a6 + xC6) * m6 - I6xz)) * cos(q4) ^ / 0.2e1 + (0.4e1 * (dq1 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) * cos(q2) + (a5 * m6 * dq2 * (a6 + xC6) * sin(q4) / 0.2e1 + dq4 * (yC6 * (a6 + xC6) * m6 - I6xy) / 0.2e1) * sin(q6) - (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q4) * dq2 / 0.2e1) * sin(q2) * cos(q5) ^ + (0.2e1 * dq1 * sin(q4) * (-m6 * (-a5 * zC6 + d5 * (a6 + xC6)) * sin(q6) + (xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 I6xx + I6zz) * cos(q2) ^ + ((-0.4e1 * dq1 * (a6 + xC6) * (a5 * sin(q5) - d4 / 0.2e1 - q3 / 0.2e1) * m6 * sin(q2) + (0.2e1 * a1 * (a6 + luan an 230 xC6) * dq1 + 0.2e1 * dq2 * yC6 * zC6) * m6 - 0.2e1 * dq2 * I6yz) * sin(q6) + 0.4e1 * dq1 * sin(q5) * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q2)) * cos(q2) + ((0.2e1 * sin(q5) * (yC6 * (a6 + xC6) * m6 - I6xy) * dq2 * sin(q4) - 0.2e1 * m6 * (dq4 * (a6 + xC6) * sin(q5) + dq5 * zC6) * a5) * sin(q2) + 0.2e1 * dq1 * m6 * sin(q4) * (-a5 * zC6 + d5 * (a6 + xC6))) * sin(q6) + (0.2e1 * dq4 * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q5) + 0.2e1 * (m6 * zC6 ^ + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1) * dq5) * sin(q2) - 0.2e1 * dq1 * sin(q4) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz)) * cos(q5) + 0.2e1 * dq1 * sin(q4) * sin(q5) * sin(q6) * (m6 * yC6 * zC6 - I6yz) * cos(q2) ^ + ((-0.2e1 * dq1 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q2) - 0.2e1 * dq2 * zC6 * m6 * (a5 * sin(q5) - d4 - q3)) * sin(q6) + 0.2e1 * sin(q5) * (m6 * zC6 ^ + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1) * dq2) * cos(q2) + ((0.2e1 * dq2 * zC6 * sin(q4) * d5 * m6 - 0.2e1 * dq5 * (m6 * yC6 * zC6 - I6yz) * sin(q5) + (0.2e1 * dq3 * zC6 - 0.2e1 * dq4 * yC6 * (a6 + xC6)) * m6 + 0.2e1 * dq4 * I6xy) * sin(q2) - 0.2e1 * dq1 * sin(q4) * sin(q5) * (m6 * yC6 * zC6 - I6yz)) * sin(q6) - 0.2e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q4) * dq2 * sin(q2)) * cos(q4) / 0.2e1 - (dq1 * (-a5 * m6 * (a6 + xC6) * sin(q6) + zC6 * (a6 + xC6) * m6 - I6xz) * cos(q2) + (dq2 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q4) - dq4 * a5 * m6 * (a6 + xC6)) * sin(q6) + dq4 * (zC6 * (a6 + xC6) * m6 - I6xz)) * cos(q2) * cos(q5) ^ + (0.2e1 * dq1 * sin(q5) * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q6) * cos(q2) ^ + ((0.2e1 * dq1 * sin(q4) * (m6 * yC6 * zC6 - I6yz) * sin(q2) + 0.2e1 * dq2 * m6 * (a5 * sin(q5) - d4 - q3) * (a6 + xC6) * sin(q4) + 0.2e1 * dq4 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q5) + 0.2e1 * dq5 * (m6 * yC6 * zC6 - I6yz)) * sin(q6) - 0.2e1 * sin(q5) * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q4) * dq2) * cos(q2) + 0.2e1 * (-m6 * (a6 + xC6) * (dq2 * d5 + dq3 * sin(q4)) * sin(q6) + ((a6 + xC6) ^ * m6 - I6xx / 0.2e1 + I6yy / 0.2e1 + I6zz / 0.2e1) * dq2) * sin(q2)) * cos(q5) / 0.2e1 + 0.2e1 * dq1 * (-m6 * (-(d4 + q3) * (a6 + xC6) * sin(q5) + zC6 * d5 + (a6 + xC6) * a5) * sin(q6) / 0.2e1 + zC6 * (a6 + xC6) * m6 - I6xz) * cos(q2) ^ + (0.2e1 * (dq1 * ((-a5 * zC6 + d5 * (a6 + xC6)) * sin(q5) + zC6 * (d4 + q3)) * sin(q4) * sin(q2) + zC6 * (a1 * dq1 * sin(q4) - dq5 * a5 * sin(q5) - dq4 * d5)) * m6 * sin(q6) - 0.2e1 * dq1 * sin(q5) * sin(q4) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q2) + 0.2e1 * (m6 * zC6 ^ + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1) * dq5 * sin(q5) + 0.2e1 * dq4 * (zC6 * (a6 + xC6) * m6 - I6xz)) * cos(q2) / 0.2e1 - (a6 + xC6) * m6 * (((-dq4 * sin(q5) * d5 - a5 * dq5) * sin(q4) + a1 * dq1 * sin(q5)) * sin(q2) - dq1 * ((-d4 - q3) * sin(q5) + a5)) * sin(q6) - (dq4 * ((a6 + xC6) ^ * m6 - I6xx / 0.2e1 + I6yy / 0.2e1 + I6zz / 0.2e1) * sin(q5) + (zC6 * (a6 + xC6) * m6 - I6xz) * dq5) * sin(q4) * sin(q2) - dq1 * (zC6 * (a6 + xC6) * m6 - I6xz); c62 = (0.4e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * (dq1 * cos(q4) * sin(q2) * sin(q4) + dq2 * cos(q4) ^ - dq2) * cos(q6) ^ + (0.2e1 * dq2 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + a5 * m6 * zC6) * cos(q4) ^ + 0.2e1 * sin(q2) * dq1 * (((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + a5 * m6 * zC6) * sin(q4) * cos(q4) - 0.2e1 * (m6 * yC6 * zC6 - I6yz) * (dq1 * cos(q2) + dq4) * sin(q4) - 0.2e1 * dq2 * (((xC6 + zC6 + a6) * (xC6 zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + a5 * m6 * zC6)) * cos(q6) + 0.2e1 * dq2 * (a5 * m6 * (a6 + xC6) * sin(q6) - zC6 * (a6 + xC6) * m6 + I6xz) * cos(q4) ^ + 0.2e1 * sin(q2) * dq1 * (a5 * m6 * (a6 + xC6) * sin(q6) - zC6 * (a6 + xC6) * m6 + I6xz) * sin(q4) * cos(q4) - 0.2e1 * (dq1 * cos(q2) + dq4) * sin(q6) * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q4) - 0.2e1 * dq2 * (a5 * m6 * (a6 + xC6) * sin(q6) - zC6 * (a6 + xC6) * m6 + I6xz)) * cos(q5) ^ / 0.2e1 + ((0.4e1 * sin(q2) * ((xC6 + luan an 231 zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * dq1 * cos(q4) ^ 0.4e1 * dq2 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q4) * cos(q4) + (0.4e1 * (dq1 * cos(q2) + dq4) * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q5) - 0.2e1 * dq5 * ((xC6 + zC6 + a6) * (xC6 zC6 + a6) * m6 - I6xx + I6zz)) * sin(q4) - 0.2e1 * sin(q2) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * dq1) * cos(q6) ^ + ((-0.8e1 * sin(q2) * (zC6 * (a6 + xC6) * m6 - I6xz) * dq1 * sin(q6) + 0.2e1 * dq2 * (m6 * yC6 * zC6 - I6yz) * sin(q5) + 0.2e1 * sin(q2) * dq1 * ((a6 + xC6) * a5 + zC6 * d5) * m6) * cos(q4) ^ + ((0.8e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * dq2 * sin(q6) + 0.2e1 * dq1 * sin(q2) * (m6 * yC6 * zC6 - I6yz) * sin(q5) - 0.2e1 * dq2 * ((a6 + xC6) * a5 + zC6 * d5) * m6) * sin(q4) - 0.2e1 * dq1 * (yC6 * (a6 + xC6) * m6 - I6xy) * cos(q2) - 0.2e1 * dq3 * zC6 * m6) * cos(q4) + ((0.2e1 * (dq1 * cos(q2) + dq4) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q5) + 0.4e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * dq5) * sin(q6) + 0.2e1 * (a5 * zC6 * (dq1 * cos(q2) + dq4) * sin(q5) - zC6 * dq1 * (d4 + q3) * cos(q2) - a5 * dq5 * (a6 + xC6)) * m6) * sin(q4) + 0.4e1 * sin(q2) * (zC6 * (a6 + xC6) * m6 - I6xz) * dq1 * sin(q6) - 0.2e1 * dq2 * (m6 * yC6 * zC6 - I6yz) * sin(q5) - 0.2e1 * dq1 * zC6 * sin(q2) * d5 * m6) * cos(q6) + ((0.2e1 * dq2 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q5) - 0.2e1 * sin(q2) * dq1 * (a5 * zC6 - d5 * (a6 + xC6)) * m6) * sin(q6) - 0.2e1 * sin(q2) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 I6xx + I6zz) * dq1) * cos(q4) ^ + (((0.2e1 * sin(q2) * dq1 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q5) + 0.2e1 * dq2 * (a5 * zC6 - d5 * (a6 + xC6)) * m6) * sin(q6) + 0.2e1 * dq2 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz)) * sin(q4) + 0.2e1 * sin(q6) * (dq1 * (m6 * yC6 * zC6 - I6yz) * cos(q2) - dq3 * m6 * (a6 + xC6))) * cos(q4) + (0.2e1 * (a5 * (a6 + xC6) * (dq1 * cos(q2) + dq4) * sin(q5) - dq1 * (d4 + q3) * (a6 + xC6) * cos(q2) + a5 * dq5 * zC6) * m6 * sin(q6) - 0.2e1 * (dq1 * cos(q2) + dq4) * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q5) 0.2e1 * (m6 * zC6 ^ + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1) * dq5) * sin(q4) + (-0.2e1 * dq2 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q5) - 0.2e1 * dq1 * sin(q2) * d5 * m6 * (a6 + xC6)) * sin(q6) + 0.2e1 * ((a6 + xC6) ^ * m6 - I6xx / 0.2e1 + I6yy / 0.2e1 + I6zz / 0.2e1) * sin(q2) * dq1) * cos(q5) / 0.2e1 + (0.2e1 * dq2 * (zC6 * (a6 + xC6) * m6 - I6xz) * cos(q4) + 0.2e1 * sin(q2) * (zC6 * (a6 + xC6) * m6 - I6xz) * dq1 * sin(q4) + (dq1 * cos(q2) + dq4) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q5) + 0.2e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * dq5) * cos(q4) * cos(q6) ^ + ((0.2e1 * dq2 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + 0.2e1 * sin(q2) * dq1 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q5) 0.2e1 * dq2 * d5 * m6 * (a6 + xC6)) * cos(q4) ^ + ((0.2e1 * sin(q2) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * dq1 * sin(q6) - 0.2e1 * dq2 * (yC6 * (a6 + xC6) * m6 - I6xy) * sin(q5) 0.2e1 * dq1 * sin(q2) * d5 * m6 * (a6 + xC6)) * sin(q4) + (-0.4e1 * (dq1 * cos(q2) + dq4) * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q5) + 0.2e1 * dq5 * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz)) * sin(q6) + 0.2e1 * ((a5 * dq1 * (a6 + xC6) * cos(q2) + dq4 * zC6 * d5) * sin(q5) - dq1 * (d4 + q3) * (a6 + xC6) * cos(q2) + a5 * dq5 * zC6) * m6) * cos(q4) + (-0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq5 * sin(q5) + (0.2e1 * dq4 * yC6 * zC6 + 0.2e1 * dq3 * (a6 + xC6)) * m6 0.2e1 * dq4 * I6yz) * sin(q4) + 0.2e1 * dq2 * (-zC6 * (d4 + q3) * sin(q5) + a5 * zC6 + d5 * (a6 + xC6)) * m6) * cos(q6) / 0.2e1 + ((0.2e1 * dq1 * sin(q2) * (m6 * yC6 * zC6 - I6yz) * sin(q5) + 0.2e1 * d5 * dq2 * m6 * zC6) * sin(q6) - 0.2e1 * dq2 * (zC6 * (a6 + xC6) * m6 I6xz)) * cos(q4) ^ / 0.2e1 + (((0.2e1 * dq2 * (m6 * yC6 * zC6 - I6yz) * sin(q5) + 0.2e1 * dq1 * zC6 * sin(q2) * d5 * m6) * sin(q6) - 0.2e1 * sin(q2) * (zC6 * (a6 + xC6) * m6 - I6xz) * dq1) * sin(q4) - 0.2e1 * luan an 232 ((a5 * dq1 * zC6 * cos(q2) - dq4 * d5 * (a6 + xC6)) * sin(q5) - zC6 * dq1 * (d4 + q3) * cos(q2) - a5 * dq5 * (a6 + xC6)) * m6 * sin(q6) + (0.2e1 * (m6 * zC6 ^ + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1) * dq1 * cos(q2) - 0.2e1 * ((a6 + xC6) ^ * m6 - I6xx / 0.2e1 + I6yy / 0.2e1 + I6zz / 0.2e1) * dq4) * sin(q5) - 0.2e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * dq5) * cos(q4) / 0.2e1 + ((dq5 * (m6 * yC6 * zC6 - I6yz) * sin(q5) + (-dq3 * zC6 + dq4 * yC6 * (a6 + xC6)) * m6 - dq4 * I6xy) * sin(q4) + dq2 * (-(d4 + q3) * (a6 + xC6) * sin(q5) - zC6 * d5 + (a6 + xC6) * a5) * m6) * sin(q6); c63 = -((dq2 * ((a6 + xC6) * sin(q6) + cos(q6) * zC6) * cos(q5) + dq1 * (-sin(q6) * zC6 + (a6 + xC6) * cos(q6)) * sin(q2)) * cos(q4) + sin(q4) * (((a6 + xC6) * sin(q6) + cos(q6) * zC6) * dq1 * sin(q2) * cos(q5) - dq2 * (-sin(q6) * zC6 + (a6 + xC6) * cos(q6)))) * m6; c64 = ((0.4e1 * (-sin(q2) * dq1 * cos(q4) + dq2 * sin(q4)) * (zC6 * (a6 + xC6) * m6 - I6xz) * cos(q5) + 0.2e1 * (dq1 * sin(q4) * sin(q2) + cos(q4) * dq2 + dq5) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz)) * sin(q5) + 0.4e1 * (cos(q2) * dq1 + dq4) * (cos(q5) + 0.1e1) * (zC6 * (a6 + xC6) * m6 - I6xz) * (cos(q5) - 0.1e1)) * cos(q6) ^ / 0.2e1 + (((0.2e1 * (-sin(q2) * dq1 * cos(q4) + dq2 * sin(q4)) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) - 0.2e1 * a5 * zC6 * cos(q4) * sin(q2) * m6 * dq1 + 0.2e1 * a5 * dq2 * zC6 * sin(q4) * m6 + 0.2e1 * (m6 * yC6 * zC6 - I6yz) * (cos(q2) * dq1 + dq4)) * cos(q5) - 0.4e1 * (dq1 * sin(q4) * sin(q2) + cos(q4) * dq2 + dq5) * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q6) + 0.2e1 * (dq2 * zC6 * d5 * cos(q4) + zC6 * sin(q2) * sin(q4) * d5 * dq1 + a5 * dq5 * (a6 + xC6)) * m6) * sin(q5) + (0.2e1 * (cos(q2) * dq1 + dq4) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) + 0.2e1 * sin(q2) * dq1 * (m6 * yC6 * zC6 - I6yz) * cos(q4) - 0.2e1 * dq2 * (m6 * yC6 * zC6 I6yz) * sin(q4) + 0.2e1 * a5 * zC6 * m6 * (cos(q2) * dq1 + dq4)) * cos(q5) ^ - 0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq5 * cos(q5) 0.2e1 * (cos(q2) * dq1 + dq4) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * sin(q6) - 0.2e1 * sin(q2) * dq1 * (m6 * yC6 * zC6 I6yz) * cos(q4) + 0.2e1 * dq2 * (m6 * yC6 * zC6 - I6yz) * sin(q4) + 0.2e1 * d5 * m6 * (a6 + xC6) * (cos(q2) * dq1 + dq4)) * cos(q6) / 0.2e1 + (((-0.2e1 * a5 * sin(q2) * m6 * dq1 * (a6 + xC6) * cos(q4) + 0.2e1 * a5 * dq2 * m6 * (a6 + xC6) * sin(q4) + 0.2e1 * (yC6 * (a6 + xC6) * m6 I6xy) * (cos(q2) * dq1 + dq4)) * sin(q6) - 0.2e1 * (-sin(q2) * dq1 * cos(q4) + dq2 * sin(q4)) * (zC6 * (a6 + xC6) * m6 - I6xz)) * cos(q5) 0.2e1 * (-dq2 * d5 * (a6 + xC6) * cos(q4) - sin(q4) * d5 * dq1 * (a6 + xC6) * sin(q2) + a5 * dq5 * zC6) * m6 * sin(q6) - 0.2e1 * dq2 * ((a6 + xC6) ^ * m6 - I6xx / 0.2e1 + I6yy / 0.2e1 + I6zz / 0.2e1) * cos(q4) 0.2e1 * sin(q4) * ((a6 + xC6) ^ * m6 - I6xx / 0.2e1 + I6yy / 0.2e1 + I6zz / 0.2e1) * dq1 * sin(q2) + 0.2e1 * dq5 * (zC6 ^ * m6 + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1)) * sin(q5) / 0.2e1 + ((0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq1 * sin(q2) * cos(q4) - 0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq2 * sin(q4) + 0.2e1 * a5 * m6 * (a6 + xC6) * (cos(q2) * dq1 + dq4)) * sin(q6) - 0.2e1 * (cos(q2) * dq1 + dq4) * (zC6 * (a6 + xC6) * m6 - I6xz)) * cos(q5) ^ / 0.2e1 + dq5 * sin(q6) * (m6 * yC6 * zC6 - I6yz) * cos(q5) + (-0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq1 * sin(q2) * cos(q4) + 0.2e1 * (yC6 * (a6 + xC6) * m6 I6xy) * dq2 * sin(q4) - 0.2e1 * zC6 * d5 * m6 * (cos(q2) * dq1 + dq4)) * sin(q6) / 0.2e1 + (cos(q2) * dq1 + dq4) * (zC6 * (a6 + xC6) * m6 I6xz); c65 = (0.2e1 * (sin(q2) * dq1 * cos(q4) - dq2 * sin(q4)) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz) * cos(q5) + 0.2e1 * luan an 233 (cos(q2) * dq1 + dq4) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 I6xx + I6zz) * sin(q5) + 0.4e1 * (zC6 * (a6 + xC6) * m6 - I6xz) * (dq1 * sin(q4) * sin(q2) + cos(q4) * dq2 + dq5)) * cos(q6) ^ / 0.2e1 + ((0.4e1 * (sin(q2) * dq1 * cos(q4) - dq2 * sin(q4)) * (zC6 * (a6 + xC6) * m6 - I6xz) * cos(q5) - 0.4e1 * (cos(q2) * dq1 + dq4) * (zC6 * (a6 + xC6) * m6 - I6xz) * sin(q5) + 0.2e1 * (dq1 * sin(q4) * sin(q2) + cos(q4) * dq2 + dq5) * ((xC6 + zC6 + a6) * (xC6 - zC6 + a6) * m6 - I6xx + I6zz)) * sin(q6) + (0.2e1 * a5 * sin(q2) * m6 * dq1 * (a6 + xC6) * cos(q4) - 0.2e1 * a5 * dq2 * m6 * (a6 + xC6) * sin(q4) - 0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * (cos(q2) * dq1 + dq4)) * cos(q5) + (0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq1 * sin(q2) * cos(q4) - 0.2e1 * (yC6 * (a6 + xC6) * m6 - I6xy) * dq2 * sin(q4) + 0.2e1 * a5 * m6 * (a6 + xC6) * (cos(q2) * dq1 + dq4)) * sin(q5) + 0.2e1 * a5 * zC6 * m6 * (dq1 * sin(q4) * sin(q2) + cos(q4) * dq2 + dq5)) * cos(q6) / 0.2e1 + ((0.2e1 * a5 * zC6 * cos(q4) * sin(q2) * m6 * dq1 + 0.2e1 * a5 * dq2 * zC6 * sin(q4) * m6 + 0.2e1 * (m6 * yC6 * zC6 - I6yz) * (cos(q2) * dq1 + dq4)) * cos(q5) + (-0.2e1 * sin(q2) * dq1 * (m6 * yC6 * zC6 - I6yz) * cos(q4) + 0.2e1 * dq2 * (m6 * yC6 * zC6 - I6yz) * sin(q4) - 0.2e1 * a5 * zC6 * m6 * (cos(q2) * dq1 + dq4)) * sin(q5) + 0.2e1 * a5 * m6 * (a6 + xC6) * (dq1 * sin(q4) * sin(q2) + cos(q4) * dq2 + dq5)) * sin(q6) / 0.2e1 + (sin(q2) * dq1 * cos(q4) - dq2 * sin(q4)) * (zC6 ^ * m6 + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1) * cos(q5) + (zC6 ^ * m6 + I6xx / 0.2e1 + I6yy / 0.2e1 - I6zz / 0.2e1) * (cos(q2) * dq1 + dq4) * sin(q5) - (zC6 * (a6 + xC6) * m6 - I6xz) * (dq1 * sin(q4) * sin(q2) + cos(q4) * dq2 + dq5); c66 ; luan an ... Luận án với đề tài: ? ?Nghiên cứu động lực học thi? ??t bị khoan xoay đập lắp máy đào phục vụ thi công hầm độ vừa nhỏ? ?? nhằm tạo sở khoa học cho việc hoàn thi? ??n thi? ??t bị có, phục vụ thi? ??t kế thi? ??t bị. ..BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG HỌC VIỆN KỸ THUẬT QUÂN SỰ BÙI VĂN TRẦM NGHIÊN CỨU ĐỘNG LỰC HỌC THI? ??T BỊ KHOAN XOAY ĐẬP LẮP TRÊN MÁY ĐÀO PHỤC VỤ THI CÔNG HẦM KHẨU ĐỘ VỪA VÀ NHỎ Chuyên ngành... thác thi? ??t bị Mục đích nghiên cứu Mục đích nghiên cứu luận án xây dựng quy trình khảo sát động học động lực học hệ giá đỡ thi? ??t bị khoan lắp máy đào Việt Nam chế tạo thi công đường hầm độ vừa nhỏ