Version 2 (modified by kpm, 13 years ago) (diff)

--

Kinematics, Transmission Ratios, and Joint Ranges

4 DOF and 7 DOF

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.

figure35.png

Figure 35 – WAM 4-DOF dimensions and D-H frames

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 show explicitly each of the four joint kinematic parameters and joint limits.

figure36.png

Figure 36 – WAM 7-DOF dimensions and D-H frames

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 Figure 43 show explicitly each of the seven joint kinematic parameters and joint limits.

figure37.png

Figure 37: WAM Arm Joint 1 Frames and Limits

figure38.png

Figure 38: WAM Arm Joint 2 Frames and Limits

figure39.png

Figure 39: WAM Arm Joint 3 Frames and Limits

figure40.png

Figure 40: WAM Arm Joint 4 Frames and Limits

figure41.png

Figure 41: WAM Arm Joint 5 Frames and Limits

figure42.png

Figure 42: WAM Arm Joint 6 Frames and Limits

figure43.png

Figure 43: WAM Arm Joint 7 Frames and Limits

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.

$^{i-l}T_{i}=\left[\begin{array}{cccc}
c\theta_{i} & -s\theta_{i}c\alpha_{i} & s\theta_{i}s\alpha_{i} & a_{i}c\theta_{i}\\
s\theta_{i} & c\theta_{i}c\alpha_{i} & -c\theta_{i}s\alpha_{i} & a_{i}s\theta_{i}\\
0 & s\alpha_{i} & c\alpha_{i} & d_{i}\\
0 & 0 & 0 & 1\end{array}\right]$

Equation 1: D-H generalized transform matrix

Table 4: 4-DOF WAM frame parameters (with blank outer link installed) and 7-DOF WAM frame parameters

iaiαidiθiiaiαidiθi
10−π/20θ110−π/20θ1
20π/20θ220π/20θ2
30.045−π/20.55θ330.045−π/20.55θ3
4−0.045π/20θ44−0.045π/20θ4
Τ000.3550−π/20.3θ5
60π/20θ6
7000.06θ7
Τ000

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:

$^{1}T_{2}=\left[\begin{array}{cccc}
cos(\theta_{2}) & -sin(\theta_{2})cos(\pi/2) & sin(\theta_{2})sin(\pi/2) & cos(\theta_{2})(0)\\
sin(\theta_{2}) & cos(\theta_{2})cos(\pi/2) & -cos(\theta_{2})sin(\pi/2) & sin(\theta_{2})(0)\\
0 & sin(\pi/2) & cos(\pi/2) & (0)\\
0 & 0 & 0 & 1\end{array}\right]$

$^{1}T_{2}=\left[\begin{array}{cccc}
cos(\theta_{2}) & 0 & sin(\theta_{2}) & 0\\
sin(\theta_{2}) & 0 & -cos(\theta_{2}) & 0\\
0 & 1 & 0 & 0\\
0 & 0 & 0 & 1\end{array}\right]$

Equation 2: D-H Matrix Example

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.

Table 6: Joint Limits

JointPositive Joint Limit Rad (deg)Negative Joint Limit Rad (deg)
12.6 (150)-2.6 (-150)
22.0 (113)-2.0 (-113)
32.8 (157)-2.8 (-157)
43.1 (180)-0.9 (-50)
51.24 (71)-4.76 (-273)
61.6 (90)-1.6 (-90)
73.0 (172)-3.0 (-172)

Forward Kinematics for the 4-DOF WAM

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 and the matrix in Equation 1.

$^{4}T_{tool}=\left[\begin{array}{cccc}
u_{x} & v_{x} & w_{x} & P_{x}\\
u_{y} & v_{y} & w_{y} & P_{y}\\
u_{z} & v_{z} & w_{z} & P_{z}\\
0 & 0 & 0 & 1\end{array}\right]$

Equation 3: Tool frame matrix

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:

$^{0}T_{Tool}=^{0}T_{1}^{1}T_{2}^{2}T_{3}^{3}T_{4}^{4}T_{Tool}$

Equation 4: Tool end tip position and orientation equation for the 4-DOF WAM

Forward Kinematics for the 7-DOF WAM

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:

$^{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}$

Equation 5: Tool end tip position and orientation equation for the 7-DOF WAM

4 DOF with Gimbals

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.

figure44.png

Figure 44: Denavit-Hartenberg Frames – 4-DOF + Gimbals

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.

Table 7: 4-DOF WAM + Gimbals DH Parameters

iaiαidiθi
10-π/20θ1
20π/20θ2
30.045-π/20.55θ3
40.4-π/20θ4
50π/20θ5-π/2
60-π/20.155θ6-π/2
7000θ7

Motor-to-Joint Transformations

Motor-to-Joint Position Transformations

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.

Table 8: Arm Transmission Ratios

ParameterValue
N142.0
N228.25
N328.25
n31.68
N418.0
N59.48
N69.48
N714.93
n61
$\left[\begin{array}{c}
J\theta_{1}\\
J\theta_{2}\\
J\theta_{3}\\
J\theta_{4}\end{array}\right]=\left[\begin{array}{cccc}
\frac{-1}{N_{1}} & 0 & 0 & 0\\
0 & \frac{1}{2N_{2}} & \frac{-1}{2N_{2}} & 0\\
0 & \frac{-n_{3}}{2N_{2}} & \frac{-n_{3}}{2N_{2}} & 0\\
0 & 0 & 0 & \frac{-1}{N_{4}}\end{array}\right]\left[\begin{array}{c}
M\theta_{1}\\
M\theta_{2}\\
M\theta_{3}\\
M\theta_{4}\end{array}\right]$

