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. OK, I Understand
 
Top