00001 % 6R Serial chain
00002
00004 [CONSTANTS]
00005
00006 %--------------------------------------------------------------------
00007 % DH Parameters
00008 %--------------------------------------------------------------------
00009
00010 a1 := 0.3
00011 a2 := 1
00012 a3 := 0
00013 a4 := 1.5
00014 a5 := 0
00015 a6 := 1.1353
00016
00017 d1 := 0.0106
00018 d2 := 0
00019 d3 := 0.2
00020 d4 := 0
00021 d5 := 0
00022 d6 := 0.1049
00023
00024 alpha1 := pi/2
00025 alpha2 := 0.0175
00026 alpha3 := pi/2
00027 alpha4 := 0.0175
00028 alpha5 := pi/2
00029 alpha6 := 1.4716
00030
00031 % Pre-compute sin/cos of alpha_i
00032 c1:=cos(alpha1)
00033 s1:=sin(alpha1)
00034
00035 c2:=cos(alpha2)
00036 s2:=sin(alpha2)
00037
00038 c3:=cos(alpha3)
00039 s3:=sin(alpha3)
00040
00041 c4:=cos(alpha4)
00042 s4:=sin(alpha4)
00043
00044 c5:=cos(alpha5)
00045 s5:=sin(alpha5)
00046
00047 c6:=cos(alpha6)
00048 s6:=sin(alpha6)
00049
00050 [LINKS]
00051
00052 link1: 0.9*white
00053 sphere 0.05 (-0.075,0,0)
00054 cylinder 0.05 (-0.075,0,0) (-0.025,0,0)
00055 cylinder 0.05 (-0.025,0,0) ( 0.025,0,0) 0.1*white
00056 cylinder 0.05 ( 0.025,0,0) ( 0.075,0,0)
00057 sphere 0.05 ( 0.075,0,0)
00058 cylinder 0.025 (-0.05 ,0,0) (-0.05 ,-0.1,0)
00059 sphere 0.025 (-0.05 ,-0.1,0)
00060 cylinder 0.025 ( 0.05 ,0,0) ( 0.05 ,-0.1,0)
00061 sphere 0.025 ( 0.05 ,-0.1,0)
00062 cylinder 0.025 (-0.05 ,-0.1,0) ( 0.05 ,-0.1,0)
00063 cylinder 0.025 (0,-0.1,0) (0,-0.2,0)
00064 sphere 0.025 (0,-0.2,0)
00065
00066 cylinder 0.025 (0,-0.2,0) (-0.2*c1,-0.2*s1,a1)
00067 sphere 0.025 (-0.2*c1,-0.2*s1,a1)
00068 cylinder 0.025 (-0.2*c1,-0.2*s1,a1)((d1+0.2)*c1,(d1+0.2)*s1,a1)
00069
00070 link2: 0.1*white
00071 sphere 0.05 (-0.075+0.2,0,0)
00072 cylinder 0.05 (-0.075+0.2,0,0) (-0.025+0.2,0,0)
00073 cylinder 0.05 (-0.025+0.2,0,0) ( 0.025+0.2,0,0) 0.9*white
00074 cylinder 0.05 ( 0.025+0.2,0,0) ( 0.075+0.2,0,0)
00075 sphere 0.05 ( 0.075+0.2,0,0)
00076 cylinder 0.025 (-0.05+0.2 ,0,0) (-0.05+0.2 ,0,0.1)
00077 sphere 0.025 (-0.05+0.2 ,0,0.1)
00078 cylinder 0.025 ( 0.05+0.2 ,0,0) ( 0.05+0.2 ,0,0.1)
00079 sphere 0.025 ( 0.05+0.2 ,0,0.1)
00080 cylinder 0.025 (-0.05+0.2 ,0,0.1) ( 0.05+0.2 ,0,0.1)
00081 cylinder 0.025 (0+0.2,0,0.1) (0+0.2,0,0.2)
00082 sphere 0.025 (0+0.2,0,0.2)
00083
00084 cylinder 0.025 (0+0.2,0,0.2) (0.2*c2,0.2*s2,0.2)
00085 sphere 0.025 (0.2*c2,0.2*s2,0.2)
00086 cylinder 0.025 (0.2*c2,0.2*s2,0.2) (0.2*c2,0.2*s2,a2)
00087 sphere 0.025 (0.2*c2,0.2*s2,a2)
00088 cylinder 0.025 (0.2*c2,0.2*s2,a2)(d2*c2-0.2,d2*s2,a2)
00089
00090 link3: 0.9*white
00091 sphere 0.05 (-0.075-0.2,0,0)
00092 cylinder 0.05 (-0.075-0.2,0,0) (-0.025-0.2,0,0)
00093 cylinder 0.05 (-0.025-0.2,0,0) ( 0.025-0.2,0,0) 0.1*white
00094 cylinder 0.05 ( 0.025-0.2,0,0) ( 0.075-0.2,0,0)
00095 sphere 0.05 ( 0.075-0.2,0,0)
00096 cylinder 0.025 (-0.05-0.2 ,0,0) (-0.05-0.2 ,0,-0.1)
00097 sphere 0.025 (-0.05-0.2 ,0,-0.1)
00098 cylinder 0.025 ( 0.05-0.2 ,0,0) ( 0.05-0.2 ,0,-0.1)
00099 sphere 0.025 ( 0.05-0.2 ,0,-0.1)
00100 cylinder 0.025 (-0.05-0.2 ,0,-0.1) ( 0.05-0.2 ,0,-0.1)
00101 cylinder 0.025 (0-0.2,0,-0.1) (0-0.2,0,-0.15)
00102 sphere 0.025 (0-0.2,0,-0.15)
00103
00104 cylinder 0.025 (0-0.2,0,-0.15) (d3*c3,d3*s3,-0.15)
00105 sphere 0.025 (d3*c3,d3*s3,-0.15)
00106 cylinder 0.025 (d3*c3,d3*s3,-0.15) (d3*c3,d3*s3,a3)
00107
00108 link4: 0.1*white
00109 sphere 0.05 (-0.075,0,0)
00110 cylinder 0.05 (-0.075,0,0) (-0.025,0,0)
00111 cylinder 0.05 (-0.025,0,0) ( 0.025,0,0) 0.9*white
00112 cylinder 0.05 ( 0.025,0,0) ( 0.075,0,0)
00113 sphere 0.05 ( 0.075,0,0)
00114 cylinder 0.025 (-0.05 ,0,0) (-0.05 ,0,0.1)
00115 sphere 0.025 (-0.05 ,0,0.1)
00116 cylinder 0.025 ( 0.05 ,0,0) ( 0.05 ,0,0.1)
00117 sphere 0.025 ( 0.05 ,0,0.1)
00118 cylinder 0.025 (-0.05 ,0,0.1) ( 0.05 ,0,0.1)
00119 cylinder 0.025 (0,0,0.1) (0,0,0.3)
00120 sphere 0.025 (0,0,0.3)
00121
00122 cylinder 0.025 (0,0,0.3) (-0.5*c4,-0.5*s4,a4)
00123 sphere 0.025 (-0.5*c4,-0.5*s4,a4)
00124 cylinder 0.025 (-0.5*c4,-0.5*s4,a4)(-0.25*c4,-0.25*s4,a4)
00125
00126 link5: 0.9*white
00127 sphere 0.05 (-0.075-0.25,0,0)
00128 cylinder 0.05 (-0.075-0.25,0,0) (-0.025-0.25,0,0)
00129 cylinder 0.05 (-0.025-0.25,0,0) ( 0.025-0.25,0,0) 0.1*white
00130 cylinder 0.05 ( 0.025-0.25,0,0) ( 0.075-0.25,0,0)
00131 sphere 0.05 ( 0.075-0.25,0,0)
00132
00133 cylinder 0.025 (-0.05-0.25 ,0,0) (-0.05-0.25 ,-0.1,0)
00134 sphere 0.025 (-0.05-0.25 ,-0.1,0)
00135 cylinder 0.025 ( 0.05-0.25 ,0,0) ( 0.05-0.25 ,-0.1,0)
00136 sphere 0.025 ( 0.05-0.25 ,-0.1,0)
00137 cylinder 0.025 (-0.05-0.25 ,-0.1,0) ( 0.05-0.25 ,-0.1,0)
00138 cylinder 0.025 (0-0.25,-0.1,0) (0-0.25,-0.2,0)
00139 sphere 0.025 (0-0.25,-0.2,0)
00140
00141 cylinder 0.025 (0-0.25,-0.2,0) (0-0.25,-0.2,0)
00142 sphere 0.025 (0-0.25,-0.2,0)
00143 cylinder 0.025 (0-0.25,-0.2,0) (0,-0.2,0)
00144
00145 link6: 0.1*white
00146 sphere 0.05 (-0.075-0.2,0,0)
00147 cylinder 0.05 (-0.075-0.2,0,0) (-0.025-0.2,0,0)
00148 cylinder 0.05 (-0.025-0.2,0,0) ( 0.025-0.2,0,0) 0.9*white
00149 cylinder 0.05 ( 0.025-0.2,0,0) ( 0.075-0.2,0,0)
00150 sphere 0.05 ( 0.075-0.2,0,0)
00151 cylinder 0.025 (-0.05-0.2 ,0,0) (-0.05-0.2 ,0,0.1)
00152 sphere 0.025 (-0.05-0.2 ,0,0.1)
00153 cylinder 0.025 ( 0.05-0.2 ,0,0) ( 0.05-0.2 ,0,0.1)
00154 sphere 0.025 ( 0.05-0.2 ,0,0.1)
00155 cylinder 0.025 (-0.05-0.2 ,0,0.1) ( 0.05-0.2 ,0,0.1)
00156 cylinder 0.025 (0-0.2,0,0.1) (0-0.2,0,0.25)
00157 sphere 0.025 (0-0.2,0,0.25)
00158
00159 cylinder 0.025 (0-0.2,0,0.25) (-0.1*c6,-0.1*s6,a6)
00160 sphere 0.025 (-0.1*c6,-0.1*s6,a6)
00161 cylinder 0.025 (-0.1*c6,-0.1*s6,a6)(d6*c6,d6*s6,a6)
00162
00163 [JOINTS]
00164
00165 revolute: link1 (-c1,-s1,a1) (d1*c1,d1*s1,a1)
00166 link2 (-d1-1,0,0)(0,0,0)
00167
00168 revolute: link2 (-c2,-s2,a2) (d2*c2,d2*s2,a2)
00169 link3 (-d2-1,0,0)(0,0,0)
00170
00171 revolute: link3 (-c3,-s3,a3) (d3*c3,d3*s3,a3)
00172 link4 (-d3-1,0,0)(0,0,0)
00173
00174 revolute: link4 (-c4,-s4,a4) (d4*c4,d4*s4,a4)
00175 link5 (-d4-1,0,0)(0,0,0)
00176
00177 revolute: link5 (-c5,-s5,a5) (d5*c5,d5*s5,a5)
00178 link6 (-d5-1,0,0)(0,0,0)
00179
00180 revolute: link6 (-c6,-s6,a6) (d6*c6,d6*s6,a6)
00181 link1 (-d6-1,0,0)(0,0,0)
00182