Equation 6: WAM Motor-to-Joint position transformations

$\left[\begin{array}{c}
J\theta_{5}\\
J\theta_{6}\\
J\theta_{7}\end{array}\right]=\left[\begin{array}{ccc}
\frac{1}{2N_{5}} & \frac{1}{2N_{5}} & 0\\
\frac{-n_{6}}{2N_{5}} & \frac{n_{6}}{2N_{5}} & 0\\
0 & 0 & \frac{1}{N_{7}}\end{array}\right]\left[\begin{array}{c}
M\theta_{5}\\
M\theta_{6}\\
M\theta_{7}\end{array}\right]$

Equation 7: Wrist Motor-to-Joint position transformations

The motor position can also be derived from joint space by taking the inverse of the multiplying matrix. For convenience, they are as follows:

$\left[\begin{array}{c}
M\theta_{1}\\
M\theta_{2}\\
M\theta_{3}\\
M\theta_{4}\end{array}\right]=\left[\begin{array}{cccc}
-N_{1} & 0 & 0 & 0\\
0 & N_{2} & \frac{-N_{2}}{n_{3}} & 0\\
0 & -N_{2} & \frac{-N_{2}}{n_{3}} & 0\\
0 & 0 & 0 & -N_{4}\end{array}\right]\left[\begin{array}{c}
J\theta_{1}\\
J\theta_{2}\\
J\theta_{3}\\
J\theta_{4}\end{array}\right]$

Equation 8: Arm Joint-to-Motor position transformations

$\left[\begin{array}{c}
M\theta_{5}\\
M\theta_{6}\\
M\theta_{7}\end{array}\right]=\left[\begin{array}{ccc}
N_{5} & \frac{-N_{5}}{n_{6}} & 0\\
N_{5} & \frac{N_{5}}{n_{6}} & 0\\
0 & 0 & -N_{7}\end{array}\right]\left[\begin{array}{c}
J\theta_{5}\\
J\theta_{6}\\
J\theta_{7}\end{array}\right]$

Equation 9: Wrist Joint-to-Motor position transformation

Motor-to-Joint Torque Transformations

Similar to the position transformations the following equations determine the joint torque from the motor torque:

$\left[\begin{array}{c}
J\tau_{1}\\
J\tau_{2}\\
J\tau_{3}\\
J\tau_{4}\end{array}\right]=\left[\begin{array}{cccc}
-N_{1} & 0 & 0 & 0\\
0 & N_{2} & -N_{2} & 0\\
0 & \frac{-N_{2}}{n_{3}} & \frac{-N_{2}}{n_{3}} & 0\\
0 & 0 & 0 & -N_{4}\end{array}\right]\left[\begin{array}{c}
M\tau_{1}\\
M\tau_{2}\\
M\tau_{3}\\
M\tau_{4}\end{array}\right]$

Equation 10: Arm Motor-to-Joint torque transformation

$\left[\begin{array}{c}
J\tau_{5}\\
J\tau_{6}\\
J\tau_{7}\end{array}\right]=\left[\begin{array}{ccc}
N_{5} & N_{5} & 0\\
\frac{-N_{5}}{n_{6}} & \frac{N_{5}}{n_{6}} & 0\\
0 & 0 & -N_{7}\end{array}\right]\left[\begin{array}{c}
M\tau_{5}\\
M\tau_{6}\\
M\tau_{7}\end{array}\right]$

Equation 11: Wrist Motor-to-Joint transformations

The following equations determine motor torque from the joint torque:

$\left[\begin{array}{c}
M\tau_{1}\\
M\tau_{2}\\
M\tau_{3}\\
M\tau_{4}\end{array}\right]=\left[\begin{array}{cccc}
\frac{-1}{N_{1}} & 0 & 0 & 0\\
0 & \frac{1}{2N_{2}} & \frac{-n_{3}}{2N_{2}} & 0\\
0 & \frac{-1}{2N_{2}} & \frac{-n_{3}}{2N_{2}} & 0\\
0 & 0 & 0 & \frac{-1}{N_{7}}\end{array}\right]\left[\begin{array}{c}
J\tau_{1}\\
J\tau_{2}\\
J\tau_{3}\\
J\tau_{4}\end{array}\right]$

Equation 12: Arm Joint-to-Motor torque transformations

$\left[\begin{array}{c}
M\tau_{5}\\
M\tau_{6}\\
M\tau_{7}\end{array}\right]=\left[\begin{array}{ccc}
\frac{1}{2N_{5}} & \frac{-n_{6}}{2N_{5}} & 0\\
\frac{1}{2N_{5}} & \frac{n_{6}}{2N_{5}} & 0\\
0 & 0 & \frac{-1}{N_{7}}\end{array}\right]\left[\begin{array}{c}
J\tau_{5}\\
J\tau_{6}\\
J\tau_{7}\end{array}\right]$

Equation 13: Wrist Joint-to-Motor torque transformations