• API
• FAQ
• Tools
• Archive
SHARE
TWEET

# workspace_kod

a guest Nov 18th, 2019 69 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. % this script determines a Homogeneous Transformation HT matrix
2. clear all
3. % declaration of symbols
4. syms d1 a1 th2 a3 q2 th4 a4 al5 q4 q5 al3 al4
5. % determination of a symbolic form of HT matrices –
6. % application of mA function
7. A1=mA(0,d1,a1,0)
8. A2=mA(th2,0,0,0)
9. A3=mA(0,0,a3,al3)
10. A4=mA(th4,0,a4,al4)
11. A5=mA(0,0,0,al5)
12. % multiplication of matrices
13. T01=A1;
14. T02=A1*A2;
15. T03=A1*A2*A3
16. T04=A1*A2*A3*A4;
17. T35=A4*A5
18. T05=A1*A2*A3*A4*A5;
19.
20. %%
21. % Horizontal workspace
22. for i=(1:1:100)
23.
24. d11=800;
25. th22=(-1.7444:0.034888:1.7444);
26. a33=600;
27.
28.
29. T01n=double(subs(T01,{d1,a1},{d11,500}))
30. T02n=double(subs(T02,{d1,a1,th2},{d11,500,th22(1,i)}))
31. T03n=double(subs(T03,{d1,a1,th2,a3,al3},{d11,500,th22(1,i),a33,-pi/2}))
32. T04n=double(subs(T04,{d1,a1,th2,a3,al3,th4,a4,al4},{d11,500,th22(1,i),a33,-pi/2,0,500,pi/2}))
33. T05n=double(subs(T05,{d1,a1,th2,a3,al3,th4,a4,al4,al5},{d11,500,th22(1,i),a33,-pi/2,0,500,pi/2,0}))
34. K=[0 0 0 1];
35.
36. T01c=T01n.*K
37. T02c=T02n.*K
38. T03c=T03n.*K
39. T04c=T04n.*K
40. T05c=T05n.*K
41. figure(1)
42. grid on
43. title('Manipulator')
44. xlabel('X-axis')
45. ylabel('Y-axis')
46. zlabel('Z-axis')
47. text(double(T04c(1,4)),double(T04c(2,4)),double(T04c(3,4)),'')
48.
49. plot3([double(T01c(1,4)) double(T02c(1,4))],[double(T01c(2,4)) double(T02c(2,4))],[double(T01c(3,4)) double(T02c(3,4))])
50. hold on
51. plot3([double(T02c(1,4)) double(T03c(1,4))],[double(T02c(2,4)) double(T03c(2,4))],[double(T02c(3,4)) double(T03c(3,4))])
52. hold on
53. plot3([double(T03c(1,4)) double(T04c(1,4))],[double(T03c(2,4)) double(T04c(2,4))],[double(T03c(3,4)) double(T04c(3,4))])
54. hold on
55. plot3([double(T04c(1,4)) double(T05c(1,4))],[double(T04c(2,4)) double(T05c(2,4))],[double(T04c(3,4)) double(T05c(3,4))])
56.
57. plot3(500, 0, 0,'-x')
58. plot3(double(T01c(1,4)),double(T01c(2,4)),double(T01c(3,4)),'-o')
59. hold on
60. plot3(double(T02c(1,4)),double(T02c(2,4)),double(T02c(3,4)),'-o')
61. hold on
62. plot3(double(T03c(1,4)),double(T03c(2,4)),double(T03c(3,4)),'-o')
63. hold on
64. plot3(double(T04c(1,4)),double(T04c(2,4)),double(T04c(3,4)),'-o')
65. hold on
66. plot3(double(T05c(1,4)),double(T05c(2,4)),double(T05c(3,4)),'-d')
67. hold on
68. axis([-2000 2000 -2000 2000 -2000 2000])
69. end
70.
71. %Vertical workspace
72.
73. for i=(1:1:100)
74.
75. d11=(100:7:100+7*i);
76. th22=0;
77. a33=(100:5:100+5*i);
78.
79.
80. T01n=double(subs(T01,{d1,a1},{d11(1,i),500}))
81. T02n=double(subs(T02,{d1,a1,th2},{d11(1,i),500,th22}))
82. T03n=double(subs(T03,{d1,a1,th2,a3,al3},{d11(1,i),500,th22,a33(1,i),-pi/2}))
83. T04n=double(subs(T04,{d1,a1,th2,a3,al3,th4,a4,al4},{d11(1,i),500,th22,a33(1,i),-pi/2,0,500,pi/2}))
84. T05n=double(subs(T05,{d1,a1,th2,a3,al3,th4,a4,al4,al5},{d11(1,i),500,th22,a33(1,i),-pi/2,0,500,pi/2,0}))
85. K=[0 0 0 1];
86.
87. T01c=T01n.*K
88. T02c=T02n.*K
89. T03c=T03n.*K
90. T04c=T04n.*K
91. T05c=T05n.*K
92. figure(1)
93. grid on
94. title('Manipulator')
95. xlabel('X-axis')
96. ylabel('Y-axis')
97. zlabel('Z-axis')
98. text(double(T04c(1,4)),double(T04c(2,4)),double(T04c(3,4)),'')
99.
100. plot3([double(T01c(1,4)) double(T02c(1,4))],[double(T01c(2,4)) double(T02c(2,4))],[double(T01c(3,4)) double(T02c(3,4))])
101. hold on
102. plot3([double(T02c(1,4)) double(T03c(1,4))],[double(T02c(2,4)) double(T03c(2,4))],[double(T02c(3,4)) double(T03c(3,4))])
103. hold on
104. plot3([double(T03c(1,4)) double(T04c(1,4))],[double(T03c(2,4)) double(T04c(2,4))],[double(T03c(3,4)) double(T04c(3,4))])
105. hold on
106. plot3([double(T04c(1,4)) double(T05c(1,4))],[double(T04c(2,4)) double(T05c(2,4))],[double(T04c(3,4)) double(T05c(3,4))])
107.
108. plot3(500, 0, 0,'-x')
109. plot3(double(T01c(1,4)),double(T01c(2,4)),double(T01c(3,4)),'-o')
110. hold on
111. plot3(double(T02c(1,4)),double(T02c(2,4)),double(T02c(3,4)),'-o')
112. hold on
113. plot3(double(T03c(1,4)),double(T03c(2,4)),double(T03c(3,4)),'-o')
114. hold on
115. plot3(double(T04c(1,4)),double(T04c(2,4)),double(T04c(3,4)),'-o')
116. hold on
117. plot3(double(T05c(1,4)),double(T05c(2,4)),double(T05c(3,4)),'-d')
118. hold on
119. axis([-2000 2000 -2000 2000 -2000 2000])
120. end
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.

Top