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