Serial6R.world
Go to the documentation of this file.
1 % 6R Serial chain
2 
4 [CONSTANTS]
5 
6 %--------------------------------------------------------------------
7 % DH Parameters
8 %--------------------------------------------------------------------
9 
10 a1 := 0.3
11 a2 := 1
12 a3 := 0
13 a4 := 1.5
14 a5 := 0
15 a6 := 1.1353
16 
17 d1 := 0.0106
18 d2 := 0
19 d3 := 0.2
20 d4 := 0
21 d5 := 0
22 d6 := 0.1049
23 
24 alpha1 := pi/2
25 alpha2 := 0.0175
26 alpha3 := pi/2
27 alpha4 := 0.0175
28 alpha5 := pi/2
29 alpha6 := 1.4716
30 
31 % Pre-compute sin/cos of alpha_i
32 c1:=cos(alpha1)
33 s1:=sin(alpha1)
34 
35 c2:=cos(alpha2)
36 s2:=sin(alpha2)
37 
38 c3:=cos(alpha3)
39 s3:=sin(alpha3)
40 
41 c4:=cos(alpha4)
42 s4:=sin(alpha4)
43 
44 c5:=cos(alpha5)
45 s5:=sin(alpha5)
46 
47 c6:=cos(alpha6)
48 s6:=sin(alpha6)
49 
50 [LINKS]
51 
52 link1: 0.9*white
53  sphere 0.05 (-0.075,0,0)
54  cylinder 0.05 (-0.075,0,0) (-0.025,0,0)
55  cylinder 0.05 (-0.025,0,0) ( 0.025,0,0) 0.1*white
56  cylinder 0.05 ( 0.025,0,0) ( 0.075,0,0)
57  sphere 0.05 ( 0.075,0,0)
58  cylinder 0.025 (-0.05 ,0,0) (-0.05 ,-0.1,0)
59  sphere 0.025 (-0.05 ,-0.1,0)
60  cylinder 0.025 ( 0.05 ,0,0) ( 0.05 ,-0.1,0)
61  sphere 0.025 ( 0.05 ,-0.1,0)
62  cylinder 0.025 (-0.05 ,-0.1,0) ( 0.05 ,-0.1,0)
63  cylinder 0.025 (0,-0.1,0) (0,-0.2,0)
64  sphere 0.025 (0,-0.2,0)
65 
66  cylinder 0.025 (0,-0.2,0) (-0.2*c1,-0.2*s1,a1)
67  sphere 0.025 (-0.2*c1,-0.2*s1,a1)
68  cylinder 0.025 (-0.2*c1,-0.2*s1,a1)((d1+0.2)*c1,(d1+0.2)*s1,a1)
69 
70 link2: 0.1*white
71  sphere 0.05 (-0.075+0.2,0,0)
72  cylinder 0.05 (-0.075+0.2,0,0) (-0.025+0.2,0,0)
73  cylinder 0.05 (-0.025+0.2,0,0) ( 0.025+0.2,0,0) 0.9*white
74  cylinder 0.05 ( 0.025+0.2,0,0) ( 0.075+0.2,0,0)
75  sphere 0.05 ( 0.075+0.2,0,0)
76  cylinder 0.025 (-0.05+0.2 ,0,0) (-0.05+0.2 ,0,0.1)
77  sphere 0.025 (-0.05+0.2 ,0,0.1)
78  cylinder 0.025 ( 0.05+0.2 ,0,0) ( 0.05+0.2 ,0,0.1)
79  sphere 0.025 ( 0.05+0.2 ,0,0.1)
80  cylinder 0.025 (-0.05+0.2 ,0,0.1) ( 0.05+0.2 ,0,0.1)
81  cylinder 0.025 (0+0.2,0,0.1) (0+0.2,0,0.2)
82  sphere 0.025 (0+0.2,0,0.2)
83 
84  cylinder 0.025 (0+0.2,0,0.2) (0.2*c2,0.2*s2,0.2)
85  sphere 0.025 (0.2*c2,0.2*s2,0.2)
86  cylinder 0.025 (0.2*c2,0.2*s2,0.2) (0.2*c2,0.2*s2,a2)
87  sphere 0.025 (0.2*c2,0.2*s2,a2)
88  cylinder 0.025 (0.2*c2,0.2*s2,a2)(d2*c2-0.2,d2*s2,a2)
89 
90 link3: 0.9*white
91  sphere 0.05 (-0.075-0.2,0,0)
92  cylinder 0.05 (-0.075-0.2,0,0) (-0.025-0.2,0,0)
93  cylinder 0.05 (-0.025-0.2,0,0) ( 0.025-0.2,0,0) 0.1*white
94  cylinder 0.05 ( 0.025-0.2,0,0) ( 0.075-0.2,0,0)
95  sphere 0.05 ( 0.075-0.2,0,0)
96  cylinder 0.025 (-0.05-0.2 ,0,0) (-0.05-0.2 ,0,-0.1)
97  sphere 0.025 (-0.05-0.2 ,0,-0.1)
98  cylinder 0.025 ( 0.05-0.2 ,0,0) ( 0.05-0.2 ,0,-0.1)
99  sphere 0.025 ( 0.05-0.2 ,0,-0.1)
100  cylinder 0.025 (-0.05-0.2 ,0,-0.1) ( 0.05-0.2 ,0,-0.1)
101  cylinder 0.025 (0-0.2,0,-0.1) (0-0.2,0,-0.15)
102  sphere 0.025 (0-0.2,0,-0.15)
103 
104  cylinder 0.025 (0-0.2,0,-0.15) (d3*c3,d3*s3,-0.15)
105  sphere 0.025 (d3*c3,d3*s3,-0.15)
106  cylinder 0.025 (d3*c3,d3*s3,-0.15) (d3*c3,d3*s3,a3)
107 
108 link4: 0.1*white
109  sphere 0.05 (-0.075,0,0)
110  cylinder 0.05 (-0.075,0,0) (-0.025,0,0)
111  cylinder 0.05 (-0.025,0,0) ( 0.025,0,0) 0.9*white
112  cylinder 0.05 ( 0.025,0,0) ( 0.075,0,0)
113  sphere 0.05 ( 0.075,0,0)
114  cylinder 0.025 (-0.05 ,0,0) (-0.05 ,0,0.1)
115  sphere 0.025 (-0.05 ,0,0.1)
116  cylinder 0.025 ( 0.05 ,0,0) ( 0.05 ,0,0.1)
117  sphere 0.025 ( 0.05 ,0,0.1)
118  cylinder 0.025 (-0.05 ,0,0.1) ( 0.05 ,0,0.1)
119  cylinder 0.025 (0,0,0.1) (0,0,0.3)
120  sphere 0.025 (0,0,0.3)
121 
122  cylinder 0.025 (0,0,0.3) (-0.5*c4,-0.5*s4,a4)
123  sphere 0.025 (-0.5*c4,-0.5*s4,a4)
124  cylinder 0.025 (-0.5*c4,-0.5*s4,a4)(-0.25*c4,-0.25*s4,a4)
125 
126 link5: 0.9*white
127  sphere 0.05 (-0.075-0.25,0,0)
128  cylinder 0.05 (-0.075-0.25,0,0) (-0.025-0.25,0,0)
129  cylinder 0.05 (-0.025-0.25,0,0) ( 0.025-0.25,0,0) 0.1*white
130  cylinder 0.05 ( 0.025-0.25,0,0) ( 0.075-0.25,0,0)
131  sphere 0.05 ( 0.075-0.25,0,0)
132 
133  cylinder 0.025 (-0.05-0.25 ,0,0) (-0.05-0.25 ,-0.1,0)
134  sphere 0.025 (-0.05-0.25 ,-0.1,0)
135  cylinder 0.025 ( 0.05-0.25 ,0,0) ( 0.05-0.25 ,-0.1,0)
136  sphere 0.025 ( 0.05-0.25 ,-0.1,0)
137  cylinder 0.025 (-0.05-0.25 ,-0.1,0) ( 0.05-0.25 ,-0.1,0)
138  cylinder 0.025 (0-0.25,-0.1,0) (0-0.25,-0.2,0)
139  sphere 0.025 (0-0.25,-0.2,0)
140 
141  sphere 0.025 (0-0.25,-0.2,0)
142  cylinder 0.025 (0-0.25,-0.2,0) (0,-0.2,0)
143 
144 link6: 0.1*white
145  sphere 0.05 (-0.075-0.2,0,0)
146  cylinder 0.05 (-0.075-0.2,0,0) (-0.025-0.2,0,0)
147  cylinder 0.05 (-0.025-0.2,0,0) ( 0.025-0.2,0,0) 0.9*white
148  cylinder 0.05 ( 0.025-0.2,0,0) ( 0.075-0.2,0,0)
149  sphere 0.05 ( 0.075-0.2,0,0)
150  cylinder 0.025 (-0.05-0.2 ,0,0) (-0.05-0.2 ,0,0.1)
151  sphere 0.025 (-0.05-0.2 ,0,0.1)
152  cylinder 0.025 ( 0.05-0.2 ,0,0) ( 0.05-0.2 ,0,0.1)
153  sphere 0.025 ( 0.05-0.2 ,0,0.1)
154  cylinder 0.025 (-0.05-0.2 ,0,0.1) ( 0.05-0.2 ,0,0.1)
155  cylinder 0.025 (0-0.2,0,0.1) (0-0.2,0,0.25)
156  sphere 0.025 (0-0.2,0,0.25)
157 
158  cylinder 0.025 (0-0.2,0,0.25) (-0.1*c6,-0.1*s6,a6)
159  sphere 0.025 (-0.1*c6,-0.1*s6,a6)
160  cylinder 0.025 (-0.1*c6,-0.1*s6,a6)(d6*c6,d6*s6,a6)
161 
162 [JOINTS]
163 
164 revolute: link1 (-c1,-s1,a1) (d1*c1,d1*s1,a1)
165  link2 (-d1-1,0,0)(0,0,0)
166 
167 revolute: link2 (-c2,-s2,a2) (d2*c2,d2*s2,a2)
168  link3 (-d2-1,0,0)(0,0,0)
169 
170 revolute: link3 (-c3,-s3,a3) (d3*c3,d3*s3,a3)
171  link4 (-d3-1,0,0)(0,0,0)
172 
173 revolute: link4 (-c4,-s4,a4) (d4*c4,d4*s4,a4)
174  link5 (-d4-1,0,0)(0,0,0)
175 
176 revolute: link5 (-c5,-s5,a5) (d5*c5,d5*s5,a5)
177  link6 (-d5-1,0,0)(0,0,0)
178 
179 revolute: link6 (-c6,-s6,a6) (d6*c6,d6*s6,a6)
180  link1 (-d6-1,0,0)(0,0,0)
181