Khảo sát động lực học robot cấu trúc song song

NGUYỄN XUÂN HỒNG BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - NGUYỄN XUÂN HỒNG CƠ HỌC KỸ THUẬT KHẢO SÁT ĐỘNG LỰC HỌC ROBOT CẤU TRÚC SONG SONG LUẬN VĂN THẠC SỸ KHOA HỌC CƠ HỌC KỸ THUẬT KHOÁ 2008-2010 HÀ NỘI – 2010 MỤC LỤC Trang DANH MỤC CÁC KÝ HIỆU VÀ CHỮ VIẾT TẮT III DANH MỤC CÁC HÌNH VẼ IV LỜI NÓI ĐẦU VI CHƯƠNG I: TỔNG QUAN 1.1 Tổng quan Robot 1.1.1 Sơ lược lịch sử phát triển công nghiệp Robot 1.1.2 Robot công nghiệp 1.1.3 Cấu trúc khớp động học phân bố khớp động học 1.1.4 Số bậc tự robot 1.2 Robot cấu trúc song song 11 1.2.1 Về cấu trúc động học robot song song 11 1.2.2 Phân loại robot cấu trúc song song 12 1.2.3 Ưu nhược điểm robot song song 16 1.2.4 Ứng dụng robot song song 17 1.3 Cơ sở khảo sát động học robot cấu trúc song song 18 1.3.1 Lựa chọn cấu trúc robot song song 18 1.3.2 Thiết lập phương trình động học 19 1.3.3 Bài toán vị trí robot cấu trúc song song 27 1.3.4 Bài toán vận tốc robot cấu trúc song song 28 1.3.5 Bài toán gia tốc robot cấu trúc song song 30 CHƯƠNG II: CƠ SƠ LÝ THUYẾT KHẢO SÁT ĐỘNG LỰC HỌC RÔ BỐT CẤU TRÚC SONG SONG 32 2.1 Các phương pháp khảo sát động lực học robot 32 2.1.1 Các phương trình Newton-Euler hệ nhiều vật 32 2.1.2 Phương trình Lagrange loại hai hệ nhiều vật 35 2.2 Hệ phương trình động lực học robot cấu trúc song 37 I 2.3 Khảo sát đại lượng động lực robot cấu trúc song song 39 2.4 Giải hệ phương trình vi phân chuyển động robot song song 47 CHƯƠNG III: TÍNH TỐN MÔ PHỎNG 50 3.1 Sơ đồ giải thuật tính tốn 50 3.2 Các chương trình tính tốn 51 3.2.1 Bài toán động học 53 3.2.2 Kết toán động học 55 3.2.3 Bài toán động lực học 59 3.2.4 Kết toán động lực học 64 3.3 Mô hoạt động 65 3.3.1 Sơ đồ khối modul chương trình 65 3.3.2 Thiết kế mơ hình 3D 66 3.3.3 Xử lý quỹ đạo đường cong phức tạp 71 3.3.4 Chương trình mơ 76 KẾT LUẬN 82 TÀI LIỆU THAM KHẢO 83 PHỤ LỤC 84 Mã nguồn chương trình động học động lực học MAPLE 84 Mã nguồn chương trình mô robot Visual Studio C++ 87 II DANH MỤC CÁC KÝ HIỆU VÀ CHỮ VIẾT TẮT DOF RPS RRR SPS SPC Cα,Cϕ,Cψ Sα,Sϕ,Sψ C(rotx) C(roty) C(rotz) S(rotx) S(roty) S(rotz) MFC ARX OpenGL Degree of freedom Revolute - Prismatic - Spherical Revolute - Revolute - Revolute Spherical - Prismatic - Spherical Spherical - Prismatic - Cardan cosα, cosϕ, cosψ sinα, sinϕ, sinψ cos(rotx) cos(roty) cos(rotz) sin(rotx) sin(roty) sin(rotz) Microsoft Foundation Class AutoCAD Runtime Extension Open Graphic Library III DANH MỤC CÁC HÌNH VẼ Trang Hình 1.1: Khớp quay Hình 1.2: Khớp lăng trụ Hình 1.3: Khớp trụ Hình 1.4: Khớp ren Hình 1.5: Khớp cầu Hình 1.6: Khớp phẳng Hình 1.7: Khớp Cardan Hình 1.8: Phân bố khớp Bản lề – Cầu – Bàn lề Hình 1.9: Phân bố khớp Cầu – Lăng trụ – Cầu Hình 1.10: Phân bố khớp Bản lề – Lặng trụ – Cầu Hình 1.11 Robot song song khơng gian Stewart – Gough 11 Hình 1.12 Robot song song Manipulator Danhilovsky 12 Hình 1.13 Robot song song phẳng 3RRR 14 Hình 1.14 Robot song song không gian 3RPS 15 Hình 1.15 Robot song song khơng gian sáu bậc tự 18 Hình 1.16 Sơ đồ bố trí hệ trục tọa độ 20 Hình 1.17 Sơ đồ phép chuyển hệ tọa độ 21 Hình 1.18: Sơ đồ chuyển hệ tọa độ từ giá cố định khớp Cardan 22 Hình 1.19: Sơ đồ chuyển hệ tọa độ khớp Cardan 23 Hình 1.20: Sơ đồ chuyển hệ tọa độ khớp lăng trụ 24 Hình 1.21: Sơ đồ chuyển hệ tọa độ từ giá cố định lên bàn máy động 25 Hình 1.22: Sơ đồ chuyển hệ tọa độ từ bàn máy động khớp cầu 26 Hình 2.1: Sơ đồ cấu trúc chân robot 42 Hình 3.1: Sơ đồ thuật giải tính động lực học 50 Hình 3.2: Đồ thị quỹ đạo chuyển động tâm bàn máy robot 55 Hình 3.3: Đồ thị tọa độ tâm bàn máy 56 IV Hình 3.4: Đồ thị vận tốc tâm bàn máy 56 Hình 3.5: Đồ thị gia tốc tâm bàn máy 57 Hình 3.6: Đồ thị độ dài chân 57 Hình 3.7: Đồ thị chung vận tốc chuyển động tịnh tiến chân 58 Hình 3.8: Đồ thị chung gia tốc chuyển động tịnh tiến chân 58 Hình 3.9: Đồ thị chung lực 64 Hình 3.10: Sơ đồ khối chương trình mơ robot song song 65 Hình 3.11: Giao diện SolidWorks 66 Hình 3.12: Các loại tài liệu SolidWorks 67 Hình 3.13: Hình vẽ 3D vẽ chi tiết giá cố định 68 Hình 3.14: Hình vẽ 3D vẽ chi tiết bàn máy di động 68 Hình 3.15: Hình vẽ khớp Cardan 68 Hình 3.16: Hình vẽ khớp cầu 69 Hình 3.17: Bản vẽ chi tiêt khớp cầu 69 Hình 3.18: Hình vẽ 3D vẽ chi tiết khớp lăng trụ 69 Hình 3.19: Xuất liệu mơ hình định dạng STL 70 Hình 3.20: Giao diện Autocad 71 Hình 3.21: Sơ đồ sở liệu vẽ AutoCad 73 Hình 3.22: Biên dạng đường cong phức tạp AutoCad 75 Hình 3.23: Giao diện chương trình RobotSimulation cho Parallel robot SPC 76 Hình 3.24: Biểu đồ UseCase 77 Hình 3.24: Biểu đồ Class 78 Hình 3.26: Chức OpenRobotFile 79 Hình 3.27: Định dạng file rob 80 Hình 3.28: Chức OpenDataFile 81 V LỜI NÓI ĐẦU R ô bốt cấu trúc song song (Parallel robot) tổ hợp có cấu trúc gồm khâu động (bàn máy di động) nối với giá cố định số mạch động học kín (thường gọi chân) Bàn máy động thường phận mang đối tượng công nghệ dụng cụ gia công Bàn máy động dẫn động động đặt khớp chân robot Robot cấu trúc song song quan tâm nghiên cứu đặc biệt năm gần ngày ứng dụng rộng rãi sản xuất đời sống Bởi khả chịu tải cao, thực thao tác phức tạp với độ xác cao Nghiên cứu, tính tốn, thiết kế, mơ robot cấu trúc song song trước hết phải xuất phát từ toán cấu trúc cấu, toán động học, động lực học, tốn mơ chuyển động theo chương trình Trong phạm vi luận văn tốt nghiệp học viên ngành Cơ học kỹ thuật, hướng dẫn tận tình thầy PGS.TS Phan Bùi Khơi Viện Cơ khí Trường Đại học Bách khoa Hà nội Tác giả lựa chọn đề tài: “Khảo sát động lực học robot cấu trúc song song” phù hợp với khả thời gian thực Mục đích đề tài trước hết tìm hiểu, làm quen với việc nghiên cứu khoa học áp dụng kiến thức trang bị vào thực tế Mặt khác việc nghiên cứu, thực đề tài góp phần nâng cao kiến thức, tiếp cận với vấn đề thực tế Luận văn chia thành chương: - Chương 1: Tổng quan Trong chương tác giả giới thiệu khái niệm robot, robot cấu trúc song song vào tìm hiểu toán động học robot cấu trúc song song - Chương 2: Cơ sở lý thuyết khảo sát động lực học robot cấu trúc song song VI Trong chương tác giả giới thiệu số phương pháp khảo sát động lực học robot áp dụng nguyên lý phù hợp vào việc khảo sát toán động lực học robot cấu trúc song song - Chương 3: Tính tốn mô robot cấu trúc song song Chương tác giả đưa sơ đồ thuật giải giải toán động lực học robot cấu trúc song song đồng thời lập chương trình mơ chuyển động robot cấu trúc song song Tác giả xin chân thành cảm ơn PGS.TS Phan Bùi Khôi tâm huyết hướng dẫn tác giả hoàn thành luận văn Tác giả Nguyễn Xuân Hồng VII CHƯƠNG I: TỔNG QUAN 1.1 Tổng quan Robot 1.1.1 Sơ lược lịch sử phát triển công nghiệp Robot Nhu cầu nâng cao suất chất luợng sản phẩm ngày cao đòi hỏi ứng dụng rộng rãi phương tiện tự động hoá sản xuất Xu hướng tạo dây chuyền thiết bị tự động có tính linh hoạt cao hình thành Những thiết bị thay dần máy tự động "cứng'' đáp ứng cơng việc định trơng lúc thị truờng ln địi hỏi thay đổi mặt hàng chủng loại, kích cỡ tính v.v Vì ngày tăng nhanh nhu cầu ứng dụng robot để tạo hệ thống sản xuất tự động linh hoạt Thuật ngữ "robot" lần xuất năm 1922 tác phẩm "Russum's Universal Robot" Karel Capek Theo tiếng Séc robot người làm tạp dịch Trong tác phẩm này, nhân vật Rossum trai tạo máy gần giống người để hầu hạ người Ước mơ viễn tưởng Karel Capek dần trở thành thực Nền công nghiệp Robot đời không ngừng phát triển Sự phát triển Robot chia thành thời kì sau: a) Thời kì đầu 1947-1961 Năm 1947, Raymond Goetz đưa ý tưởng sản xuất hệ robot chép hình Đến năm 1948, hệ M1 đời không ngừng cải thiện để hoàn chỉnh Đến năm 1954, hệ M4 đời dựa sở song song cải tiến mặt học phương thức truyền động phần điều khiển tay máy Năm 1954, người Mĩ lao vào dự án lớn hàng không, thúc đẩy sử dụng lượng hạt nhân Nhóm General Electric tạo hệ thống điều khiển từ xa có hiệu cao, với mẫu Handyman (1958) Trong giai đoạn đời phát triển máy tính điện tử tạo tiền quan trọng thúc đẩy phát triển mạnh mẽ robot Những sở lĩnh vực điều khiển từ xa bắt đầu xuất giai đoạn b) Thời kì phát triển Robot 1962-1975 Năm 1961, sản phẩm robot công nghiệp xuất có tên Vowtan cơng ty AMF-Mỹ Cùng năm này, sản phẩm mang tên Unimate lần đưa vào sử dụng nhà máy General Motors Trong năm tiếp theo, người máy Daimber Benz xuất Đức Trong năm tiếp theo, nước khác bắt đầu sản xuất ứng dụng robot sản xuất với quy mô ngày mở rộng: Anh từ năm 1967, Thuỵ Điển từ năm 1968, Pháp từ năm 1972, Italia từ năm 1973 c) Thời kì từ năm 1975 Từ năm 1975, xu hướng phát triển robot nâng cao tính sử dụng việc điều khiển máy tính điện tử gắn thêm cảm biến để nhận biết xử lí mơi trường làm việc Các nhà khoa học truờng đại học Stanford chế tạo thành công robot lắp ráp loại máy bơm nước đièu khiển máy vi tính PDP-10 sở xử lí thơng tin từ cảm biến lực hình ảnh Cùng thời gian này, IBM chế tạo thành cơng robot có cảm biến xúc giác cảm biến lực điều khiển máy vi tính dùng để lắp ráp máy có 20 cụm chi tiết Vào giai đoạn này, nhiều nước tiến hành cơng trình ngiên cứu chế tạo loại robot tổ hợp: tay máy điều khiển máy vi tính có gắn thiết bị cảm biến thiết bi giao tiếp ngưòi - máy Đến năm 80 kỉ 20, phát triển tin học vi điều khiển thúc đẩy công nghiệp robot phát triển mạnh mẽ chất luợng só lượng Cho đến nay, giới có nhiều phịng thí nghiệm, nhiều công ty sản xuất robot với trang thiết bị đại Robot ngày trở nên thông minh gần gũi với người Ngày nay, robot không phục vụ công nghiệp mà ngày sâu vào đời sống hàng ngày người > alpha[1]:=0; alpha[2]:=30; alpha[3]:=120; alpha[4]:=150; alpha[5]:=240; alpha[6]:=270; beta[1]:=-30; beta[2]:=60; beta[3]:=90; beta[4]:=180; beta[5]:=210; beta[6]:=300; for i from by to alpha[i]:= alpha[i]*Pi/180; beta[i]:= beta[i]*Pi/180; R1[i]:=889; R2[i]:=589; H1[i]:=0; H2[i]:=0; end do; # KHOI DONG CHE DO GHI DU LIEU RA FILE DAT > fFileKetqua:=fopen("c:/Robot6RPC.dat",WRITE,TEXT); # NAP CAC GIA TRI BUOC NHAY, THOI GIAN, SO BUOC NHAY > dt:=0.003; > TT:=0.60377; > t0:=0.0; > Num:=round(TT/dt); > omega4:=11.77; # NAP CAC GIA TRI KHOI DAU – DIEU KIEN DAU > phi10[0]:=0.2838131;phi20[0]:=-0.2109253;phi30[0]:=0.3484228;phi40[0]:=0.2230167;phi50[0]:=0.1881834;phi60[0]:=-0.3990176; psi10[0]:=-0.2425446;psi20[0]:=-0.2440316;psi30[0]:=-0.3628818;psi40[0]:=0.4273298;psi50[0]:=-0.4064646;psi60[0]:=-0.3433798; d10[0]:=1078.01700;d20[0]:=1143.39000;d30[0]:=1243.65300;d40[0]:=1165.02100;d50[0]:=1113.36 600;d60[0]:=1133.03100; > XXp[1]:=120; YYp[1]:=0; ZZp[1]:=1034; rrotx[1]:=0.1; rroty[1]:=0; rrotz[1]:=0; XXp[2]:=119.9156; YYp[2]:=4.498945; ZZp[2]:=1032.944; rrotx[2]:=0.09998875; rroty[2]:=0.001499944;rrotz[2]:=0.001499775; # GIAI HE PHUONG TRINH THEO SO BUOC NHAY Num DE DI HET QUY DAO BIEN DANG > for i from by to Num # CHUYEN DOI BIEN DE VIEC GIAI PHUONG TRINH O NHIEU BUOC phi[1]:=phi1[i];psi[1]:=psi1[i];d[1]:=d1[i]; phi[2]:=phi2[i];psi[2]:=psi2[i];d[2]:=d2[i]; phi[3]:=phi3[i];psi[3]:=psi3[i];d[3]:=d3[i]; phi[4]:=phi4[i];psi[4]:=psi4[i];d[4]:=d4[i]; phi[5]:=phi5[i];psi[5]:=psi5[i];d[5]:=d5[i]; phi[6]:=phi6[i];psi[6]:=psi6[i];d[6]:=d6[i]; # TANG GIA TRI BUOC NHAY t:=t0+i*dt; # NAP GIA TRI CUA BIEN DANG Xp:=R2[1]*cos(omega4*t); Yp:=R2[2]*sin(omega4*t); Zp:=R2[3]*(100+0.01*cos(omega4*t+Pi/3)); rotx:=R2[4]*cos(5*t); roty:=R2[5]*sin(5*t); rotz:=R2[6]*cos(5*t)*sin(5*t); # GIAI HE 18 PHUONG TRINH VOI 18 AN SO T[i]:=fsolve({f[1],f[2],f[3],f[4],f[5],f[6],f[7],f[8],f[9],f[10],f[11],f[12],f[13],f[14],f[ 15],f[16],f[17],f[18]}, { 85 phi1[i]=phi10[i-1],psi1[i]=psi10[i-1],d1[i]=d10[i-1], phi2[i]=phi20[i-1],psi2[i]=psi20[i-1],d2[i]=d20[i-1], phi3[i]=phi30[i-1],psi3[i]=psi30[i-1],d3[i]=d30[i-1], phi4[i]=phi40[i-1],psi4[i]=psi40[i-1],d4[i]=d40[i-1], phi5[i]=phi50[i-1],psi5[i]=psi50[i-1],d5[i]=d50[i-1], phi6[i]=phi60[i-1],psi6[i]=psi60[i-1],d6[i]=d60[i-1]}); # DANG KY NHAN KET QUA assign(T[i]): phi10[i]:=phi1[i];psi10[i]:=psi1[i];d10[i]:=d1[i]; phi20[i]:=phi2[i];psi20[i]:=psi2[i];d20[i]:=d2[i]; phi30[i]:=phi3[i];psi30[i]:=psi3[i];d30[i]:=d3[i]; phi40[i]:=phi4[i];psi40[i]:=psi4[i];d40[i]:=d4[i]; phi50[i]:=phi5[i];psi50[i]:=psi5[i];d50[i]:=d5[i]; phi60[i]:=phi6[i];psi60[i]:=psi6[i];d60[i]:=d6[i]; # IN KET QUA RA FILE DU LIEU fprintf(fFileKetqua,"\n%.6lf,%.6lf,%.6lf,%.6lf,%.6lf,%.6lf,%.6lf,%.6lf,%.6lf,%.6lf,%.6lf,% 6lf,%.6lf,%.6lf,%.6lf,%.6lf,%.6lf,%.6lf,%.6lf,%.6lf,%.6lf", phi10[i],psi10[i],d10[i], phi20[i],psi20[i],d20[i], phi30[i],psi30[i],d30[i], phi40[i],psi40[i],d40[i], phi50[i],psi50[i],d50[i], phi60[i],psi60[i],d60[i], Xp,Yp,Zp, Rotx,Roty,Rotz); end do; # DAY HET DU LIEU RA FILE KET QUA VA KET THUC QUA TRINH GIAI > fflush(fFileKetqua); > fclose(fFileKetqua); 86 Mã nguồn chương trình mơ robot Visual Studio C++ // Model.h: interface for the CModel class ////////////////////////////////////////////////////////////////////// #pragma once #include "gl\gl.h" #include "gl\glu.h" #define MAU_OCDEN #define MAU_OCTRANG #define MAU_NHOM #define MAU_DONGCO #define MAU_TRUCVIT #define MAU_TRUCTRON #define MAU_CHOT #define MAU_BANHRANG class wPoint { public: double px; double py; double pz; wPoint(){ px = 0.0; py = 0.0; pz = 0.0; } void copyFrom(wPoint* pCopy){ if(pCopy) { px = pCopy->px; px = pCopy->px; px = pCopy->px; } } }; class wVector { public: double vx; double vy; double vz; wVector() { vx = 0.0; vy = 0.0; vz = 0.0; } void copyFrom(wVector* pCopy){ if(pCopy) { vx = pCopy->vx; vy = pCopy->vy; vz = pCopy->vz; } } }; class wVertice { public: wVector normal; wPoint pt1; wPoint pt2; wPoint pt3; void copyFrom(wVertice* pCopy){ if(pCopy) { normal.copyFrom(&pCopy->normal); pt1.copyFrom(&pCopy->pt1); 87 pt2.copyFrom(&pCopy->pt2); pt3.copyFrom(&pCopy->pt3); } } }; class CModel { public: CModel(); virtual ~CModel(); protected: vector m_vtVertice; static int snNumberObject; int m_nDisplayList; public: CString m_sFileName; public: wPoint m_pOrigin; wVector m_vRotVec; double m_dRotAngle; double bool m_dAlpha; m_bBlend; double double coorMax[3]; coorMin[3]; int m_nRed; int m_nGreen; int m_nBlue; float m_nObjectColorAmbient[3]; float m_nObjectColorDiffuse[3]; float m_nObjectColorSpecular[3]; float m_dShininess; public: void SetRotAng(double const & angle); void SetOrigin(double const& orix,double const& oriy,double const& oriz); void SetRotVec(double const&vecx,double const&vecy,double const&vecz); void InitModel(const CString& sFileName, const bool& bBuildModel = true); void BuildMaterial(); void BuildModel(); void ReadFile(); void PreDraw(); void Draw(); void DrawWithCenAngle( double angle, double x, double y, double z, double vecx=0.0, double vecy=0.0, double vecz=1.0, double scalex=1.0,double scaley=1.0,double scalez=1.0); // void SetColorIndex(const int& nColorIndex); void SetMaterialValue(GLfloat ObjectColorAmbient[3], GLfloat ObjectColorDiffuse[3], GLfloat ObjectColorSpecular[3], GLfloat Shininess); public: void copyDataFrom(CModel* pModel); }; ############################################## ############################################## // Model.cpp: implementation of the CModel class // ////////////////////////////////////////////////////////////////////// #include "stdafx.h" #include "Model.h" #include "resource.h" #include 88 #include #include #include using namespace std; #include " / /Util/Util.h" #define PI 3.14159265358979323846 ////////////////////////////////////////////////////////////////////// // Construction/Destruction ////////////////////////////////////////////////////////////////////// int CModel::snNumberObject = 1; CModel::CModel() { snNumberObject += 1; m_nDisplayList = snNumberObject; m_sFileName = _T(""); m_nObjectColorAmbient[0] = 0.0215f; m_nObjectColorAmbient[1] = 0.1745f; m_nObjectColorAmbient[2] = 0.0215f; m_nObjectColorDiffuse[0] = 0.07568f; m_nObjectColorDiffuse[1] = 0.61424f; m_nObjectColorDiffuse[2] = 0.07568f; m_nObjectColorSpecular[0] = 0.633f; m_nObjectColorSpecular[1] = 0.727811f; m_nObjectColorSpecular[2] = 0.633f; m_dShininess = 76.8f; m_dAlpha = 0.0; m_bBlend = false; SetOrigin(0.0,0.0,0.0); SetRotAng(0.0); SetRotVec(0.0,0.0,1.0); } CModel::~CModel() { for(int i = 0; i < m_vtVertice.size(); i++){ if(m_vtVertice[i]) delete m_vtVertice[i]; } m_vtVertice.clear(); } void CModel::SetRotAng(double const &angle) { m_dRotAngle = angle; } void CModel::SetOrigin(double const &orix, double const &oriy, double const &oriz) { m_pOrigin.px = orix; m_pOrigin.py = oriy; m_pOrigin.pz = oriz; } void CModel::SetRotVec(double const&vecx,double const&vecy,double const&vecz) { m_vRotVec.vx = vecx; m_vRotVec.vy = vecy; m_vRotVec.vz = vecz; } void CModel::InitModel(const CString& sFileName, const bool& bBuildModel) { m_sFileName = sFileName; if(bBuildModel){ BuildModel(); } } 89 void CModel::BuildModel() { ReadFile(); PreDraw(); } void CModel::BuildMaterial() { glMaterialfv(GL_FRONT,GL_AMBIENT,m_nObjectColorAmbient); glMaterialfv(GL_FRONT,GL_DIFFUSE,m_nObjectColorDiffuse); glMaterialfv(GL_FRONT,GL_SPECULAR,m_nObjectColorSpecular); glMaterialf(GL_FRONT,GL_SHININESS,m_dShininess); } void CModel::ReadFile() { CString sExt = CUtil::getFileExt(m_sFileName); sExt.MakeUpper(); if(sExt == _T(".XSTL")) { FILE *fp = NULL; TCHAR buffer_temp[100] = _T("\0"); fp = _tfopen(m_sFileName, _T("rt")); if(fp) { { wVertice* pVertice = new wVertice; // normal x y z _ftscanf(fp, _T("%lf %lf %lf"), &pVertice->normal.vx, &pVertice>normal.vy, &pVertice->normal.vz); _ftscanf(fp, _T("%lf %lf %lf"), &pVertice->pt1.px, &pVertice->pt1.py, &pVertice->pt1.pz); _ftscanf(fp, _T("%lf %lf %lf"), &pVertice->pt2.px, &pVertice->pt2.py, &pVertice->pt2.pz); _ftscanf(fp, _T("%lf %lf %lf"), &pVertice->pt3.px, &pVertice->pt3.py, &pVertice->pt3.pz); m_vtVertice.push_back(pVertice); } while(!feof(fp)); fclose(fp); } } } void CModel::PreDraw() { glNewList(m_nDisplayList, GL_COMPILE); glBegin(GL_TRIANGLES); for(int i = 0; i < int(m_vtVertice.size()); i++) wVertice* pVertice = m_vtVertice[i]; { glNormal3d(pVertice->normal.vx, pVertice->normal.vy, pVertice->normal.vz); glVertex3d(pVertice->pt1.px, pVertice->pt1.py, pVertice->pt1.pz); glVertex3d(pVertice->pt2.px, pVertice->pt2.py, pVertice->pt2.pz); glVertex3d(pVertice->pt3.px, pVertice->pt3.py, pVertice->pt3.pz); } glEnd(); glDisable(GL_NORMALIZE); glEndList(); } void CModel::Draw() { DrawWithCenAngle(m_dRotAngle, m_pOrigin.px, m_pOrigin.py, m_pOrigin.pz, m_vRotVec.vx, m_vRotVec.vy, m_vRotVec.vz); } void CModel::DrawWithCenAngle( double angle, double x, double y, double z, double vecx, double vecy, double vecz, double scalex,double scaley,double scalez) 90 { if((scalex!=1.0)||(scaley!=1.0)||(scalez!=1.0)) glEnable(GL_NORMALIZE); glPushMatrix(); if(m_bBlend){ glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA,GL_ONE); } else { glDisable(GL_BLEND); } glCallList(m_nDisplayList); glPopMatrix(); } void CModel::SetMaterialValue(GLfloat ObjectColorAmbient[3], GLfloat ObjectColorDiffuse[3], GLfloat ObjectColorSpecular[3], GLfloat Shininess) { m_nObjectColorAmbient[0]=ObjectColorAmbient[0]; m_nObjectColorAmbient[1]=ObjectColorAmbient[1]; m_nObjectColorAmbient[2]=ObjectColorAmbient[2]; m_nObjectColorDiffuse[0]=ObjectColorDiffuse[0]; m_nObjectColorDiffuse[1]=ObjectColorDiffuse[1]; m_nObjectColorDiffuse[2]=ObjectColorDiffuse[2]; m_nObjectColorSpecular[0]=ObjectColorSpecular[0]; m_nObjectColorSpecular[1]=ObjectColorSpecular[1]; m_nObjectColorSpecular[2]=ObjectColorSpecular[2]; m_dShininess=Shininess; } void CModel::copyDataFrom(CModel* pModel) { if(pModel){ m_sFileName = pModel->m_sFileName; for(int i = 0; i < pModel->m_vtVertice.size(); i++){ wVertice* pVertice = new wVertice(); pVertice->copyFrom(pModel->m_vtVertice[i]); m_vtVertice.push_back(pVertice); } } } ############################################## ############################################## // Mechanic.h: interface for the CMechanic class ////////////////////////////////////////////////////////////////////// #include "Model.h" #pragma once #define NUM_OBJ #define NUM_LEG class CMechanicData { public: CMechanicData(){ m_dXp = 0.0; m_dYp = 0.0; m_dZp = 0.0; m_dRotx = 0.0; m_dRoty = 0.0; m_dRotz = 0.0; for(int i = 0; i < NUM_LEG; i++) { m_dPhi[i] = 0.0; m_dPsi[i] = 0.0; 91 m_dLen[i] = 0.0; } } public: double double double double double double double double double }; m_dXp; m_dYp; m_dZp; m_dRotx; m_dRoty; m_dRotz; m_dPhi[NUM_LEG]; m_dPsi[NUM_LEG]; m_dLen[NUM_LEG]; class CLeg { public: CLeg(); virtual ~CLeg(); public: CModel m_Object[NUM_OBJ]; public: // Thong so cua chan robot double m_dAlpha; double m_dR1; double m_dH11; double m_dH12; double m_dBeta; double m_dR2; double m_dH21; double m_dH22; public: void InitModel(); void InitPosition(); void BuildMaterial(); void copyDataFrom(CLeg* pLeg); void SetColorIndex(const int& nColorIndex); }; class CMechanic { public: CMechanic(); virtual ~CMechanic(); public: double m_dXmax,m_dYmax,m_dZmax; double m_dXmin,m_dYmin,m_dZmin; double m_ddx,m_ddy,m_ddz; double Transval,Scaleval; double m_dTimeMark; double m_dTimeDelta; double m_dDistTotal; double m_dDistSecme; double m_dAnglSecme; double initvec[2];//Vec to khoi gan gia tri dau cho bien can tim CModel m_ModelDe; CModel m_ModelGia; CLeg m_Leg[NUM_LEG]; std::vector m_vtData; public: void InitModel(); void InitPosition(); void Position(double const & dTime,long const & nTime); void Draw(double const & dTime,long const & nTime); 92 void DrawOrbit(int nIndex); bool OpenRobotFile(const CString& sRobotFile); bool OpenDataFile(const CString& sDataFile); }; ############################################## ############################################## // Mechanic.cpp: implementation of the CMechanic class ////////////////////////////////////////////////////////////////////// #include "stdafx.h" #include "Mechanic.h" #include "resource.h" #include "math.h" #include "./ / /Glut/Glut.h" #define PI 3.14159265358979323846 #include "mmsystem.h" #include "./ / /Util/Util.h" CLeg::CLeg() { m_dAlpha = 0.0; m_dR1 = 0.0; m_dH11 = 0.0; m_dH12 = 0.0; m_dBeta = 0.0; m_dR2 = 0.0; m_dH21 = 0.0; m_dH22 = 0.0; } CLeg::~CLeg() { } void CLeg::InitModel() { m_Object[0].InitModel(_T("Robot6RPC/CARDAN1.STL")); m_Object[0].SetColorIndex(MAU_NHOM); m_Object[1].InitModel(_T("Robot6RPC/CARDAN2.STL")); m_Object[1].SetColorIndex(MAU_NHOM); m_Object[2].InitModel(_T("Robot6RPC/CARDAN3.STL")); m_Object[2].SetColorIndex(MAU_NHOM); m_Object[3].InitModel(_T("Robot6RPC/ONG1.STL")); m_Object[3].SetColorIndex(MAU_NHOM); m_Object[4].InitModel(_T("Robot6RPC/ONG2.STL")); m_Object[4].SetColorIndex(MAU_NHOM); m_Object[5].InitModel(_T("Robot6RPC/CAU2.STL")); m_Object[5].SetColorIndex(MAU_NHOM); m_Object[6].InitModel(_T("Robot6RPC/CAU1.STL")); m_Object[6].SetColorIndex(MAU_NHOM); } void CLeg::InitPosition() { for(int i = 0; i < NUM_OBJ; i++){ m_Object[i].SetOrigin(i*100,i*100,0.0); m_Object[i].SetRotAng(0.0); } } void CLeg::BuildMaterial() { for(int i = 0; i < NUM_OBJ; i++){ m_Object[i].BuildMaterial(); } 93 } void CLeg::copyDataFrom(CLeg* pLeg) { if(pLeg){ for(int i = 0; i < NUM_OBJ; i++) { m_Object[i].InitModel(pLeg->m_Object[i].m_sFileName); // m_Object[i].PreDraw(); m_Object[i].SetColorIndex(MAU_CHOT); } } } void CLeg::SetColorIndex(const int& nColorIndex) { for(int i = 0; i < NUM_OBJ; i++){ m_Object[i].SetColorIndex(nColorIndex); } } ////////////////////////////////////////////////////////////////////// // Construction/Destruction ////////////////////////////////////////////////////////////////////// CMechanic::CMechanic() { m_dXmax=0.0; m_dYmax=0.0; m_dZmax=0.0; m_dXmin=0.0; m_dYmin=0.0; m_dZmin=0.0; m_ddx=0.0; m_ddy=0.0; m_ddz=0.0; Transval=120; Scaleval=2; m_dTimeMark=0.0; m_dTimeDelta=10.0; m_dDistTotal=30.0; m_dDistSecme=3.0; m_dAnglSecme=9.0*PI/180.0; } CMechanic::~CMechanic() { for(int i = 0; i < m_vtData.size(); i++) delete m_vtData[i]; m_vtData.clear(); } ////////////////////////////////////////////////////////////////////////// void CMechanic::InitModel() { m_ModelDe.InitModel(_T("Robot6RPC/DE.STL")); m_ModelDe.SetColorIndex(MAU_NHOM); if(NUM_LEG > 0){ m_Leg[0].InitModel(); for(int i = 1; i < NUM_LEG; i++){ m_Leg[i].copyDataFrom(&m_Leg[0]); // m_Leg[i].SetColorIndex(MAU_DONGCO); } } m_Leg[0].SetColorIndex(MAU_CHOT); m_Leg[1].SetColorIndex(MAU_TRUCVIT); m_Leg[2].SetColorIndex(MAU_TRUCTRON); m_Leg[3].SetColorIndex(MAU_BANHRANG); m_Leg[4].SetColorIndex(MAU_OCTRANG); m_Leg[5].SetColorIndex(MAU_NHOM); m_ModelGia.InitModel(_T("Robot6RPC/GIA.STL")); m_ModelGia.SetColorIndex(MAU_DONGCO); SetModelTransform(); 94 InitPosition(); } void CMechanic::InitPosition() { for(int i = 0; i < NUM_LEG; i++){ m_Leg[i].m_dR1 = 1200; m_Leg[i].m_dR2 = 800; m_Leg[i].m_dH11 = 40; m_Leg[i].m_dH12 = 150; m_Leg[i].m_dH21 = 40; m_Leg[i].m_dH22 = 120; } m_Leg[0].m_dAlpha = 0.0; m_Leg[0].m_dBeta = 0.0; m_Leg[1].m_dAlpha = 35.0; m_Leg[1].m_dBeta = 85.0; m_Leg[2].m_dAlpha = 120.0; m_Leg[2].m_dBeta = 120.0; m_Leg[3].m_dAlpha = 155.0; m_Leg[3].m_dBeta = 205.0; m_Leg[4].m_dAlpha = 240.0; m_Leg[4].m_dBeta = 240.0; m_Leg[5].m_dAlpha = 275.0; m_Leg[5].m_dBeta = 325.0; CMechanicData* pData = new CMechanicData(); pData->m_dXp = 0.0; pData->m_dYp = 1924.3224; pData->m_dZp = 0.0; pData->m_dRotx = 0.0; pData->m_dRoty = 0.0; pData->m_dRotz = 50.1787; pData->m_dPhi[0] = 23.7309; pData->m_dPsi[0] = 19.7827; pData->m_dLen[0] = 1535.9218; pData->m_dPhi[1] = 14.3434; pData->m_dPsi[1] = 0.0890; pData->m_dLen[1] = 1334.6560; pData->m_dPhi[2] = 23.7309; pData->m_dPsi[2] = 19.7827; pData->m_dLen[2] = 1535.9218; pData->m_dPhi[3] = 14.3434; pData->m_dPsi[3] = 0.0890; pData->m_dLen[3] = 1334.6560; pData->m_dPhi[4] = 23.7309; pData->m_dPsi[4] = 19.7827; pData->m_dLen[4] = 1535.9218; pData->m_dPhi[5] = 14.3434; pData->m_dPsi[5] = 0.0890; pData->m_dLen[5] = 1334.6560; m_vtData.push_back(pData); } void CMechanic::Draw(double const & dTime,long const & nTime) { int i; Position(dTime,nTime); // Draw DE glPushMatrix(); glTranslated(0.0, 0.0, 0.0); m_ModelDe.BuildMaterial(); m_ModelDe.Draw(); glPopMatrix(); for(int i = 0; i < NUM_LEG; i++){ // DRAW CARDAN1 glPushMatrix(); glRotated(m_Leg[i].m_dAlpha, 0.0, 0.0, 1.0); glTranslated(m_Leg[i].m_dR1, 0.0, 0.0); 95 if(i == 0) m_Leg[i].m_Object[0].SetColorIndex(MAU_OCTRANG); m_Leg[i].m_Object[0].BuildMaterial(); m_Leg[i].m_Object[0].Draw(); // DRAW CARDAN2 glPushMatrix(); glTranslated(0.0, 0.0, 0.0); glRotated(m_vtData[nTime]->m_dPhi[i], 1.0, 0.0, 0.0); m_Leg[i].m_Object[1].BuildMaterial(); m_Leg[i].m_Object[1].Draw(); // DRAW CARDAN3 glPushMatrix(); glRotated(m_vtData[nTime]->m_dPsi[i], 0.0, 1.0, 0.0); m_Leg[i].m_Object[2].BuildMaterial(); m_Leg[i].m_Object[2].Draw(); // DRAW ONG1 glPushMatrix(); glTranslated(0.0, 0.0, m_Leg[i].m_dH12); // DRAW ONG2 glPushMatrix(); glTranslated(0.0, 0.0, m_vtData[nTime]->m_dLen[i]m_Leg[i].m_dH12-m_Leg[i].m_dH22); m_Leg[i].m_Object[4].BuildMaterial(); m_Leg[i].m_Object[4].Draw(); // DRAW CAU1 glPushMatrix(); m_Leg[i].m_Object[5].BuildMaterial(); m_Leg[i].m_Object[5].Draw(); glPopMatrix(); glPopMatrix(); m_Leg[i].m_Object[3].BuildMaterial(); m_Leg[i].m_Object[3].Draw(); glPopMatrix(); glPopMatrix(); glPopMatrix(); glPopMatrix(); } for(int i = 1; i < nTime; i++){ DrawOrbit(i); } // DRAW GIA glPushMatrix(); glTranslated(m_vtData[nTime]->m_dXp, m_vtData[nTime]->m_dYp, m_vtData[nTime]->m_dZp); glRotated(m_vtData[nTime]->m_dRotx, 1.0, 0.0, 0.0); glRotated(m_vtData[nTime]->m_dRoty, 0.0, 1.0, 0.0); glRotated(m_vtData[nTime]->m_dRotz, 0.0, 0.0, 1.0); for(int i = 0; i < NUM_LEG; i++){ // DRAW CAU1 glPushMatrix(); glRotated(m_Leg[i].m_dBeta, 0.0, 0.0, 1.0); glTranslated(m_Leg[i].m_dR2, 0.0, 0.0); m_Leg[i].m_Object[6].BuildMaterial(); m_Leg[i].m_Object[6].Draw(); glPopMatrix(); } m_ModelGia.BuildMaterial(); m_ModelGia.Draw(); glPopMatrix(); } void CMechanic::DrawOrbit(int nIndex) { // DRAW GIA glPushMatrix(); glTranslated(m_vtData[nIndex]->m_dXp, m_vtData[nIndex]->m_dYp, >m_dZp+m_Leg[0].m_dH21+m_Leg[0].m_dH22); glRotated(m_vtData[nIndex]->m_dRotx, 1.0, 0.0, 0.0); glRotated(m_vtData[nIndex]->m_dRoty, 0.0, 1.0, 0.0); glRotated(m_vtData[nIndex]->m_dRotz, 0.0, 0.0, 1.0); glColor3f(1.0f,0.0f,0.0f); 96 m_vtData[nIndex]- float m_nObjectColorAmbient[3]; float m_nObjectColorDiffuse[3]; float m_nObjectColorSpecular[3]; float m_dShininess=20.8f; m_nObjectColorDiffuse[0]=1.0f; m_nObjectColorDiffuse[1]=0.0f; m_nObjectColorDiffuse[2]=0.0f; m_nObjectColorAmbient[0]=0.5; m_nObjectColorAmbient[1]=0.5; m_nObjectColorAmbient[2]=0.5; m_nObjectColorSpecular[0]=0.5; m_nObjectColorSpecular[1]=0.5; m_nObjectColorSpecular[2]=0.5; glutSolidSphere(14,25,25); glPopMatrix(); } void CMechanic::Position(const double &dTime,long const & nTime) { } bool CMechanic::OpenRobotFile(const CString& sRobotFile) { FILE *fp = NULL; TCHAR buffer_temp[100] = _T("\0"); TCHAR text[50] = _T("\0"); TCHAR text2[50] = _T("\0"); fp = _tfopen(sRobotFile, _T("rt")); if(fp){ _fgetts(buffer_temp, 100, fp); // File comment _fgetts(buffer_temp, 100, fp); _stscanf(buffer_temp, _T("%s %lf %lf %lf %lf %lf %lf"), &text, &m_Leg[0].m_dR1, &m_Leg[1].m_dR1, &m_Leg[2].m_dR1, &m_Leg[3].m_dR1, &m_Leg[4].m_dR1, &m_Leg[5].m_dR1); _fgetts(buffer_temp, 100, fp); _stscanf(buffer_temp, _T("%s %lf %lf %lf %lf %lf %lf"), &text, &m_Leg[0].m_dR2, &m_Leg[1].m_dR2, &m_Leg[2].m_dR2, &m_Leg[3].m_dR2, &m_Leg[4].m_dR2, &m_Leg[5].m_dR2); _fgetts(buffer_temp, 100, fp); _stscanf(buffer_temp, _T("%s %lf %lf %lf %lf %lf %lf"), &text, &m_Leg[0].m_dH11, &m_Leg[1].m_dH11, &m_Leg[2].m_dH11, &m_Leg[3].m_dH11, &m_Leg[4].m_dH11, &m_Leg[5].m_dH11); _fgetts(buffer_temp, 100, fp); _stscanf(buffer_temp, _T("%s %lf %lf %lf %lf %lf %lf"), &text, &m_Leg[0].m_dH12, &m_Leg[1].m_dH12, &m_Leg[2].m_dH12, &m_Leg[3].m_dH12, &m_Leg[4].m_dH12, &m_Leg[5].m_dH12); _fgetts(buffer_temp, 100, fp); _stscanf(buffer_temp, _T("%s %lf %lf %lf %lf %lf %lf"), &text, &m_Leg[0].m_dH21, &m_Leg[1].m_dH21, &m_Leg[2].m_dH21, &m_Leg[3].m_dH21, &m_Leg[4].m_dH21, &m_Leg[5].m_dH21); _fgetts(buffer_temp, 100, fp); _stscanf(buffer_temp, _T("%s %lf %lf %lf %lf %lf %lf"), &text, &m_Leg[0].m_dH22, &m_Leg[1].m_dH22, &m_Leg[2].m_dH22, &m_Leg[3].m_dH22, &m_Leg[4].m_dH22, &m_Leg[5].m_dH22); _fgetts(buffer_temp, 100, fp); _stscanf(buffer_temp, _T("%s %lf %lf %lf %lf %lf %lf"), &text, &m_Leg[0].m_dAlpha, &m_Leg[1].m_dAlpha, &m_Leg[2].m_dAlpha, &m_Leg[3].m_dAlpha, &m_Leg[4].m_dAlpha, &m_Leg[5].m_dAlpha); _fgetts(buffer_temp, 100, fp); _stscanf(buffer_temp, _T("%s %lf %lf %lf %lf %lf %lf"), &text, &m_Leg[0].m_dBeta, &m_Leg[1].m_dBeta, &m_Leg[2].m_dBeta, &m_Leg[3].m_dBeta, &m_Leg[4].m_dBeta, &m_Leg[5].m_dBeta); _fgetts(buffer_temp, 100, fp); // STL File CString sDe = _T(""), sCardan1 = _T(""), sCardan2 = _T(""), sCardan3 = _T(""), sOng1 = _T(""), sOng2 = _T(""), sCau1 = _T(""), sCau2 = _T(""), sGia = _T(""); _fgetts(buffer_temp, 100, fp); // DE _stscanf(buffer_temp, _T("%s %s"), &text, &text2); sDe = text2; _fgetts(buffer_temp, 100, fp); // CARDAN1 _stscanf(buffer_temp, _T("%s %s"), &text, &text2); sCardan1 = text2; _fgetts(buffer_temp, 100, fp); // CARDAN2 _stscanf(buffer_temp, _T("%s %s"), &text, &text2); sCardan2 = text2; 97 _fgetts(buffer_temp, 100, fp); // CARDAN3 _stscanf(buffer_temp, _T("%s %s"), &text, &text2); sCardan3 = text2; _fgetts(buffer_temp, 100, fp); // ONG1 _stscanf(buffer_temp, _T("%s %s"), &text, &text2); sOng1 = text2; _fgetts(buffer_temp, 100, fp); // ONG2 _stscanf(buffer_temp, _T("%s %s"), &text, &text2); sOng2 = text2; _fgetts(buffer_temp, 100, fp); // CAU1 _stscanf(buffer_temp, _T("%s %s"), &text, &text2); sCau1 = text2; _fgetts(buffer_temp, 100, fp); // CAU2 _stscanf(buffer_temp, _T("%s %s"), &text, &text2); sCau2 = text2; _fgetts(buffer_temp, 100, fp); // GIA _stscanf(buffer_temp, _T("%s %s"), &text, &text2); sGia = text2; _fgetts(buffer_temp, 100, fp); // DATA CMechanicData* pData = new CMechanicData(); _fgetts(buffer_temp, 100, fp); // Xp Yp Zp Rotx Roty Rotz _stscanf(buffer_temp, _T("%s %lf %lf %lf %lf %lf %lf"), &text, &pData->m_dXp, &pData->m_dYp, &pData->m_dZp, &pData->m_dRotx, &pData->m_dRoty, &pData->m_dRotz); _fgetts(buffer_temp, 100, fp); // Phi _stscanf(buffer_temp, _T("%s %lf %lf %lf %lf %lf %lf"), &text, &pData->m_dPhi[0], &pData->m_dPhi[1], &pData->m_dPhi[2], &pData->m_dPhi[3], &pData->m_dPhi[4], &pData>m_dPhi[5]); _fgetts(buffer_temp, 100, fp); // Psi _stscanf(buffer_temp, _T("%s %lf %lf %lf %lf %lf %lf"), &text, &pData->m_dPsi[0], &pData->m_dPsi[1], &pData->m_dPsi[2], &pData->m_dPsi[3], &pData->m_dPsi[4], &pData>m_dPsi[5]); _fgetts(buffer_temp, 100, fp); // d _stscanf(buffer_temp, _T("%s %lf %lf %lf %lf %lf %lf"), &text, &pData->m_dLen[0], &pData->m_dLen[1], &pData->m_dLen[2], &pData->m_dLen[3], &pData->m_dLen[4], &pData>m_dLen[5]); pData->m_dRotx = pData->m_dRotx * 180/ PI; pData->m_dRoty = pData->m_dRoty * 180/ PI; pData->m_dRotz = pData->m_dRotz * 180/ PI; for(int i = 0; i < NUM_LEG; i++){ pData->m_dPhi[i] = pData->m_dPhi[i] * 180/ PI; pData->m_dPsi[i] = pData->m_dPsi[i] * 180/ PI; } m_vtData.push_back(pData); CString sDir = CUtil::getFileDir(sRobotFile); m_ModelDe.InitModel(CUtil::getFullPath(sDir,sDe)); for(int i = 0; i < NUM_LEG; i++) { m_Leg[i].m_Object[0].InitModel(CUtil::getFullPath(sDir,sCardan1)); m_Leg[i].m_Object[1].InitModel(CUtil::getFullPath(sDir,sCardan2)); m_Leg[i].m_Object[2].InitModel(CUtil::getFullPath(sDir,sCardan3)); m_Leg[i].m_Object[3].InitModel(CUtil::getFullPath(sDir,sOng1)); m_Leg[i].m_Object[3].m_bBlend = true; m_Leg[i].m_Object[4].InitModel(CUtil::getFullPath(sDir,sOng2)); m_Leg[i].m_Object[5].InitModel(CUtil::getFullPath(sDir,sCau1)); m_Leg[i].m_Object[6].InitModel(CUtil::getFullPath(sDir,sCau2)); m_Leg[i].SetColorIndex(MAU_BANHRANG); } // m_ModelGia.InitModel(CUtil::getFullPath(sDir,sGia)); m_ModelGia.m_bBlend=true; fclose(fp); return true; } return false; } bool CMechanic::OpenDataFile(const CString& sRobotFile) { 98 FILE *fp = NULL; fp = _tfopen(sRobotFile, _T("rt")); if(fp){ CMechanicData* pData = m_vtData.back(); m_vtData.pop_back(); for(int i = 0; i < m_vtData.size(); i++) delete m_vtData[i]; m_vtData.clear(); m_vtData.push_back(pData); do{ pData = new CMechanicData(); // 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24 _ftscanf(fp, _T("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf ,%lf,%lf"), &pData->m_dPhi[0], &pData->m_dPsi[0], &pData->m_dLen[0], &pData->m_dPhi[1], &pData->m_dPsi[1], &pData->m_dLen[1], &pData->m_dPhi[2], &pData->m_dPsi[2], &pData->m_dLen[2], &pData->m_dPhi[3], &pData->m_dPsi[3], &pData->m_dLen[3], &pData->m_dPhi[4], &pData->m_dPsi[4], &pData->m_dLen[4], &pData->m_dPhi[5], &pData->m_dPsi[5], &pData->m_dLen[5], &pData->m_dXp, &pData->m_dYp, &pData->m_dZp, &pData->m_dRotx, &pData>m_dRoty, &pData->m_dRotz); pData->m_dXp *= 100, pData->m_dZp *= 100, pData->m_dYp *= 100; pData->m_dLen[0] *= 100, pData->m_dLen[1] *= 100, pData->m_dLen[2] *= 100, pData->m_dLen[3] *= 100, pData->m_dLen[4] *= 100, pData->m_dLen[5] *= 100; pData->m_dRotx *= 180/PI, pData->m_dRoty *= 180/PI, pData->m_dRotz *= 180/PI; pData->m_dPhi[0] *= 180/PI, pData->m_dPhi[1] *= 180/PI, pData->m_dPhi[2] *= 180/PI, pData->m_dPhi[3] *= 180/PI, pData->m_dPhi[4] *= 180/PI, pData->m_dPhi[5] *= 180/PI; pData->m_dPsi[0] *= 180/PI, pData->m_dPsi[1] *= 180/PI, pData->m_dPsi[2] *= 180/PI, pData->m_dPsi[3] *= 180/PI, pData->m_dPsi[4] *= 180/PI, pData->m_dPsi[5] *= 180/PI; if(pData->m_dLen[0] > 0.1){ m_vtData.push_back(pData); } else { delete pData; } } while(!feof(fp)); fclose(fp); return true; } return false; } 99 ... giới thiệu khái niệm robot, robot cấu trúc song song vào tìm hiểu tốn động học robot cấu trúc song song - Chương 2: Cơ sở lý thuyết khảo sát động lực học robot cấu trúc song song VI Trong chương... thiệu số phương pháp khảo sát động lực học robot áp dụng nguyên lý phù hợp vào việc khảo sát toán động lực học robot cấu trúc song song - Chương 3: Tính tốn mơ robot cấu trúc song song Chương tác... 1.2 Robot cấu trúc song song 11 1.2.1 Về cấu trúc động học robot song song 11 1.2.2 Phân loại robot cấu trúc song song 12 1.2.3 Ưu nhược điểm robot song song