VRML Model of 3-D Robot Arm
VRML Model of 3-D Robot Arm
MATLAB VRML MATLAB Simulink i
MATLAB Simulink V-Realm Build Joystick ii
Abstract The major purpose of this thesis presents the procedure of VRML modeling of 3-D robot arm in computer. We use virtual reality toolbox of MATLAB7.1 to perform our researchful result. By computing vectors and trigonometric function, we understand the moving track of robot arm. Using many block diagrams to show our program after counting. Last we connect the figure by using V-Realm Build program with Joystick block to control the robot arm. Let robot arm can move any place we want. Before using the toolbox, we must have background knowledge about the motion mode of three domains robot arm and basic block diagram. These will be introduced in our thesis. We really hope that the thesis can provide much useful information on dynamic simulation. iii
...i...ii Abstract...iii...iv...v...viii...1 1.1...1 1.2...1 1.3...1...3...6 3.1...6 3.2 Joystick...14 3.3 VRML...17 3.4 VRML...18...20 4.1...20 4.2...47...53 5.1...53 5.2...54 5.3...55...56 iv
1.1...2 1.2...2 2.1...3 2.2...4 3.1...6 3.2 3.4 Data Type Conversion...8 Gain...9 3.5 Math Function...10 3.6 MATLAB Fcn...11 3.7 Mux...11 3.8...12 3.9 Dead Zone...12 3.10 Saturation...13 3.11 Sign...13 3.12 Joystick...14 3.13 Joystick...15 3.14 Joystick...16 3.15 VR Sink...17 3.16 VR Sink...17 3.17...18 4.1...20 4.2...21 4.3 a4+a5 & a6...21 4.4 θ4...22 v
4.5 Fcn1(a4)...22 4.6 Fcn2(a4)...23 4.7 θ5...23 4.8 Fcn2(a5)...24 4.9 Fcn3(a5)...25 4.10 a6...25 4.11 X1(a6)...26 4.12 Y1(a6)...27 4.13 Z1(a6)...27 4.14 Fcn1(a4+a5)...28 4.15 a3...29 4.16 C1 translation &rotation...30 4.17 Fcn1(C1 translation)...30 4.18 Fcn2(C1 translation)...31 4.19 Fcn3(C1 translation)...31 4.20 Fcn4(C1 & C4 rotation)...32 4.21 Fcn5(C1 & C4 rotation)...33 4.22 Fcn6(C1 & C4 rotation)...33 4.23 a3...34 4.24 C2 translation...34 4.25 Fcn1(C2 translation)...35 4.26 Fcn2(C2 translation)...36 4.27 Fcn3(C2 translation)...36 4.28 Fcn1(C3 translation)...38 4.29 Fcn2(C3 translation)...38 4.30 Fcn3(C3 translation)...39 vi
4.31 C4 & S2 translation...40 4.32 Fcn1(C4 & S2 translation)...40 4.33 Fcn2(C4 & S2 translation)...41 4.34 Fcn3(C4 & S2 translation)...41 4.35 Fcn4(C4 & S2 translation)...42 4.36 Fcn5(C4 & S2 translation)...43 4.37 Fcn6(C4 & S2 translation)...43 4.38 Fcn1(S3 translation)...44 4.39 Fcn2(S3 translation)...45 4.40 Fcn3(S3 translation)...45 4.41...47 4.42...47 4.43...48 4.44 X...49 4.45...50 4.46...50 4.47...51 4.48...51 4.49...52 4.50...52 5.1...53 5.2...53 vii
3.1...7 3.2...19 viii
1.1 MATLAB Simulink 1.2... 1.3 MATLAB Simulink V-Realm Build MATLAB VRML Joystick 1.1 1.2 1
1.1 1.2 2
2.1 XYZ 2.1 X1 Y 1 Z 1 C 1 X2 Y 2 Z 2 C 2 C1 C 2 2.1 C C2 1 1 C1X = ( Cco 1 sθ1) cosθ3 2 (2.1) 1 C1 Y = C1 θ1 2 ( sin ) (2.2) 1 C1Z = ( Cco 1 sθ1) sinθ3 2 (2.3) ( ) 1 C2X = Cco 1 sθ1 + ( C2cosθ2) cosθ3 2 (2.4) 3
1 C = C + C 2 ( sinθ ) ( sinθ ) 2Y 1 1 2 2 (2.5) ( ) 1 C2Z = Cco 1 sθ1 + ( C2cosθ2) sinθ3 2 (2.6) (2.1) (2.6) C1 C 2 XYZ 2.1 C1 C 2 C1 C 2 2.2 2.2 θ = tan 4 1 Y X+Z 2 2 (2.7) ( ) X = C cos θ + θ cosθ 1 1 4 5 3 (2.8) 4
( ) Y = C sin θ + θ 1 1 4 5 (2.9) ( ) Z = C cos θ + θ sinθ 1 1 4 5 3 (2.10) θ = cos 5 θ = tan 6 1 1 X + Y + Z 2 C 2 2 2 1 ( Y-Y1 ) ( X-X ) + ( Z-Z ) 2 2 1 1 (2.11) (2.12) θ = tan 3 1 Z X (2.13) θ3 θ 4 θ 5 θ 6 C1 C 2 1 (2.7) arctan2 tan 360 arctan2 (2.8) (2.9) (2.10) X 1 Y 1 Z 1 (2.12) θ6 +Z -Z C1 C 2 ( sin θ, 0, cosθ ) 3 3 X Y Z 5
MATLAB 7.1 Model 3.1 Simulink 3.1 3.1 3.1 6
3.1 A Constant 1 B Demux C Input D Data Type conversion E Gain F Display G Output H Sum ++ +- I Math Function exp log square conj J MATLAB Fcn u K Integrator L Dead Zone M Saturation N Sign 1 1 0-1 O Mux P Joystick Input Q VR Sink 7
Demux 3.2 input Data Type Conversion MATLAB Joystick Buttons Demux Buttons boolean uint16 3.3 3.4 3.2 Demux 3.3 Data Type Conversion 8
3.4 Gain Math Function 3.5 MATLAB Fcn 3.6 u u () ( ) ( ) ( ) ( ( )) u 1 +u 2 -cos u 3 +sin u 4 (3.1) (3.1) u(1) u(2) u(3) u(4) 3.6 3.7 Mux Demux 3.8 3.9 Dead Zone +0.5-0.5 2 1.5-0.8-0.3-0.3 0 9
0,if Dead Zone ( -Dead Zone ),if > Dead Zone (3.2) 3.10 Saturation ±5 6 5-8 -5 ±5 3.11 Sign 1 0-1 0 Joystick VR Sink 3.5 Math Function 10
3.6 MATLAB Fcn 3.7 Mux 11
3.8 3.9 Dead Zone 12
3.10 Saturation 3.11 Sign 13
3.2 Joystick Joystick 3.12 Axes Button Axes (X,Y) +1-1 0 Joystick (X,Y) Axes [1 2] Joystick Axes Demux 3.12 Joystick (X,Y) u(1) u(2) ( ( ) ( )) θ =arctan2 u 2,u 1 (3.3) (X,Y) + - 14
3.12 Saturation Buttons Button Demux Demux Data Type Conversion Button Z Button Axe Sum Z 3.13 3.14 Joystick Input 3.13 Joystick 15
3.14 Joystick 3.13 3.14 Joystick Axes Demux (X,Y) Dead Zone Dead Zone Sign 1 0-1 Saturation -1 (X,Y) -1 (X,Y) 16
3.3 VRML VRML MATLAB Toolbox V-Realm Bulid VRML 3.15 3.15 VR Sink VRML translation rotation scale VR Sink 3.16 3.16 VR Sink 17
3.4VRML 3.17 3.17 3.2 18
3.2 Height( ) Radius( ) (c1) 2 0.2 (c2) 2 0.2 (c3) 0.7 0.1 (c4) 2 0.1 (s1) 0.2 (s2) 0.2 (s3) 0.2 (s4) 0.2 (cp1) 1 0.2 ( ) (cp2) 0.3 1 (cp3) 6 3 0.3 (c1) (c2) MATLAB toolbox V-Realm Bulider VR Sink 19
4.1 4.1 MATLAB File New Model 10.0 inf 4.1 4.1 Joystick VR Sink VR Joystick VR Sink 20
Joystick Axes (X,Y) Joystick buttons Z (X,Y,Z) (X,Y,Z) (X,Y,Z) 4.2 4.2 (X,Y,Z) C1 C 2 θ3 θ 4 θ 5 θ 6 a4+a5 & a6 4.3 4.3 a4+a5 & a6 21
1 θ 4 + θ 5 2 θ 6 θ 4 + θ 5 θ 4 θ5 θ4 4.4 θ4 4.4 In1 Joystick (X,Y,Z) Fcn1 u(1) u(2) u(3) (X,Y,Z) 4.5 4.5 Fcn1(a4) 22
4.4 Math function Fcn1 (X,Y,Z) Gain 0.5 arc cos 2 Fcn2 4.6 4.6 Fcn2(a4) θ4 a4+a5 & a6 2 θ 4.7 5 4.7 θ5 23
4.7 In1 Joystick (X,Y,Z) Fcn1 Y u(2) Fcn2 (X,Z) 4.8 4.8 Fcn2(a5) (X,Z) Fcn3 Mux θ 4.9 5 24
4.9 Fcn3(a5) θ5 atan2(u(1),u(2)) θ4 θ 5 θ a6 4.10 6 4.10 a6 25
In1 θ 3 In2 Joystick (X,Y,Z) In3 a4+a5 X Y Z X X u(1) Y Y u(2) Z Z u(3) X1 Y1 Z1 4.11 4.12 4.13 4.11 X1(a6) 26
4.12 Y1(a6) 4.13 Z1(a6) 27
X-X1 Y-Y1 Z-Z1 Mux u(1)-u(2) (X-X1)*(X-X1) (Z-Z1)*(Z-Z1) u*u Fcn1 u(1)+u(2) Mux arctan atan2(u(1),u(2)) θ a4 a5 Mux Fcn1 4.14 6 4.14 Fcn1(a4+a5) θ4 θ 5 θ 6 θ3 a3 Z X arctan 4.15 28
4.15 a3 C1 translation & rotation C S1 4.16 In1 θ4 θ 5 In2 θ3 Fcn1 C1 X 4.17 Fcn2 C1 Y 4.18 Fcn3 C1 Z 4.19 u(1) θ 4 + θ 5 u(2) θ 3 1 cos θ θ )cos( θ ) (4.1) + 5 3 sin θ θ ) (4.2) + 5 cos θ θ )sin( θ ) (4.3) + 5 3 29
4.16 C1 translation & rotation 4.17 Fcn1(C1 translation) 30
4.18 Fcn2(C1 translation) 4.19 Fcn3(C1 translation) 31
MUX X Y Z X sin( θ ) Fcn4 4.20 Y Z 3 cos( θ3) Fcn5 4.21 θ 4 + θ 5 4.22 pi/2 run pi/2 90 (cylinder1)c1 S1 sphere1 C1 S1 translation C1 translation X Y Z Fcn7 Fcn8 Fcn9 2 4.20 Fcn4(C1 & C4 rotation) 32
4.21 Fcn5(C1 & C4 rotation) 4.22 Fcn6(C1 & C4 rotation) 33
θ3 a3 θ3 Z X arctan 4.23 4.23 a3 u(1) X u(2) Z (cylinder2)c2 4.24 4.24 C2 translation 34
Fcn1 X 4.25 Fcn2 Y 4.26 Fcn3 Z 4.27 C1 C2 θ 6 ( ) cos θ + θ5) + cos( θ6) cos( θ3) (4.4) ( sin θ θ ) sin( θ )) + + 5 6 (4.5) ( ) cos θ + θ5) + cos( θ6) sin( θ3) (4.6) 4.25 Fcn1(C2 translation) 35
4.26 Fcn2(C2 translation) 4.27 Fcn3(C2 translation) 36
u(1) θ 4 + θ 5 u(2) θ6 u(3) θ3 C2 C3 translation C1 C2 C3 C4 C3(cylinder3) C2 θ 3 = 0.35 cos( θ ) cos( θ ) (4.7) 6 3 = 0.35 sin( θ ) (4.8) 6 = 0.35 cos( θ ) sin( θ ) (4.9) 6 3 0.35 C3 translation Fcn1 4.28 Fcn2 4.29 Fcn3 4.30 C2 C3 C2 & C3 rotation θ6 C1 rotation X sin( θ ) Y 0 Z cos( θ ) 3 3 37
4.28 Fcn1(C3 translation) 4.29 Fcn2(C3 translation) 38
4.30 Fcn3(C3 translation) C4 & S2 translation 4.31 input output C4 S2 translation C4(cylinder4) C1 C4 (X,Y,Z) X = (cos( θ + θ ) + 0.7 cos( θ )) cos( θ ) (4.10) 4 5 6 3 = (sin( θ + θ ) + 0.7 sin( θ )) (4.11) 4 5 6 = (cos( θ + θ ) + 0.7 cos( θ ))sin( θ ) (4.12) 4 5 6 3 39
4.31 C4 & S2 translation 4.31 Fcn1 C4 X 4.32 Fcn2 C4 Y 4.33 Fcn3 Z 4.34 4.32 Fcn1(C4 & S2 translation) 40
4.33 Fcn2(C4 & S2 translation) 4.34 Fcn3(C4 & S2 translation) 41
C1 C2 C3 C4 S2 S2 C2 C4 S2 C2 C2 Fcn4 X 4.35 Fcn5 Y 4.36 Fcn6 Z 4.37 4.35 Fcn4(C4 & S2 translation) 42
4.36 Fcn5(C4 & S2 translation) 4.37 Fcn6(C4 & S2 translation) 43
C3 C4 S3(sphere3) X = 0.7 cos( θ )cos( θ ) (4.13) 6 3 = 0.7 sin( θ ) (4.14) 6 = 0.7 cos( θ )sin( θ ) (4.15) 6 3 Fcn1 Fcn2 Fcn3 X Y Z Fcn1 4.38 Fcn2 4.39 Fcn3 4.40 u(2) θ 6 u(3) θ 3 4.38 Fcn1(S3 translation) 44
4.39 Fcn2(S3 translation) 4.40 Fcn3(S3 translation) 45
CP1 CP2 CP3 给 X Y Z VR Sink 46
4.2 MATLAB V-Realm Builder2.0 4.41 4.41 4.42 47
4.42 New World Transform children rotation translation height radius 4.43 4.43 90 Rotation Z 1 Z Rotation 90 90 4.44 X 90 48
4.44 X C1 VR-Sink VR-Sink VR-Sink 3.3 4.45 49
4.45 Viewpoint orientation X -1 90 4.46 position (0.5,8,0) 4.47 4.46 50
4.47 X Y Z 4.48 X Y 1 2 Z 4.49 4.50 4.48 51
4.49 4.50 52
5.1 4.49 X Y Z 4.50 5.1 5.1 5.2 53
5.2 MATLAB7.1 6.5 MATLAB 54
5.3 MATLAB Joystick X-Y Z 55
[1] Matlab 2003 [2] /Simulink 2001 [3] MATLAB 7 2006 [4] Matlab 6.X 2003 [5] Palm, William J. MATLAB 6. 2003 56