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