When the rotation \(R_i\) is around a fixed frame, it is pre-multiplied by the current sequence of rotations. If the rotation \(R_i\) is around the current frame, it is post-multiplied by the current sequence of rotations.
Now that the rotation sequence is completed, the sequence of rotations are evaluated. Before that, some simplification is made as follows
\begin {align*} R & = R_{x,\beta } \overbrace {R_{z,-\alpha } R_{z,\alpha } }^{I} R_{x,-\beta } R_{y,\gamma } \\ & =\overbrace { R_{x,\beta } R_{x,-\beta }}^{I} R_{y,\gamma }\\ & = R_{y,\gamma } \end {align*}
Therefore\[ R=R_{y,\gamma }=\begin {pmatrix} \cos \gamma & 0 & \sin \gamma \\ 0 & 1 & 0\\ -\sin \gamma & 0 & \cos \gamma \end {pmatrix} = \begin {pmatrix} \cos 90^{0} & 0 & \sin 90^{0}\\ 0 & 1 & 0\\ -\sin 90^{0} & 0 & \cos 90^{0} \end {pmatrix} = \begin {pmatrix} 0 & 0 & 1\\ 0 & 1 & 0\\ -1 & 0 & 0 \end {pmatrix} \]
Only angle \(\gamma \) was needed in finding the final result. The above result shows that the final orientation is as if one rotation of \(+90\) degree was made around the fixed \(y\) axes. The following diagram shows the result after each rotation, which confirms the above result.3
The homogeneous transformation matrix based on the use of the DH convention contains only 4 parameters \(d,a,\alpha ,\theta \). Since a general rigid body transformation requires 6 parameters (3 angles for orientations, and 3 for translation), this implies the DH homogeneous transformation matrix can not be used to represent any arbitrary rigid body transformation. However, the DH convention can be used to represent any rigid body transformation that meets two conditions, as specified on page 78 of the text book. These are
At least one of the above two DH constraints need to be violated in order to come up with a configuration that cannot be described using the DH transformation matrix. This is done by making \(x_1\) axis not perpendicular to \(z_0\) axis. The following diagram illustrates this
The first step is to assign the \(z_i\) axis for each link. There are 5 links numbered \(0,1,2,3,4\) and four joints numbered \(1,2,3,4\). Link 0 is fixed and does not move. Frame \(i\) is attached to link \(i\) but its \(z_i\) axis is oriented along the line of motion of joint \(i+1\). Once all the \(z\) axis are established, then the rest of the frames are configured by insuring that that each \(x\) axis is perpendicular to the last frame \(z\) axis.
So the rule to follow is that \(x_{i}\) axis must be perpendicular to \(z_{i-1}\) axis.
The frames are drawn below. The following choices were made. \(z_0\) intersects \(z_1\) (case 3 in the textbook), hence \(x_1\) is arbitrary. The origin \(o_1\) is the point of intersection of \(z_1\) and \(z_0\) as shown in the problem statement.
Next, \(z_1\) and \(z_2\) also intersect (case 3). The choice of \(x_2\) is arbitrary. The origin \(o_2\) could be located also where \(z_1\) intersect \(z_2\). But any point along \(z_2\) will also work. Here \(o_2\) was placed at point \(p_2\).
Next, \(z_2\) is parallel to \(z_3\). This is case (2) in the textbook. There are infinitely many common normals. Here origin \(o_3\) can be anywhere along \(z_{3}\). It is placed at start of the end effector as shown.
The following diagram shows the frames locations.
| \(i\) | \(\theta _{i}\) | \(d_{i}\) | \(a_{i}\) | \(\alpha _{i}\) |
| \(1\) | \(0\) | \(L_{1}+d_{1}\) | \(0\) | \(90^{0}\) |
| \(2\) | \(90^{0}+\theta _{2}\) | \(0\) | \(0\) | \(90^{0}\) |
| \(3\) | \(90^{0}\) | \(L_{3}+d_{3}\) | \(0\) | \(0\) |
| \(4\) | \(\theta _{4}\) | \(L_{4}\) | \(0\) | \(90^{0}\) |
The homogeneous transformation matrices \(T_{i}^{i-1}\) are now evaluated, and then \(T_{4}^{0}\) is found in order to verify that the location of the end effector. To verify, when \(\theta _2=0,\theta _4=0\), then the end effector \(x,y,z\) position relative to the base frame should be located at \begin {align*} x & =d_3 + L_3+L_4\\ y & =0\\ z & =L_1+d_1 \end {align*}
These are The homogeneous transformation matrices \(T_{i}^{i-1}\) \begin {align*} T_{1}^{0} & =\left ( \begin {array} [c]{cccc}1 & 0 & 0 & 0\\ 0 & 0 & -1 & 0\\ 0 & 1 & 0 & d_{1}+L_{1}\\ 0 & 0 & 0 & 1 \end {array} \right ) \\ T_{2}^{1} & =\left ( \begin {array} [c]{cccc}-\sin \theta _{2} & 0 & \cos \theta _{2} & 0\\ \cos \theta _{2} & 0 & \sin \theta _{2} & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 \end {array} \right ) \\ T_{3}^{2} & =\left ( \begin {array} [c]{cccc}0 & -1 & 0 & 0\\ 1 & 0 & 0 & 0\\ 0 & 0 & 1 & d_{3}+L_{3}\\ 0 & 0 & 0 & 1 \end {array} \right ) \\ T_{4}^{3} & =\left ( \begin {array} [c]{cccc}\cos \theta _{4} & 0 & \sin \theta _{4} & 0\\ \sin \theta _{4} & 0 & -\cos \theta _{4} & 0\\ 0 & 1 & 0 & L_{4}\\ 0 & 0 & 0 & 1 \end {array} \right ) \end {align*}
\(T_{4}^{0}=T_{1}^{0}T_{2}^{1}T_{3}^{2}T_{4}^{3}\) was found and evaluated for \(\theta _2=0\). The result is \[ \boxed { \left ( \begin {array}[c]{cccc}0 & 1 & 0 & d_{3}+L_{3}+L_{4}\\ -1 & 0 & 0 & 0\\ 0 & 0 & 1 & d_{1}+L_{1}\\ 0 & 0 & 0 & 1 \end {array} \right ) } \] Which shows the correct position vector of the end effector frame.
The following is the Matlab function DH2T
function T = DH2T(a,alpha,d,theta) %generated the transformation matrix for a DH table row %ME 739, Univ. Of Wisconsin, Madison. Spring 2015 T=[cos(theta) -sin(theta)*cos(alpha) sin(theta)*sin(alpha) a*cos(theta); sin(theta) cos(theta)*cos(alpha) -cos(theta)*sin(alpha) a*sin(theta); 0 sin(alpha) cos(alpha) d; 0 0 0 1 ]; end
The following is the script used and below it is the output generated.
clear all; close all; clc %case 1 d=0; a=10; alpha=0; theta=0; T1=DH2T(a,alpha,d,theta) %case 2 d=10; a=10; alpha=0; theta=0; T2=DH2T(a,alpha,d,theta) %case 3 d=10; a=10; alpha=pi; theta=0; T3=DH2T(a,alpha,d,theta) %case 4 d=0; a=10; alpha=pi; theta=pi; T4=DH2T(a,alpha,d,theta)
T1 = 1 0 0 10 0 1 0 0 0 0 1 0 0 0 0 1 T2 = 1 0 0 10 0 1 0 0 0 0 1 10 0 0 0 1 T3 = 1 0 0 10 0 -1 -1.2246e-16 0 0 1.2246e-16 -1 10 0 0 0 1 T4 = -1 1.2246e-16 1.4998e-32 -10 1.2246e-16 1 1.2246e-16 1.2246e-15 0 1.2246e-16 -1 0 0 0 0 1
\begin {align*} J_{v} & =\begin {pmatrix} \frac {\partial x}{\partial \theta _{1}} & \frac {\partial x}{\partial \theta _{2}} & \frac {\partial x}{\partial d_{3}}\\ \frac {\partial y}{\partial \theta _{1}} & \frac {\partial y}{\partial \theta _{2}} & \frac {\partial y}{\partial d_{3}}\\ \frac {\partial z}{\partial \theta _{1}} & \frac {\partial z}{\partial \theta _{2}} & \frac {\partial z}{\partial d_{3}}\end {pmatrix} \\ & =\begin {pmatrix} \frac {\partial }{\partial \theta _{1}}\cos \theta _{1}\cos \theta _{2}\left ( L_{2}+d_{3}\right ) & \frac {\partial }{\partial \theta _{2}}\cos \theta _{1}\cos \theta _{2}\left ( L_{2}+d_{3}\right ) & \frac {\partial }{\partial d_{3}}\cos \theta _{1}\cos \theta _{2}\left ( L_{2}+d_{3}\right ) \\ \frac {\partial }{\partial \theta _{1}}\sin \theta _{1}\cos \theta _{2}\left ( L_{2}+d_{3}\right ) & \frac {\partial }{\partial \theta _{2}}\sin \theta _{1}\cos \theta _{2}\left ( L_{2}+d_{3}\right ) & \frac {\partial }{\partial d_{3}}\sin \theta _{1}\cos \theta _{2}\left ( L_{2}+d_{3}\right ) \\ \frac {\partial }{\partial \theta _{1}}\left ( L_{1}+\sin \theta _{2}\left ( L_{2}+d_{3}\right ) \right ) & \frac {\partial }{\partial \theta _{2}}\left ( L_{1}+\sin \theta _{2}\left ( L_{2}+d_{3}\right ) \right ) & \frac {\partial }{\partial d_{3}}\left ( L_{1}+\sin \theta _{2}\left ( L_{2}+d_{3}\right ) \right ) \end {pmatrix} \\ & =\begin {pmatrix} -\sin \theta _{1}\cos \theta _{2}\left ( L_{2}+d_{3}\right ) & -\cos \theta _{1}\sin \theta _{2}\left ( L_{2}+d_{3}\right ) & \cos \theta _{1}\cos \theta _{2}\\ \cos \theta _{1}\cos \theta _{2}\left ( L_{2}+d_{3}\right ) & -\sin \theta _{1}\sin \theta _{2}\left ( L_{2}+d_{3}\right ) & \sin \theta _{1}\cos \theta _{2}\\ 0 & \cos \theta _{2}\left ( L_{2}+d_{3}\right ) & \sin \theta _{2}\end {pmatrix} \end {align*}
\begin {align*} v & =J_{v}\dot {q}\\ & =\begin {pmatrix} -\sin \theta _{1}\cos \theta _{2}\left ( L_{2}+d_{3}\right ) & -\cos \theta _{1}\sin \theta _{2}\left ( L_{2}+d_{3}\right ) & \cos \theta _{1}\cos \theta _{2}\\ \cos \theta _{1}\cos \theta _{2}\left ( L_{2}+d_{3}\right ) & -\sin \theta _{1}\sin \theta _{2}\left ( L_{2}+d_{3}\right ) & \sin \theta _{1}\cos \theta _{2}\\ 0 & \cos \theta _{2}\left ( L_{2}+d_{3}\right ) & \sin \theta _{2}\end {pmatrix}\begin {pmatrix} 1\\ 0\\ 1 \end {pmatrix} \\ & =\begin {pmatrix} -\sin \pi \cos \frac {\pi }{2}\left ( L_{2}+L_{2}\right ) & -\cos \pi \sin \frac {\pi }{2}\left ( L_{2}+L_{2}\right ) & \cos \pi \cos \frac {\pi }{2}\\ \cos \pi \cos \frac {\pi }{2}\left ( L_{2}+L_{2}\right ) & -\sin \pi \sin \frac {\pi }{2}\left ( L_{2}+L_{2}\right ) & \sin \pi \cos \frac {\pi }{2}\\ 0 & \cos \frac {\pi }{2}\left ( L_{2}+L_{2}\right ) & \sin \frac {\pi }{2}\end {pmatrix}\begin {pmatrix} 1\\ 0\\ 1 \end {pmatrix} \\ & =\begin {pmatrix} 0 & 2L_{2} & 0\\ 0 & 0 & 0\\ 0 & 0 & 1 \end {pmatrix}\begin {pmatrix} 1\\ 0\\ 1 \end {pmatrix} \\ & =\begin {pmatrix} 0\\ 0\\ 1 \end {pmatrix} \end {align*}
Mathematically, \(J_{v}\) is singular when its determinant is zero, or when \(J_{v}\) has a zero eigenvalue. The determinant of the Jacobian 4 was found to be \[ \boxed { | J_v | = (d_3+L_2)^2 \cos \theta _2 } \] The above is zero when \(\cos \theta _2=0\). This occurs when \(\theta _2=\pm \frac {\pi }{2}\) (and any odd integer multiple of this value).
This implies that the singularity occurs when the arm is fully extended vertically in the upward position, or if physically possible, when the arm is fully extended vertically but in the downwards position.
The frames are first assigned to each link by insuring that that the z axis of each frame follows the DH convention, which means \(z_i\) axis attached to link \(i\) is aligned in the direction of motion of joint \(i+1\).
For a revolute joint, this will be its axis of spin, and for prismatic joint, this will be its direction of sliding. Once this is done the forward kinematics matrices are found using either the DH table method or using homogeneous transformation by direct inspection. In this case, the homogeneous transformation by direct inspection method was used. The frames are assigned as follows
Define the matrices \(T^0_1,T^1_2,T^2_3\) using homogeneous transformation. Since \(x'=J_0 q'\) then to find \(J_0\) we need to first find forward kinematics. By inspection, we find each transformation matrix to be
\begin {align*} T^0_1 &= \left ( \begin {array}{cccc} \cos \left (\theta _1\right ) & 0 & \sin \left (\theta _1\right ) & L \cos \left (\theta _1\right ) \\ \sin \left (\theta _1\right ) & 0 & -\cos \left (\theta _1\right ) & L \sin \left (\theta _1\right ) \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ \end {array} \right )\\ T^1_2 &= \left ( \begin {array}{cccc} \cos \left (\theta _2\right ) & 0 & -\sin \left (\theta _2\right ) & L \cos \left (\theta _2\right ) \\ \sin \left (\theta _2\right ) & 0 & \cos \left (\theta _2\right ) & L \sin \left (\theta _2\right ) \\ 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ \end {array} \right )\\ T^2_3 &= \left ( \begin {array}{cccc} \cos \left (\theta _3\right ) & 0 & -\sin \left (\theta _3\right ) & L \cos \left (\theta _3\right ) \\ \sin \left (\theta _3\right ) & 0 & \cos \left (\theta _3\right ) & L \sin \left (\theta _3\right ) \\ 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ \end {array} \right ) \end {align*}
Therefore \begin {align*} T^0_2 &= T^0_1 T^1_2 =\left ( \begin {array}{cccc} \cos \left (\theta _1\right ) \cos \left (\theta _2\right ) & -\sin \left (\theta _1\right ) & -\cos \left (\theta _1\right ) \sin \left (\theta _2\right ) & L \cos \left (\theta _1\right )+L \cos \left (\theta _2\right ) \cos \left (\theta _1\right ) \\ \cos \left (\theta _2\right ) \sin \left (\theta _1\right ) & \cos \left (\theta _1\right ) & -\sin \left (\theta _1\right ) \sin \left (\theta _2\right ) & L \sin \left (\theta _1\right )+L \cos \left (\theta _2\right ) \sin \left (\theta _1\right ) \\ \sin \left (\theta _2\right ) & 0 & \cos \left (\theta _2\right ) & L \sin \left (\theta _2\right ) \\ 0 & 0 & 0 & 1 \\ \end {array} \right )\\ T^0_3 &= T^0_2 T^2_3\\ &=\text {\tiny $\left ( \begin {array}{cccc} \cos \left (\theta _1\right ) \cos \left (\theta _2\right ) \cos \left (\theta _3\right )-\sin \left (\theta _1\right ) \sin \left (\theta _3\right ) & \cos \left (\theta _1\right ) \sin \left (\theta _2\right ) & -\cos \left (\theta _3\right ) \sin \left (\theta _1\right )-\cos \left (\theta _1\right ) \cos \left (\theta _2\right ) \sin \left (\theta _3\right ) & L \left (\cos \left (\theta _1\right ) \left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right )-\sin \left (\theta _1\right ) \sin \left (\theta _3\right )\right ) \\ \cos \left (\theta _2\right ) \cos \left (\theta _3\right ) \sin \left (\theta _1\right )+\cos \left (\theta _1\right ) \sin \left (\theta _3\right ) & \sin \left (\theta _1\right ) \sin \left (\theta _2\right ) & \cos \left (\theta _1\right ) \cos \left (\theta _3\right )-\cos \left (\theta _2\right ) \sin \left (\theta _1\right ) \sin \left (\theta _3\right ) & L \left (\left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right ) \sin \left (\theta _1\right )+\cos \left (\theta _1\right ) \sin \left (\theta _3\right )\right ) \\ \cos \left (\theta _3\right ) \sin \left (\theta _2\right ) & -\cos \left (\theta _2\right ) & -\sin \left (\theta _2\right ) \sin \left (\theta _3\right ) & L \left (\cos \left (\theta _3\right )+1\right ) \sin \left (\theta _2\right ) \\ 0 & 0 & 0 & 1 \\ \end {array} \right )$} \end {align*}
In short notation the above can be written as \begin {align*} T^0_2 &=\left ( \begin {array}{cccc} C_1 C_2 & -S_1 & -C_1 S_2 & L C_1 \left (C_2+1\right ) \\ C_2 S_1 & C_1 & -S_1 S_2 & L \left (C_2+1\right ) S_1 \\ S_2 & 0 & C_2 & L S_2 \\ 0 & 0 & 0 & 1 \\ \end {array} \right )\\ T^0_3 &=\left ( \begin {array}{cccc} C_1 C_2 C_3-S_1 S_3 & C_1 S_2 & -C_3 S_1-C_1 C_2 S_3 & L \left (C_1 \left (C_2 \left (C_3+1\right )+1\right )-S_1 S_3\right ) \\ C_2 C_3 S_1+C_1 S_3 & S_1 S_2 & C_1 C_3-C_2 S_1 S_3 & L \left (\left (C_2 \left (C_3+1\right )+1\right ) S_1+C_1 S_3\right ) \\ C_3 S_2 & -C_2 & -S_2 S_3 & L \left (C_3+1\right ) S_2 \\ 0 & 0 & 0 & 1 \\ \end {array} \right ) \end {align*}
Now we find the base Jacobian \(J_0\) where \(x'= J_0 q'\) \[ \boxed { J_0 = \left (\begin {array}{ccc} J_{v_1} & J_{v_2} & J_{v_3} \\ J_{\omega _1} & J_{\omega _2}& J_{\omega _3} \end {array} \right ) } \] where \begin {align*} J^0_{v_i} &= \epsilon _i z^0_{i-1} + \hat {\epsilon }\left ( z^0_{i-1} \times (o^0_n - o^0_{i-1})\right )\\ J^0_{\omega _i} &= \hat \epsilon z^0_{i-1} \end {align*}
\(z^0_0\) is given by \(\left ( \begin {array}{c} 0\\0\\1 \end {array} \right )\) and \begin {align*} J_{v_1} &= z^0_0 \times (o^0_3 - o^0_0) \\ &= \left ( \begin {array}{c} 0 \\ 0 \\ 1 \\ \end {array} \right )\times \left ( \begin {array}{c} L \left (\cos \left (\theta _1\right ) \left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right )-\sin \left (\theta _1\right ) \sin \left (\theta _3\right )\right ) \\ L \left (\sin \left (\theta _1\right ) \left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right )+\sin \left (\theta _3\right ) \cos \left (\theta _1\right )\right ) \\ L \sin \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right ) \\ \end {array} \right )\\ &= \left ( \begin {array}{c} -L \left (\sin \left (\theta _1\right ) \left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right )+\sin \left (\theta _3\right ) \cos \left (\theta _1\right )\right ) \\ L \left (\cos \left (\theta _1\right ) \left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right )-\sin \left (\theta _1\right ) \sin \left (\theta _3\right )\right ) \\ 0 \\ \end {array} \right )\\ J_{v_2} &= z^0_1 \times (o^0_3 - o^0_1) \\ &= \left ( \begin {array}{c} \sin \left (\theta _1\right ) \\ -\cos \left (\theta _1\right ) \\ 0 \\ \end {array} \right )\times \left (\left ( \begin {array}{c} L \left (\cos \left (\theta _1\right ) \left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right )-\sin \left (\theta _1\right ) \sin \left (\theta _3\right )\right ) \\ L \left (\sin \left (\theta _1\right ) \left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right )+\sin \left (\theta _3\right ) \cos \left (\theta _1\right )\right ) \\ L \sin \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right ) \\ \end {array} \right )-\left ( \begin {array}{c} L \cos \left (\theta _1\right ) \\ L \sin \left (\theta _1\right ) \\ 0 \\ \end {array} \right )\right )\\ &= \left ( \begin {array}{c} -L \sin \left (\theta _2\right ) \cos \left (\theta _1\right ) \left (\cos \left (\theta _3\right )+1\right ) \\ -L \sin \left (\theta _1\right ) \sin \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right ) \\ L \cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right ) \\ \end {array} \right )\\ J_{v_3} &= z^0_2 \times (o^0_3 - o^0_2) \\ &= \left ( \begin {array}{c} \sin \left (\theta _2\right ) \left (-\cos \left (\theta _1\right )\right ) \\ -\sin \left (\theta _1\right ) \sin \left (\theta _2\right ) \\ \cos \left (\theta _2\right ) \\ \end {array} \right )\times \text {\scriptsize $ \left (\left ( \begin {array}{c} L \left (\cos \left (\theta _1\right ) \left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right )-\sin \left (\theta _1\right ) \sin \left (\theta _3\right )\right ) \\ L \left (\sin \left (\theta _1\right ) \left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right )+\sin \left (\theta _3\right ) \cos \left (\theta _1\right )\right ) \\ L \sin \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right ) \\ \end {array} \right )-\left ( \begin {array}{c} L \cos \left (\theta _1\right ) \left (\cos \left (\theta _2\right )+1\right ) \\ L \sin \left (\theta _1\right ) \left (\cos \left (\theta _2\right )+1\right ) \\ L \sin \left (\theta _2\right ) \\ \end {array} \right )\right )$}\\ &= \left ( \begin {array}{c} -L \left (\sin \left (\theta _1\right ) \cos \left (\theta _3\right )+\sin \left (\theta _3\right ) \cos \left (\theta _1\right ) \cos \left (\theta _2\right )\right ) \\ L \left (\cos \left (\theta _1\right ) \cos \left (\theta _3\right )-\sin \left (\theta _1\right ) \sin \left (\theta _3\right ) \cos \left (\theta _2\right )\right ) \\ -L \sin \left (\theta _2\right ) \sin \left (\theta _3\right ) \\ \end {array} \right )\\ J_{w_1} &= \left ( \begin {array}{c} 0 \\ 0 \\ 1 \\ \end {array} \right )\\ J_{w_2} &= \left ( \begin {array}{c} \sin \left (\theta _1\right ) \\ -\cos \left (\theta _1\right ) \\ 0 \\ \end {array} \right )\\ J_{w_3} &= \left ( \begin {array}{c} \sin \left (\theta _2\right ) \left (-\cos \left (\theta _1\right )\right ) \\ -\sin \left (\theta _1\right ) \sin \left (\theta _2\right ) \\ \cos \left (\theta _2\right ) \\ \end {array} \right ) \end {align*}
Hence the Jacobian becomes \[ J_0 = \text {\scriptsize $\left ( \begin {array}{ccc} -L \left (\left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right ) \sin \left (\theta _1\right )+\cos \left (\theta _1\right ) \sin \left (\theta _3\right )\right ) & -L \cos \left (\theta _1\right ) \left (\cos \left (\theta _3\right )+1\right ) \sin \left (\theta _2\right ) & -L \left (\cos \left (\theta _3\right ) \sin \left (\theta _1\right )+\cos \left (\theta _1\right ) \cos \left (\theta _2\right ) \sin \left (\theta _3\right )\right ) \\ L \left (\cos \left (\theta _1\right ) \left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right )-\sin \left (\theta _1\right ) \sin \left (\theta _3\right )\right ) & -L \left (\cos \left (\theta _3\right )+1\right ) \sin \left (\theta _1\right ) \sin \left (\theta _2\right ) & L \left (\cos \left (\theta _1\right ) \cos \left (\theta _3\right )-\cos \left (\theta _2\right ) \sin \left (\theta _1\right ) \sin \left (\theta _3\right )\right ) \\ 0 & L \cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right ) & -L \sin \left (\theta _2\right ) \sin \left (\theta _3\right ) \\ 0 & \sin \left (\theta _1\right ) & -\cos \left (\theta _1\right ) \sin \left (\theta _2\right ) \\ 0 & -\cos \left (\theta _1\right ) & -\sin \left (\theta _1\right ) \sin \left (\theta _2\right ) \\ 1 & 0 & \cos \left (\theta _2\right ) \\ \end {array} \right )$} \] Or in short notation \[ \boxed { J_0 = \left ( \begin {array}{ccc} -L \left (\left (C_2 \left (C_3+1\right )+1\right ) S_1+C_1 S_3\right ) & -L C_1 \left (C_3+1\right ) S_2 & -L \left (C_3 S_1+C_1 C_2 S_3\right ) \\ L \left (C_1 \left (C_2 \left (C_3+1\right )+1\right )-S_1 S_3\right ) & -L \left (C_3+1\right ) S_1 S_2 & L \left (C_1 C_3-C_2 S_1 S_3\right ) \\ 0 & L C_2 \left (C_3+1\right ) & -L S_2 S_3 \\ 0 & S_1 & -C_1 S_2 \\ 0 & -C_1 & -S_1 S_2 \\ 1 & 0 & C_2 \\ \end {array} \right ) } \] When \(L=1\) the above becomes \[ \boxed { J_0 = \left ( \begin {array}{ccc} -\left (C_2 \left (C_3+1\right )+1\right ) S_1-C_1 S_3 & -C_1 \left (C_3+1\right ) S_2 & -C_3 S_1-C_1 C_2 S_3 \\ C_1 \left (C_2 \left (C_3+1\right )+1\right )-S_1 S_3 & -\left (C_3+1\right ) S_1 S_2 & C_1 C_3-C_2 S_1 S_3 \\ 0 & C_2 \left (C_3+1\right ) & -S_2 S_3 \\ 0 & S_1 & -C_1 S_2 \\ 0 & -C_1 & -S_1 S_2 \\ 1 & 0 & C_2 \\ \end {array} \right ) } \]
Using the duality property where \begin {align*} x' &= J_0 q' \\ \tau &= J^T f \end {align*}
We can determine \(\tau \) for each Jacobian at each configuration. The following table gives the result of this computation
| # | \(J_0\) | \(J^T f\) | \(\tau \) |
| \(\left (\begin {array}{c} 0\\30\\0\end {array}\right )\) | \(\text {\scriptsize $\left ( \begin {array}{ccc} 0 & -L & 0 \\ \left (1+\sqrt {3}\right ) L & 0 & L \\ 0 & \sqrt {3} L & 0 \\ 0 & 0 & -\frac {1}{2} \\ 0 & -1 & 0 \\ 1 & 0 & \frac {\sqrt {3}}{2} \\ \end {array} \right )$}\) | \(\text {\scriptsize $\left ( \begin {array}{cccccc} 0 & \left (1+\sqrt {3}\right ) L & 0 & 0 & 0 & 1 \\ -L & 0 & \sqrt {3} L & 0 & -1 & 0 \\ 0 & L & 0 & -\frac {1}{2} & 0 & \frac {\sqrt {3}}{2} \\ \end {array} \right )\left ( \begin {array}{c} f_x \\ 0 \\ 0 \\ 0 \\ 0 \\ \tau _z \\ \end {array} \right )$}\) | \(\text {\scriptsize $\left ( \begin {array}{c} \tau _z \\ -L f_x \\ \frac {\sqrt {3} \tau _z}{2} \\ \end {array} \right )$}\) |
| \(\left (\begin {array}{c} 0\\0\\30\end {array}\right )\) | \(\text {\scriptsize $\left ( \begin {array}{ccc} -\frac {L}{2} & 0 & -\frac {L}{2} \\ \left (2+\frac {\sqrt {3}}{2}\right ) L & 0 & \frac {\sqrt {3} L}{2} \\ 0 & \left (1+\frac {\sqrt {3}}{2}\right ) L & 0 \\ 0 & 0 & 0 \\ 0 & -1 & 0 \\ 1 & 0 & 1 \\ \end {array} \right )$}\) | \(\text {\scriptsize $\left ( \begin {array}{cccccc} -\frac {L}{2} & \left (2+\frac {\sqrt {3}}{2}\right ) L & 0 & 0 & 0 & 1 \\ 0 & 0 & \left (1+\frac {\sqrt {3}}{2}\right ) L & 0 & -1 & 0 \\ -\frac {L}{2} & \frac {\sqrt {3} L}{2} & 0 & 0 & 0 & 1 \\ \end {array} \right )\left ( \begin {array}{c} f_x \\ 0 \\ 0 \\ 0 \\ 0 \\ \tau _z \\ \end {array} \right )$}\) | \(\text {\scriptsize $\left ( \begin {array}{c} \tau _z-\frac {L f_x}{2} \\ 0 \\ \tau _z-\frac {L f_x}{2} \\ \end {array} \right )$}\) |
| \(\left (\begin {array}{c} 0\\90\\90\end {array}\right )\) | \(\text {\scriptsize $\left ( \begin {array}{ccc} -L & -L & 0 \\ L & 0 & 0 \\ 0 & 0 & -L \\ 0 & 0 & -1 \\ 0 & -1 & 0 \\ 1 & 0 & 0 \\ \end {array} \right )$}\) | \(\text {\scriptsize $\left ( \begin {array}{cccccc} -L & L & 0 & 0 & 0 & 1 \\ -L & 0 & 0 & 0 & -1 & 0 \\ 0 & 0 & -L & -1 & 0 & 0 \\ \end {array} \right )\left ( \begin {array}{c} f_x \\ 0 \\ 0 \\ 0 \\ 0 \\ \tau _z \\ \end {array} \right )$}\) | \(\text {\scriptsize $\left ( \begin {array}{c} \tau _z-L f_x \\ -L f_x \\ 0 \\ \end {array} \right )$}\) |
Since \[ f = \left ( \begin {array}{c} f_x \\ 0 \\ 0 \\ 0 \\ 0 \\ \tau _z \\ \end {array} \right ) \] then only entries in the \(J^T\) matrix which are not zero at location \((i,1)\) and \((i,6)\) where \(i\) is the row number of \(J^T\) will contribute to the joint torques. So for configuration 1 above, we see that \(J^T\) has non zero value in one of these locations in each row. But for configuration 2, the second row of \(J^T\) has zero in both the first column and the last column. This means no matter what joint torque is applied to joint 2, it will have no effect on end effector forces produced.
Similarly, for the third configuration, we see that the last row of \(J^T\) also has zero entry in the first and last column. This means no matter what joint torque is applied to joint 3, it will have no effect on end effector forces produced when in this configuration.
To find \(J^1_0\) we need to transform \(J_0\) to frame 1 using \(J^i_0 = \left ( \begin {array}{cc} R^i_0&0\\0&R^i_0 \end {array}\right ) J_0\) where in this case \(i=1\). But \(R^1_0=\left ( R^0_1 \right )^{-1} = \left ( R^0_1 \right )^T\). We found \(T^0_1\) from above, so we can extract \(R^0_1\) part from it \begin {align*} T^0_1 &=\left ( \begin {array}{cccc} \cos \left (\theta _1\right ) & 0 & \sin \left (\theta _1\right ) & L \cos \left (\theta _1\right ) \\ \sin \left (\theta _1\right ) & 0 & -\cos \left (\theta _1\right ) & L \sin \left (\theta _1\right ) \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ \end {array} \right )\\ R^0_1 &=\left ( \begin {array}{ccc} \cos \left (\theta _1\right ) & 0 & \sin \left (\theta _1\right ) \\ \sin \left (\theta _1\right ) & 0 & -\cos \left (\theta _1\right ) \\ 0 & 1 & 0 \\ \end {array} \right ) \end {align*}
Hence \begin {align*} \left ( \begin {array}{cc} R^i_0&0\\0&R^i_0 \end {array}\right ) & = \left ( \begin {array}{cccccc} \cos \left (\theta _1\right ) & \sin \left (\theta _1\right ) & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ \sin \left (\theta _1\right ) & -\cos \left (\theta _1\right ) & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & \cos \left (\theta _1\right ) & \sin \left (\theta _1\right ) & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & \sin \left (\theta _1\right ) & -\cos \left (\theta _1\right ) & 0 \\ \end {array} \right ) \end {align*}
mutliplying the above with \(J_0\) found earlier gives \begin {align*} J^1_0 &= \left ( \begin {array}{cc} R^i_0&0\\0&R^i_0 \end {array}\right ) J_0 \\ &=\left ( \begin {array}{ccc} -L \sin \left (\theta _3\right ) & -L \left (\cos \left (\theta _3\right )+1\right ) \sin \left (\theta _2\right ) & -L \cos \left (\theta _2\right ) \sin \left (\theta _3\right ) \\ 0 & L \cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right ) & -L \sin \left (\theta _2\right ) \sin \left (\theta _3\right ) \\ -L \left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right ) & 0 & -L \cos \left (\theta _3\right ) \\ 0 & 0 & -\sin \left (\theta _2\right ) \\ 1 & 0 & \cos \left (\theta _2\right ) \\ 0 & 1 & 0 \\ \end {array} \right ) \end {align*}
Hence \(J^1_{0_v}\) is the top three rows given by \[ \boxed { J^1_{0_v} =\left ( \begin {array}{ccc} -L \sin \left (\theta _3\right ) & -L \left (\cos \left (\theta _3\right )+1\right ) \sin \left (\theta _2\right ) & -L \cos \left (\theta _2\right ) \sin \left (\theta _3\right ) \\ 0 & L \cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right ) & -L \sin \left (\theta _2\right ) \sin \left (\theta _3\right ) \\ -L \left (\cos \left (\theta _2\right ) \left (\cos \left (\theta _3\right )+1\right )+1\right ) & 0 & -L \cos \left (\theta _3\right ) \\ \end {array} \right ) } \] When \(L=1\) the above becomes \[ \boxed { J^1_{0_v} =\left ( \begin {array}{ccc} -\sin \left (\theta _3\right ) & -\cos \left (\theta _3\right ) \sin \left (\theta _2\right )-\sin \left (\theta _2\right ) & -\cos \left (\theta _2\right ) \sin \left (\theta _3\right ) \\ 0 & \cos \left (\theta _3\right ) \cos \left (\theta _2\right )+\cos \left (\theta _2\right ) & -\sin \left (\theta _2\right ) \sin \left (\theta _3\right ) \\ -\cos \left (\theta _3\right ) \cos \left (\theta _2\right )-\cos \left (\theta _2\right )-1 & 0 & -\cos \left (\theta _3\right ) \\ \end {array} \right ) } \]
The singularity of \(J^1_v\) can be found mathematically by finding the conditions under which the determinant of \(J^1_v\) is zero, or the conditions under which one of the eigevalues become zero. Or we can use geometry and consider the cases where the arm is in singular direction. Mathematically, the determinant of \(J^1_v\) is \[ |J^1_v| = -L^3 \sin \left (\theta _3\right ) \left (\cos \left (\theta _2\right )+1\right ) \left (\cos \left (\theta _3\right )+1\right ) \] When \(L=1\) the above becomes \[ |J^1_v| = -(1+\cos \theta _2)(1+\cos \theta _3)\sin \theta _3 \] To make \(|J^1_v|\), the above implies the corresponding joint angles have to be the following \begin {align*} \theta _2 &= \pm \pi \\ \theta _3 &= \{\pm \pi , 0 \} \end {align*}
The joint angle \(\theta _1\) can be any value since it does not contribute to making the determinant zero since the determinant does not depend on \(\theta _1\). The above shows there are a total of 5 configurations that will result in singularity.
The following diagram illustrates the above singularites found and also sketches the the singular direction.
The analytical linear velocity Jacobian \(J_a\) is given by \[ J_a = E_p(x_p) J_0 \]
Where \(E_p(x_p)\) is the representation matrix found from \[ \left ( \begin {array}{c} u'\\v'\\w' \end {array} \right ) = E_p(x_p) \left ( \begin {array}{c} x'\\y'\\z' \end {array} \right ) \] Since we are given the expressions for \(u,v,w\), then we differentiate them w.r.t time to determine \(E_p(x_p)\) and obtain
\begin {align*} \left (\begin {array}{c} u' \\ v' \\ w' \end {array} \right ) & = \left (\begin {array}{c} 2 x' + 4 y' \\ x' + y' - z' \\ z' \end {array} \right )\\ & = \left (\begin {array}{ccc} 2 & 3 & 0 \\ 1 &1& -1 \\ 0 & 0 & 1 \end {array} \right ) \left (\begin {array}{c} x' \\ y' \\ z' \end {array} \right ) \end {align*}
Hence \[ E_p(x_p) = \left ( \begin {array}{ccc} 2 & 3 & 0 \\ 1 & 1 & -1 \\ 0 & 0 & 1 \end {array}\right ) \]
Therefore \(J_a=E_p(x_p) J_0\) becomes, at the required angles \[ \boxed { J_a = \left ( \begin {array}{ccc} L & -2 L & 0 \\ 0 & -L & L \\ 0 & 0 & -L \\ \end {array} \right ) } \] And at \(L=1\) \[ J_a = \left ( \begin {array}{ccc} 1 & -2 & 0 \\ 0 & -1 & 1 \\ 0 & 0 & -1 \\ \end {array} \right ) \]
Using geometry as illustrated below
Hence based on the above diagram \(q_1\) is found to be \[ q_1 = z_e \] Taking the projection of the end effector vector on the \(xy\) plane gives \begin {align*} x_e &= q_3 \cos (q_2) \\ y_e &= q_3 \sin (q_2) \end {align*}
Dividing the second equation above by the first one gives \(\frac {y_e}{x_e} = \tan (q_2)\). Hence \(q_2 = \atantwo (x_e,y_e)\). If \(x=0\) then there is no solution (singular direction). A second solution is \(q_2 = \atantwo (x_e,y_e) + \pi \) and finally \(q_3=\sqrt {x^2_e + y^2_e}\). Therefore the two solutions are \[ \boxed { \begin {pmatrix} q_1 \\ q_2 \\ q_3 \end {pmatrix}_{\{1\}} = \begin {pmatrix} z_e \\ \atantwo (x_e,y_e) \\ \sqrt {x^2_e + y^2_e} \end {pmatrix} \qquad \begin {pmatrix} q_1 \\ q_2 \\ q_3 \end {pmatrix}_{\{2\}} = \begin {pmatrix} z_e \\ \atantwo (x_e,y_e)+\pi \\ \sqrt {x^2_e + y^2_e} \end {pmatrix} } \]
Source code for problem 1
axes[x_, y_, z_, f_, a_] := Graphics3D[Join[{Arrowheads[a]}, Arrow[{{0, 0, 0}, #}] & /@ {{x, 0, 0}, {0, y, 0}, {0, 0, z}}, {Text[Style["x", FontSize -> Scaled[f]], {1.2*x, 0.1*y, 0.1*z}], Text[Style["y", FontSize -> Scaled[f]], {0.1 x, 1.2*y, 0.1*z}], Text[Style["z", FontSize -> Scaled[f]], {0.1*x, 0.1*y, 1.2*z}]}]]; rotZ[a_] := {{Cos[a], -Sin[a], 0}, {Sin[a], Cos[a], 0}, {0, 0, 1}}; rotX[a_] := {{1, 0, 0}, {0, Cos[a], -Sin[a]}, {0, Sin[a], Cos[a]}}; rotY[a_] := {{Cos[a], 0, Sin[a]}, {0, 1, 0}, {-Sin[a], 0, Cos[a]}}; obj = Cuboid[{1, -.5, -.1}, {-1, .5, .1}]; show[mat_, arrow_, fixed_] := Show[Graphics3D[ { GeometricTransformation[{ {Opacity[0.7], obj}, {Red, Arrow[{{0, 0, 0}, {1.2, 0, 0}}]}, {Red, Arrow[{{0, 0, 0}, {0, 1.2, 0}}]}, {Red, Arrow[{{0, 0, 0}, {0, 0, 1.2}}]}, Text[Style["x", Red, FontSize -> Scaled[0.07]], {1.4, 0, 0}], Text[Style["y", Red, FontSize -> Scaled[0.07]], {0, 1.4, 0}], Text[Style["z", Red, FontSize -> Scaled[0.07]], {0, 0, 1.4}], If[fixed, Sequence @@ {}, arrow] }, mat ], If[fixed, arrow, Sequence @@ {}] }, Boxed -> False, Axes -> None, ViewPoint -> {3, 1.5, 1.5}, SphericalRegion -> True, ImageSize -> 250, ImageMargins -> 0, ImagePadding -> 0, PlotRangePadding -> None], axes[1.6, 1.6, 1.5, 0.1, 0.04]]; show[mat_] := Show[Graphics3D[ { GeometricTransformation[{ {Opacity[0.7], obj}, {Red, Arrow[{{0, 0, 0}, {1.2, 0, 0}}]}, {Red, Arrow[{{0, 0, 0}, {0, 1.2, 0}}]}, {Red, Arrow[{{0, 0, 0}, {0, 0, 1.2}}]}, Text[Style["x", Red, FontSize -> Scaled[0.07]], {1.4, 0, 0}], Text[Style["y", Red, FontSize -> Scaled[0.07]], {0, 1.4, 0}], Text[Style["z", Red, FontSize -> Scaled[0.07]], {0, 0, 1.4}] }, mat ] }, Boxed -> False, Axes -> None, ViewPoint -> {3, 1.5, 1.5}, SphericalRegion -> True, ImageSize -> 200, ImageMargins -> 0, ImagePadding -> 0, PlotRangePadding -> None], axes[1.6, 1.6, 1.5, 0.1, 0.04]]; p1 = Grid[{ {Style["+45 around fixed z", Bold, 14], Style["after rotation ", Bold, 14], Style["Final rotation matrix", Bold, 14]}, { show[rotZ[0], makeCurvedArrow[.5, "z", 1], True], show[rotZ[45 Degree]], MatrixForm[N@rotZ[45 Degree]] } }, Spacings -> {1, 1}, Frame -> All, FrameStyle -> LightGray ] p2 = Grid[{ {Style["30 around current x", Bold, 14], Style["after rotation ", Bold, 14], Style["Final rotation matrix", Bold, 14]}, { show[rotZ[45 Degree], makeCurvedArrow[.5, "x", 1], False], show[rotZ[45 Degree].rotX[30 Degree]], MatrixForm[N@rotZ[45 Degree].rotX[30 Degree]] }}, Spacings -> {1, 1}, Frame -> All, FrameStyle -> LightGray ] p3 = Grid[{ {Style["-45 around fixed z", Bold, 14], Style["after rotation ", Bold, 14], Style["Final rotation matrix", Bold, 14]}, { show[rotZ[45 Degree].rotX[30 Degree], makeCurvedArrow[.5, "z", -1], True], show[rotZ[-45 Degree].rotZ[45 Degree].rotX[30 Degree]], MatrixForm[N@rotZ[-45 Degree].rotZ[45 Degree].rotX[30 Degree]] }}, Spacings -> {1, 1}, Frame -> All, FrameStyle -> LightGray ] p4 = Grid[{ {Style["+90 around current y", Bold, 14], Style["after rotation ", Bold, 14], Style["Final rotation matrix", Bold, 14]}, { show[rotZ[-45 Degree].rotZ[45 Degree].rotX[30 Degree], makeCurvedArrow[.5, "y", 1], False], show[rotZ[-45 Degree].rotZ[45 Degree].rotX[30 Degree].rotY[90 Degree]], MatrixForm[N@rotZ[-45 Degree].rotZ[45 Degree].rotX[30 Degree].rotY[90 Degree]] }}, Spacings -> {1, 1}, Frame -> All, FrameStyle -> LightGray ] p5 = Grid[{ {Style["-30 around fixed x", Bold, 14], Style["after rotation ", Bold, 14], Style["Final rotation matrix", Bold, 14]}, { show[rotZ[-45 Degree].rotZ[45 Degree].rotX[30 Degree].rotY[90 Degree], makeCurvedArrow[.5, "x", -1], True], show[rotX[-30 Degree].rotZ[-45 Degree].rotZ[45 Degree].rotX[30 Degree].rotY[90 Degree]], MatrixForm[Chop[N@rotX[-30 Degree].rotZ[-45 Degree].rotZ[45 Degree].rotX[30 Degree].rotY[90 Degree]]] } }, Spacings -> {1, 1}, Frame -> All, FrameStyle -> LightGray]
