| 1 | = Kinematics, Transmission Ratios, and Joint Ranges = |
| 2 | |
| 3 | == 4 DOF and 7 DOF == |
| 4 | A good introduction to coordinate frames, transformations and kinematics is beyond the scope of this document. There are several good introductory robotics books available. We recommend Spong, M.; Hutchinson, S.; Vidyasagar, M. Robot Modeling and Control; 2006 John Wiley & Sons as we use the variant of the Denavit-Hartenberg (D-H) method that is from this book to establish the coordinate frames. |
| 5 | |
| 6 | {{{ |
| 7 | #!div class="center" align="center" |
| 8 | [[Image(htdocs:wam/figure35.png)]] |
| 9 | |
| 10 | '''Figure 35 – WAM 4-DOF dimensions and D-H frames''' |
| 11 | }}} |
| 12 | |
| 13 | Figure 35 shows the 4-DOF WAM system in the zero position. A positive joint motion is based on the right hand rule for each axis. Figure 37 through Figure 40 on the following pages show explicitly each of the four joint kinematic parameters and joint limits. |
| 14 | |
| 15 | {{{ |
| 16 | #!div class="center" align="center" |
| 17 | [[Image(htdocs:wam/figure36.png)]] |
| 18 | |
| 19 | '''Figure 36 – WAM 7-DOF dimensions and D-H frames''' |
| 20 | }}} |
| 21 | Figure 36 shows the entire 7-DOF WAM system in the zero position. A positive joint motion is based on the right hand rule for each axis. Figure 37 through |
| 22 | Figure 43 on the following pages show explicitly each of the seven joint kinematic parameters and joint limits. |
| 23 | |
| 24 | {{{ |
| 25 | #!div class="center" align="center" |
| 26 | [[Image(htdocs:wam/figure37.png)]] |
| 27 | |
| 28 | '''Figure 37: WAM Arm Joint 1 Frames and Limits''' |
| 29 | |
| 30 | [[Image(htdocs:wam/figure38.png)]] |
| 31 | |
| 32 | '''Figure 38: WAM Arm Joint 2 Frames and Limits''' |
| 33 | |
| 34 | [[Image(htdocs:wam/figure39.png)]] |
| 35 | |
| 36 | '''Figure 39: WAM Arm Joint 3 Frames and Limits''' |
| 37 | |
| 38 | [[Image(htdocs:wam/figure40.png)]] |
| 39 | |
| 40 | '''Figure 40: WAM Arm Joint 4 Frames and Limits''' |
| 41 | |
| 42 | [[Image(htdocs:wam/figure41.png)]] |
| 43 | |
| 44 | '''Figure 41: WAM Arm Joint 5 Frames and Limits''' |
| 45 | |
| 46 | [[Image(htdocs:wam/figure42.png)]] |
| 47 | |
| 48 | '''Figure 42: WAM Arm Joint 6 Frames and Limits''' |
| 49 | |
| 50 | [[Image(htdocs:wam/figure43.png)]] |
| 51 | |
| 52 | '''Figure 43: WAM Arm Joint 7 Frames and Limits''' |
| 53 | }}} |
| 54 | Equation 1 below gives the transform between two adjacent D-H coordinate frames. The D-H parameters that were derived from this equation are located in Table 4 below. Note that c and s stand for cos and sin respectively. |
| 55 | |
| 56 | {{{ |
| 57 | #!div class="center" align="center" |
| 58 | {{{ |
| 59 | #!latex |
| 60 | $^{i-l}T_{i}=\left[\begin{array}{cccc} |
| 61 | c\theta_{i} & -s\theta_{i}c\alpha_{i} & s\theta_{i}s\alpha_{i} & a_{i}c\theta_{i}\\ |
| 62 | s\theta_{i} & c\theta_{i}c\alpha_{i} & -c\theta_{i}s\alpha_{i} & a_{i}s\theta_{i}\\ |
| 63 | 0 & s\alpha_{i} & c\alpha_{i} & d_{i}\\ |
| 64 | 0 & 0 & 0 & 1\end{array}\right]$ |
| 65 | }}} |
| 66 | |
| 67 | '''Equation 1: D-H generalized transform matrix''' |
| 68 | |
| 69 | '''Table 4: 4-DOF WAM frame parameters (with blank outer link installed) and 7-DOF WAM frame parameters''' |
| 70 | ||i||a,,i,,||α,,i,,||d,,i,,||θ,,i,,||||||i||a,,i,,||α,,i,,||d,,i,,||θ,,i,,|| |
| 71 | ||1||0||−π/2||0||θ,,1,,||||||1||0||−π/2||0||θ,,1,,|| |
| 72 | ||2||0||π/2||0||θ,,2,,||||||2||0||π/2||0||θ,,2,,|| |
| 73 | ||3||0.045||−π/2||0.55||θ,,3,,||||||3||0.045||−π/2||0.55||θ,,3,,|| |
| 74 | ||4||−0.045||π/2||0||θ,,4,,||||||4||−0.045||π/2||0||θ,,4,,|| |
| 75 | ||Τ||0||0||0.35||||||||5||0||−π/2||0.3||θ,,5,,|| |
| 76 | ||||||||||||||||6||0||π/2||0||θ,,6,,|| |
| 77 | ||||||||||||||||7||0||0||0.06||θ,,7,,|| |
| 78 | ||||||||||||||||Τ||0||0||0|| |
| 79 | }}} |
| 80 | |
| 81 | For example, to generate the transform from coordinate Frame 2 to coordinate Frame 1 (i.e. the position and orientation of Frame 2 described in terms of Frame 1 which is also a rotation about joint 2), use the parameters in the second row of Table 4 as follows: |
| 82 | |
| 83 | {{{ |
| 84 | #!div class="center" align="center" |
| 85 | {{{ |
| 86 | #!latex |
| 87 | $^{1}T_{2}=\left[\begin{array}{cccc} |
| 88 | cos(\theta_{2}) & -sin(\theta_{2})cos(\pi/2) & sin(\theta_{2})sin(\pi/2) & cos(\theta_{2})(0)\\ |
| 89 | sin(\theta_{2}) & cos(\theta_{2})cos(\pi/2) & -cos(\theta_{2})sin(\pi/2) & sin(\theta_{2})(0)\\ |
| 90 | 0 & sin(\pi/2) & cos(\pi/2) & (0)\\ |
| 91 | 0 & 0 & 0 & 1\end{array}\right]$ |
| 92 | |
| 93 | $^{1}T_{2}=\left[\begin{array}{cccc} |
| 94 | cos(\theta_{2}) & 0 & sin(\theta_{2}) & 0\\ |
| 95 | sin(\theta_{2}) & 0 & -cos(\theta_{2}) & 0\\ |
| 96 | 0 & 1 & 0 & 0\\ |
| 97 | 0 & 0 & 0 & 1\end{array}\right]$ |
| 98 | }}} |
| 99 | |
| 100 | '''Equation 2: D-H Matrix Example''' |
| 101 | }}} |
| 102 | |
| 103 | Each of the joints has a mechanical stop that limits the motion. Refer to Table 6 below for a complete listing of the joint limits for each axis. |
| 104 | |
| 105 | {{{ |
| 106 | #!div class="center" align="center" |
| 107 | '''Table 6: Joint Limits''' |
| 108 | ||Joint||Positive Joint Limit Rad (deg)||Negative Joint Limit Rad (deg) |
| 109 | ||1||2.6 (150)||-2.6 (-150) |
| 110 | ||2||2.0 (113)||-2.0 (-113) |
| 111 | ||3||2.8 (157)||-2.8 (-157) |
| 112 | ||4||3.1 (180)||-0.9 (-50) |
| 113 | ||5||1.24 (71)||-4.76 (-273) |
| 114 | ||6||1.6 (90)||-1.6 (-90) |
| 115 | ||7||3.0 (172)||-3.0 (-172) |
| 116 | }}} |
| 117 | |
| 118 | '''Forward Kinematics for the 4-DOF WAM''' |
| 119 | |
| 120 | The forward kinematics of the 4-DOF WAM system is used to determine the end tip location and orientation. These transformations are generated using the parameters in Table 4 on page 64 and the matrix in Equation 1 on page 64. |
| 121 | |
| 122 | {{{ |
| 123 | #!div class="center" align="center" |
| 124 | {{{ |
| 125 | #!latex |
| 126 | $^{4}T_{tool}=\left[\begin{array}{cccc} |
| 127 | u_{x} & v_{x} & w_{x} & P_{x}\\ |
| 128 | u_{y} & v_{y} & w_{y} & P_{y}\\ |
| 129 | u_{z} & v_{z} & w_{z} & P_{z}\\ |
| 130 | 0 & 0 & 0 & 1\end{array}\right]$ |
| 131 | }}} |
| 132 | |
| 133 | '''Equation 3: Tool frame matrix''' |
| 134 | }}} |
| 135 | |
| 136 | You define the frame for your specific end-effector. The forward kinematics are determined for any frame on the robot by mulitplying all of the transforms up to and including the final frame. To determine the tool end tip location and orientation use the following equation: |
| 137 | |
| 138 | {{{ |
| 139 | #!div class="center" align="center" |
| 140 | {{{ |
| 141 | #!latex |
| 142 | $^{0}T_{Tool}=^{0}T_{1}^{1}T_{2}^{2}T_{3}^{3}T_{4}^{4}T_{Tool}$ |
| 143 | }}} |
| 144 | |
| 145 | '''Equation 4: Tool end tip position and orientation equation for the 4-DOF WAM''' |
| 146 | }}} |
| 147 | |
| 148 | '''Forward Kinematics for the 7-DOF WAM''' |
| 149 | |
| 150 | As with the previous example, you define the frame for your specific end-effector. The forward kinematics are determined for any frame on the robot by mulitplying all of the transforms up to and including the final frame. To determine the end tip location and orientation use the following equation: |
| 151 | |
| 152 | {{{ |
| 153 | #!div class="center" align="center" |
| 154 | {{{ |
| 155 | #!latex |
| 156 | $^{0}T_{Tool}=^{0}T_{1}^{1}T_{2}^{2}T_{3}^{3}T_{4}^{4}T_{5}^{5}T_{6}^{6}T_{7}^{7}T_{Tool}$ |
| 157 | }}} |
| 158 | |
| 159 | '''Equation 5: Tool end tip position and orientation equation for the 7-DOF WAM''' |
| 160 | }}} |
| 161 | |
| 162 | === 4 DOF with Gimbals === |
| 163 | A 4-DOF WAM Arm can be outfitted with optional 3-axis non-motorized gimbals that give precise angular feedback. The kinematics of the first 4 joints is identical to a 4-DOF WAM Arm. The kinematics for the additional 3 axes is shown in Figure 44. |
| 164 | |
| 165 | {{{ |
| 166 | #!div class="center" align="center" |
| 167 | [[Image(htdocs:wam/figure44.png)]] |
| 168 | |
| 169 | '''Figure 44: Denavit-Hartenberg Frames – 4-DOF + Gimbals''' |
| 170 | }}} |
| 171 | |
| 172 | Figure 44 shows the WAM Gimbals in the zero position. A positive joint motion is based on the right hand rule for each axis. The D-H parameters that were derived from Figure 44 are located in Table 7 below. |
| 173 | |
| 174 | {{{ |
| 175 | #!div class="center" align="center" |
| 176 | '''Table 7: 4-DOF WAM + Gimbals DH Parameters''' |
| 177 | ||i||a,,i,,||α,,i,,||d,,i,,||θ,,i,, |
| 178 | ||1||0||-π/2||0||θ,,1,, |
| 179 | ||2||0||π/2||0||θ,,2,, |
| 180 | ||3||0.045||-π/2||0.55||θ,,3,, |
| 181 | ||4||0.4||-π/2||0||θ,,4,, |
| 182 | ||5||0||π/2||0||θ,,5,,-π/2 |
| 183 | ||6||0||-π/2||0.155||θ,,6,,-π/2 |
| 184 | ||7||0||0||0||θ,,7,, |
| 185 | }}} |
| 186 | |
| 187 | == Motor-to-Joint Transformations == |
| 188 | '''Motor-to-Joint Position Transformations''' |
| 189 | |
| 190 | The following transformations show the change in joint positions as a function of motor positions. The input transmission ratios and the differential transmission ratios are calculated from known pulley, pinion, and cable diameters. |
| 191 | |
| 192 | {{{ |
| 193 | #!div class="center" align="center" |
| 194 | '''Table 8: Arm Transmission Ratios''' |
| 195 | ||Parameter||Value |
| 196 | ||N,,1,,||42.0 |
| 197 | ||N,,2,,||28.25 |
| 198 | ||N,,3,,||28.25 |
| 199 | ||n,,3,,||1.68 |
| 200 | ||N,,4,,||18.0 |
| 201 | ||N,,5,,||9.48 |
| 202 | ||N,,6,,||9.48 |
| 203 | ||N,,7,,||14.93 |
| 204 | ||n,,6,,||1 |
| 205 | |
| 206 | {{{ |
| 207 | #!latex |
| 208 | $\left[\begin{array}{c} |
| 209 | J\theta_{1}\\ |
| 210 | J\theta_{2}\\ |
| 211 | J\theta_{3}\\ |
| 212 | J\theta_{4}\end{array}\right]=\left[\begin{array}{cccc} |
| 213 | \frac{-1}{N_{1}} & 0 & 0 & 0\\ |
| 214 | 0 & \frac{1}{2N_{2}} & \frac{-1}{2N_{2}} & 0\\ |
| 215 | 0 & \frac{-n_{3}}{2N_{2}} & \frac{-n_{3}}{2N_{2}} & 0\\ |
| 216 | 0 & 0 & 0 & \frac{-1}{N_{4}}\end{array}\right]\left[\begin{array}{c} |
| 217 | M\theta_{1}\\ |
| 218 | M\theta_{2}\\ |
| 219 | M\theta_{3}\\ |
| 220 | M\theta_{4}\end{array}\right]$ |
| 221 | }}} |
| 222 | |
| 223 | '''Equation 6: WAM Motor-to-Joint position transformations ''' |
| 224 | |
| 225 | {{{ |
| 226 | #!latex |
| 227 | $\left[\begin{array}{c} |
| 228 | J\theta_{5}\\ |
| 229 | J\theta_{6}\\ |
| 230 | J\theta_{7}\end{array}\right]=\left[\begin{array}{ccc} |
| 231 | \frac{1}{2N_{5}} & \frac{1}{2N_{5}} & 0\\ |
| 232 | \frac{-n_{6}}{2N_{5}} & \frac{n_{6}}{2N_{5}} & 0\\ |
| 233 | 0 & 0 & \frac{1}{N_{7}}\end{array}\right]\left[\begin{array}{c} |
| 234 | M\theta_{5}\\ |
| 235 | M\theta_{6}\\ |
| 236 | M\theta_{7}\end{array}\right]$ |
| 237 | }}} |
| 238 | |
| 239 | '''Equation 7: Wrist Motor-to-Joint position transformations''' |
| 240 | }}} |
| 241 | |
| 242 | The motor position can also be derived from joint space by taking the inverse of the multiplying matrix. For convenience, they are as follows: |
| 243 | |
| 244 | {{{ |
| 245 | #!div class="center" align="center" |
| 246 | {{{ |
| 247 | #!latex |
| 248 | $\left[\begin{array}{c} |
| 249 | M\theta_{1}\\ |
| 250 | M\theta_{2}\\ |
| 251 | M\theta_{3}\\ |
| 252 | M\theta_{4}\end{array}\right]=\left[\begin{array}{cccc} |
| 253 | -N_{1} & 0 & 0 & 0\\ |
| 254 | 0 & N_{2} & \frac{-N_{2}}{n_{3}} & 0\\ |
| 255 | 0 & -N_{2} & \frac{-N_{2}}{n_{3}} & 0\\ |
| 256 | 0 & 0 & 0 & -N_{4}\end{array}\right]\left[\begin{array}{c} |
| 257 | J\theta_{1}\\ |
| 258 | J\theta_{2}\\ |
| 259 | J\theta_{3}\\ |
| 260 | J\theta_{4}\end{array}\right]$ |
| 261 | }}} |
| 262 | |
| 263 | '''Equation 8: Arm Joint-to-Motor position transformations ''' |
| 264 | |
| 265 | {{{ |
| 266 | #!latex |
| 267 | $\left[\begin{array}{c} |
| 268 | M\theta_{5}\\ |
| 269 | M\theta_{6}\\ |
| 270 | M\theta_{7}\end{array}\right]=\left[\begin{array}{ccc} |
| 271 | N_{5} & \frac{-N_{5}}{n_{6}} & 0\\ |
| 272 | N_{5} & \frac{N_{5}}{n_{6}} & 0\\ |
| 273 | 0 & 0 & -N_{7}\end{array}\right]\left[\begin{array}{c} |
| 274 | J\theta_{5}\\ |
| 275 | J\theta_{6}\\ |
| 276 | J\theta_{7}\end{array}\right]$ |
| 277 | }}} |
| 278 | |
| 279 | '''Equation 9: Wrist Joint-to-Motor position transformation''' |
| 280 | }}} |
| 281 | |
| 282 | '''Motor-to-Joint Torque Transformations''' |
| 283 | |
| 284 | Similar to the position transformations the following equations determine the joint torque from the motor torque: |
| 285 | |
| 286 | {{{ |
| 287 | #!div class="center" align="center" |
| 288 | {{{ |
| 289 | #!latex |
| 290 | $\left[\begin{array}{c} |
| 291 | J\tau_{1}\\ |
| 292 | J\tau_{2}\\ |
| 293 | J\tau_{3}\\ |
| 294 | J\tau_{4}\end{array}\right]=\left[\begin{array}{cccc} |
| 295 | -N_{1} & 0 & 0 & 0\\ |
| 296 | 0 & N_{2} & -N_{2} & 0\\ |
| 297 | 0 & \frac{-N_{2}}{n_{3}} & \frac{-N_{2}}{n_{3}} & 0\\ |
| 298 | 0 & 0 & 0 & -N_{4}\end{array}\right]\left[\begin{array}{c} |
| 299 | M\tau_{1}\\ |
| 300 | M\tau_{2}\\ |
| 301 | M\tau_{3}\\ |
| 302 | M\tau_{4}\end{array}\right]$ |
| 303 | }}} |
| 304 | |
| 305 | '''Equation 10: Arm Motor-to-Joint torque transformation ''' |
| 306 | |
| 307 | {{{ |
| 308 | #!latex |
| 309 | $\left[\begin{array}{c} |
| 310 | J\tau_{5}\\ |
| 311 | J\tau_{6}\\ |
| 312 | J\tau_{7}\end{array}\right]=\left[\begin{array}{ccc} |
| 313 | N_{5} & N_{5} & 0\\ |
| 314 | \frac{-N_{5}}{n_{6}} & \frac{N_{5}}{n_{6}} & 0\\ |
| 315 | 0 & 0 & -N_{7}\end{array}\right]\left[\begin{array}{c} |
| 316 | M\tau_{5}\\ |
| 317 | M\tau_{6}\\ |
| 318 | M\tau_{7}\end{array}\right]$ |
| 319 | }}} |
| 320 | |
| 321 | '''Equation 11: Wrist Motor-to-Joint transformations''' |
| 322 | }}} |
| 323 | |
| 324 | The following equations determine motor torque from the joint torque: |
| 325 | |
| 326 | {{{ |
| 327 | #!div class="center" align="center" |
| 328 | {{{ |
| 329 | #!latex |
| 330 | $\left[\begin{array}{c} |
| 331 | M\tau_{1}\\ |
| 332 | M\tau_{2}\\ |
| 333 | M\tau_{3}\\ |
| 334 | M\tau_{4}\end{array}\right]=\left[\begin{array}{cccc} |
| 335 | \frac{-1}{N_{1}} & 0 & 0 & 0\\ |
| 336 | 0 & \frac{1}{2N_{2}} & \frac{-n_{3}}{2N_{2}} & 0\\ |
| 337 | 0 & \frac{-1}{2N_{2}} & \frac{-n_{3}}{2N_{2}} & 0\\ |
| 338 | 0 & 0 & 0 & \frac{-1}{N_{7}}\end{array}\right]\left[\begin{array}{c} |
| 339 | J\tau_{1}\\ |
| 340 | J\tau_{2}\\ |
| 341 | J\tau_{3}\\ |
| 342 | J\tau_{4}\end{array}\right]$ |
| 343 | }}} |
| 344 | |
| 345 | '''Equation 12: Arm Joint-to-Motor torque transformations ''' |
| 346 | |
| 347 | {{{ |
| 348 | #!latex |
| 349 | $\left[\begin{array}{c} |
| 350 | M\tau_{5}\\ |
| 351 | M\tau_{6}\\ |
| 352 | M\tau_{7}\end{array}\right]=\left[\begin{array}{ccc} |
| 353 | \frac{1}{2N_{5}} & \frac{-n_{6}}{2N_{5}} & 0\\ |
| 354 | \frac{1}{2N_{5}} & \frac{n_{6}}{2N_{5}} & 0\\ |
| 355 | 0 & 0 & \frac{-1}{N_{7}}\end{array}\right]\left[\begin{array}{c} |
| 356 | J\tau_{5}\\ |
| 357 | J\tau_{6}\\ |
| 358 | J\tau_{7}\end{array}\right]$ |
| 359 | }}} |
| 360 | |
| 361 | '''Equation 13: Wrist Joint-to-Motor torque transformations''' |
| 362 | }}} |