Starting with the relation \[ R_{3}^{1}=R_{2}^{1}R_{3}^{2}\] Pre-multiplying both sides by \((R_{2}^{1})^{-1}\) which exists since \(R\) is a rotation matrix and hence invertible results in \[ R_{3}^{2}=(R_{2}^{1})^{-1}R_{3}^{1}\] For a rotation matrix the following relation holds \[ (R_{2}^{1})^{-1}=(R_{2}^{1})^{T}\] Therefore \begin {align*} R_{3}^{2} & =(R_{2}^{1})^{T}R_{3}^{1}\\ & =\begin {pmatrix} 1 & 0 & 0\\ 0 & \frac {1}{2} & \frac {-\sqrt {3}}{2}\\ 0 & \frac {\sqrt {3}}{2} & \frac {1}{2}\end {pmatrix} ^{T}\begin {pmatrix} 0 & 0 & -1\\ 0 & 1 & 0\\ 1 & 0 & 0 \end {pmatrix} \\ & =\begin {pmatrix} 1 & 0 & 0\\ 0 & \frac {1}{2} & \frac {\sqrt {3}}{2}\\ 0 & \frac {-\sqrt {3}}{2} & \frac {1}{2}\end {pmatrix}\begin {pmatrix} 0 & 0 & -1\\ 0 & 1 & 0\\ 1 & 0 & 0 \end {pmatrix} \\ & =\begin {pmatrix} 0 & 0 & -1\\ \frac {\sqrt {3}}{2} & \frac {1}{2} & 0\\ \frac {1}{2} & \frac {-\sqrt {3}}{2} & 0 \end {pmatrix} \end {align*}
The goal is to determine \(T_{1}^{0},T_{2}^{0},T_{3}^{0}\) and \(T_{3}^{2}\). \(T_{i}^{i-1}\) is the homogeneous transformation from frame \(\{i-1\}\) to frame \(\{i\}\) given by \[ T_{i}^{i-1}=\begin {pmatrix} R_{i}^{i-1} & d\\ 0 & 1 \end {pmatrix} \] Where \(d\) is the position vector from the origin of frame \(\{i-1\}\) to the origin of frame \(\{i\}\) expressed in frame \(\{i-1\}\), and \(R_{i}^{i-1}\) is the rotation matrix.
By direct inspection of the above diagram the following transformations are obtained \[ T_{1}^{0}=\begin {pmatrix} 0 & 1 & 0 & 0\\ 0 & 0 & -1 & 0\\ -1 & 0 & 0 & 1\\ 0 & 0 & 0 & 1 \end {pmatrix} ,T_{2}^{0}=\begin {pmatrix} 0 & 0 & -1 & 0\\ -1 & 0 & 0 & 1\\ 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} ,T_{2}^{1}=\begin {pmatrix} 0 & -1 & 0 & 1\\ 0 & 0 & -1 & 0\\ 1 & 0 & 0 & -1\\ 0 & 0 & 0 & 1 \end {pmatrix} \] Given the above transformations \(T_{1}^{0}T_{2}^{1}\) is found and checked to be the same as \(T_{2}^{0}\)
\begin {align*} T_{1}^{0}T_{2}^{1} & =\begin {pmatrix} 0 & 1 & 0 & 0\\ 0 & 0 & -1 & 0\\ -1 & 0 & 0 & 1\\ 0 & 0 & 0 & 1 \end {pmatrix}\begin {pmatrix} 0 & -1 & 0 & 1\\ 0 & 0 & -1 & 0\\ 1 & 0 & 0 & -1\\ 0 & 0 & 0 & 1 \end {pmatrix} \\ & =\begin {pmatrix} 0 & 0 & -1 & 0\\ -1 & 0 & 0 & 1\\ 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \end {align*}
Which is the same as \(T_{2}^{0}\) as expected.
The distance from the table surface to the center of the small cube is \(0.1\) meter. The goal is to determine \(T_{1}^{0},T_{2}^{0},T_{3}^{0}\) and \(T_{3}^{2}\). By direct inspection of the given figure the following transformations are obtained \[ T_{1}^{0}=\begin {pmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 1\\ 0 & 0 & 1 & 1\\ 0 & 0 & 0 & 1 \end {pmatrix} ,T_{2}^{0}=\begin {pmatrix} 1 & 0 & 0 & -0.5\\ 0 & 1 & 0 & 1.5\\ 0 & 0 & 1 & 1.1\\ 0 & 0 & 0 & 1 \end {pmatrix} ,T_{3}^{0}=\begin {pmatrix} 0 & 1 & 0 & -0.5\\ 1 & 0 & 0 & 1.5\\ 0 & 0 & -1 & 3\\ 0 & 0 & 0 & 1 \end {pmatrix} \]
In each of the above, the first column of \(T_{i}^{i-1}\) is the projection of \(\hat {x}\) in frame \(i\) into frame \(i-1\) and the second is the projection of \(\hat {y}\) in frame \(i\) into frame \(i-1\) and the third column the projection of \(\hat {z}\) in frame \(i\) into frame \(i-1\). The fourth column of \(T\) is the position vector of the center of frame \(i\) expressed in frame \(i-1\). By inspection \(T_{3}^{2}\) is found to be
\[ T_{3}^{2}=\begin {pmatrix} 0 & 1 & 0 & 0\\ 1 & 0 & 0 & 0\\ 0 & 0 & -1 & 1.9\\ 0 & 0 & 0 & 1 \end {pmatrix} \]
PART 1:
Let \(d\) be the position vector of the origin of frame \(\left \{ B\right \}\) relative to frame \(\left \{ A\right \}\). Let \(r^{A}\) be the position vector of a point relative to frame \(\left \{A\right \}\), and \(r^{B}\) be the position vector of the point relative to frame \(\left \{ B\right \}\) as shown in the following diagram
From the above diagram \(r^A\) is found as \begin {equation} r^{A}=R_{B}^{A}r^{B}+d \tag {1} \end {equation}
From the problem statement, \(R_{B}^{A}= \begin {pmatrix} \frac {-1}{2} & 0 & \frac {-\sqrt {3}}{2}\\ 0 & 1 & 0\\ \frac {\sqrt {3}}{2} & 0 & \frac {-1}{2}\end {pmatrix} \) and \(d=\begin {pmatrix} -2\\ 4\\ 2 \end {pmatrix} \)
Taking time derivative of (1) and using the chain rule results in
\begin {align} v^{A}=\frac {dR_{B}^{A}}{dt}r^{B}+R_{B}^{A}v^{B}+\frac {d}{dt}d \tag {2} \end {align}
\(R_{B}^{A}\) does not depend on time, therefore \(\frac {dR_{B}^{A}}{dt}=0\). Since frame \(\left \{ B\right \}\) does not move relative to frame \(\left \{ A\right \} \), therefore \(\frac {d}{dt}d=0\). Using these results (2) simplifies to
\[ v^{A}=R_{B}^{A}v^{B}\]
Solving for \(v^{B}\) from the above, and noting that \(\left ( R_{B}^{A}\right ) ^{-1}=\left ( R_{B}^{A}\right ) ^{T}\) since it is a rotation matrix gives
\[ v^{B}=\left ( R_{B}^{A}\right ) ^{T}v^{A}\]
Substituting the values given in the problem in the above results in
\begin {align*} v^{B} & =\begin {pmatrix} \frac {-1}{2} & 0 & \frac {-\sqrt {3}}{2}\\ 0 & 1 & 0\\ \frac {\sqrt {3}}{2} & 0 & \frac {-1}{2}\end {pmatrix} ^{T}\begin {pmatrix} -2\\ 4\\ 2 \end {pmatrix} \\ & =\begin {pmatrix} -\frac {1}{2} & 0 & \frac {1}{2}\sqrt {3}\\ 0 & 1 & 0\\ -\frac {1}{2}\sqrt {3} & 0 & -\frac {1}{2}\end {pmatrix}\begin {pmatrix} -2\\ 4\\ 2 \end {pmatrix} \end {align*}
Therefore \[ v^{B}=\begin {pmatrix} \sqrt {3}+1\\ 4\\ \sqrt {3}-1 \end {pmatrix} \]
PART 2:
The norm of the velocity vectors are \begin {align*} \left \Vert v^{A}\right \Vert & =\left \Vert \begin {pmatrix} -2\\ 4\\ 2 \end {pmatrix} \right \Vert =2\sqrt {6}\\ \left \Vert v^{B}\right \Vert & =\begin {pmatrix} \sqrt {3}+1\\ 4\\ \sqrt {3}-1 \end {pmatrix} =2\sqrt {6} \end {align*}
They have the same magnitude. The reason is that frame \(\left \{ B\right \}\) itself does not move nor rotate relative to \(\{A\}\). Therefore frame \(B\) is fixed relative from frame \(\{A\}\). Hence the velocity of a point relative to frame \(\{A\}\) will have the same magnitude relative to \(\left \{ B\right \}\). The velocity vector has different representation depending on the frame of reference, but has the same magnitude.
PART (A)
The first step is to assign the \(z_{i}\) axes for each link as follows
The four Denavit-Hartenberg parameters are defined as follows1
\(a_{i}\) (link length). The distance between axis \(z_{i-1}\) and \(z_{i}\) measured along \(x_{i}\).
\(\alpha _{i}\) (link twist angle). The angle between \(z_{i-1}\) and \(z_{i}\) measured in a plane normal to \(x_{i}\) using the right hand rule, around \(x_{i}\) (not \(x_{i-1}\)) to determine the positive sense of this angle.
\(d_{i}\) (link offset). The distance from origin \(o_{i-1}\) to the intersection of the \(x_{i}\) axis with \(z_{i-1}\) measured along \(z_{i-1}\) axis.
\(\theta _{i}\) (Joint angle). The angle from \(x_{i-1}\) to \(x_{i}\) measured in plane normal to \(z_{i-1}\).
When assigning the frames using the above rules, we need to insure2 that \(x_{i+1}\perp z_{i}\) and \(x_{i+1}\) intersects \(z_{i}\). Using the above rules the DH table is written down. There is one row in the table for each link. Hence there will be three rows. Link 0 is the base link and attached to the ground and does not show in the table.
| \(a\) (link length) | \(\alpha \) (link twist angle) | \(d\) (link offset) | \(\theta \) (Joint angle) | |
| link 1 | \(0\) | \(90^{0}\) | \(L_{1}\) | \(\theta _{1}\) |
| link 2 | \(L_{2}\) | \(0\) | \(0\) | \(\theta _{2}\) |
| link 3 | \(L_{3}\) | \(0\) | \(0\) | \(\theta _{3}\) |
The following diagram shows the parameters for the first link
The following diagram shows the parameters for the second link
The following diagram shows the parameters for the third link
Now the forward transformations using equation (3.10) on page 77 of the textbook is found \[ A=\begin {pmatrix} C_{\theta } & -S_{\theta }C_{\alpha } & S_{\theta }S_{\alpha } & aC_{\theta }\\ S_{\theta } & C_{\theta }C_{\alpha } & -C_{\theta }S_{\alpha } & aS_{\theta }\\ 0 & S_{\alpha } & C_{\alpha } & d\\ 0 & 0 & 0 & 1 \end {pmatrix} \]
Hence
\begin {align*} A_{1} & =\begin {pmatrix} \cos \theta _{1} & -\sin \theta _{1}\cos \frac {\pi }{2} & \sin \theta _{1}\sin \frac {\pi }{2} & 0\\ \sin \theta _{1} & \cos \theta _{1}\cos \frac {\pi }{2} & -\cos \theta _{1}\sin \frac {\pi }{2} & 0\\ 0 & \sin \frac {\pi }{2} & \cos \frac {\pi }{2} & L_{1}\\ 0 & 0 & 0 & 1 \end {pmatrix} =\begin {pmatrix} \cos \theta _{1} & 0 & \sin \theta _{1} & 0\\ \sin \theta _{1} & 0 & -\cos \theta _{1} & 0\\ 0 & 1 & 0 & L_{1}\\ 0 & 0 & 0 & 1 \end {pmatrix} \\ A_{2} & =\begin {pmatrix} \cos \theta _{2} & -\sin \theta _{2}\cos 0 & \sin \theta _{2}\sin 0 & L_{2}\cos \theta _{2}\\ \sin \theta _{2} & \cos \theta _{2}\cos 0 & -\cos \theta _{2}\sin 0 & L_{2}\sin \theta _{2}\\ 0 & \sin 0 & \cos 0 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} =\begin {pmatrix} \cos \theta _{2} & -\sin \theta _{2} & 0 & L_{2}\cos \theta _{2}\\ \sin \theta _{2} & \cos \theta _{2} & 0 & L_{2}\sin \theta _{2}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \\ A_{3} & =\begin {pmatrix} \cos \theta _{3} & -\sin \theta _{3}\cos 0 & \sin \theta _{3}\sin 0 & L_{3}\cos \theta _{3}\\ \sin \theta _{3} & \cos \theta _{3}\cos 0 & -\cos \theta _{3}\sin 0 & L_3 \sin \theta _3 \\ 0 & \sin 0 & \cos 0 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} =\begin {pmatrix} \cos \theta _{3} & -\sin \theta _{3} & 0 & L_{3}\cos \theta _{3}\\ \sin \theta _{3} & \cos \theta _{3} & 0 & L_3\sin \theta _3\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \end {align*}
Using the above, \(T_{3}^{0}\) is found
\begin {align*} T_{3}^{0} & =A_{1}A_{2}A_{3}\\ & =\begin {pmatrix} \cos \theta _{1} & 0 & \sin \theta _{1} & 0\\ \sin \theta _{1} & 0 & -\cos \theta _{1} & 0\\ 0 & 1 & 0 & L_{1}\\ 0 & 0 & 0 & 1 \end {pmatrix}\begin {pmatrix} \cos \theta _{2} & -\sin \theta _{2} & 0 & L_{2}\cos \theta _{2}\\ \sin \theta _{2} & \cos \theta _{2} & 0 & L_{2}\sin \theta _{2}\\ 0 & 0 & 1 & L_{1}\\ 0 & 0 & 0 & 1 \end {pmatrix}\begin {pmatrix} \cos \theta _{3} & -\sin \theta _{3} & 0 & L_{3}\cos \theta _{3}\\ \sin \theta _{3} & \cos \theta _{3} & 0 & L_{3}\sin \theta _{3}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \end {align*}
For verification of the above, let \(\theta _{1}=0,\theta _{2}=0,\theta _{3}=0\) then the fourth column of \(T_{3}^{0}\) gives the position vector of the end effector relative to the base when the manipulator is in the position in the problem. Substituting these values for the angles gives
\[ T_{3}^{0}=\begin {pmatrix} 1 & 0 & 0 & L_{2}+L_{3}\\ 0 & 0 & -1 & 0\\ 0 & 1 & 0 & L_{1}\\ 0 & 0 & 0 & 1 \end {pmatrix} \]
The above says that the end effector is at position vector \(p^{0}=\left ( L_{2}+L_{3},0,L_{1}\right )\) which means \(x_{3}=L_{2}+L_{3},y_{3}=0,z_{3}=L_{1}\). From the diagram this result is correct.
PART (B) For the second manipulator, the same steps were repeated. The first step is to assign the axes for each link as follows
The DH table is written down
| \(a\) (link length) | \(\alpha \) (link twist angle) | \(d\) (link offset) | \(\theta \) (Joint angle) | |
| link 1 | \(0\) | \(90^{0}\) | \(0\) | \(\theta _{1}+90^{0}\) |
| link 2 | \(0\) | \(-90^{0}\) | \(L_{1}+L_{2}+d_{2}\) | \(0\) |
| link 3 | \(L_{3}\) | \(0\) | \(0\) | \(\theta _{3}-90^{0}\) |
The forward transformations using equation (3.10) on page 77 of the textbook gives
\[ A=\begin {pmatrix} C_{\theta } & -S_{\theta }C_{\alpha } & S_{\theta }S_{\alpha } & aC_{\theta }\\ S_{\theta } & C_{\theta }C_{\alpha } & -C_{\theta }S_{\alpha } & aS_{\theta }\\ 0 & S_{\alpha } & C_{\alpha } & d\\ 0 & 0 & 0 & 1 \end {pmatrix} \]
Using the above, and noting that \(\cos \left ( x+90^{0}\right ) =-\sin x\),\(\sin \left ( x+90^{0}\right ) =\cos x\),\(\cos \left ( x-90^{0}\right ) =\sin x\) and \(\sin \left ( x-90^{0}\right ) =-\cos \left ( x\right ) \) results in
\begin {align*} A_{1} & =\begin {pmatrix} \cos \left ( \theta _{1}+90\right ) & -\sin \left ( \theta _{1}+90\right ) \cos \frac {\pi }{2} & \sin \left ( \theta _{1}+90\right ) \sin \frac {\pi }{2} & 0\\ \sin \left ( \theta _{1}+90\right ) & \cos \left ( \theta _{1}+90\right ) \cos \frac {\pi }{2} & -\cos \left ( \theta _{1}+90\right ) \sin \frac {\pi }{2} & 0\\ 0 & \sin \frac {\pi }{2} & \cos \frac {\pi }{2} & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} =\begin {pmatrix} -\sin \theta _{1} & 0 & \cos \theta _{1} & 0\\ \cos \theta _{1} & 0 & \sin \theta _{1} & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \\ A_{2} & =\begin {pmatrix} \cos 0 & -\sin 0\cos \left ( -90^{0}\right ) & \sin 0\sin \left ( -90^{0}\right ) & 0\cos 0\\ \sin 0 & \cos 0\cos \left ( -90^{0}\right ) & -\cos 0\sin \left ( -90^{0}\right ) & 0\sin 0\\ 0 & \sin \left ( -90^{0}\right ) & \cos \left ( -90^{0}\right ) & L_{1}+L_{2}+d_{2}\\ 0 & 0 & 0 & 1 \end {pmatrix} =\begin {pmatrix} 1 & 0 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & -1 & 0 & L_{1}+L_{2}+d_{2}\\ 0 & 0 & 0 & 1 \end {pmatrix} \\ A_{2} & = \text {\scriptsize $\begin {pmatrix} \cos \left ( \theta _{3}-90^{0}\right ) & -\sin \left ( \theta _{3}-90^{0}\right ) \cos 0 & \sin \left ( \theta _{3}-90^{0}\right ) \sin 0 & L_{3}\cos \left ( \theta _{3}-90^{0}\right ) \\ \sin \left ( \theta _{3}-90^{0}\right ) & \cos \left ( \theta _{3}-90^{0}\right ) \cos 0 & -\cos \left ( \theta _{3}-90^{0}\right ) \sin 0 & L_{3}\sin \left ( \theta _{3}-90^{0}\right ) \\ 0 & \sin 0 & \cos 0 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix}$ } =\begin {pmatrix} \sin \theta _{3} & \cos \theta _{3} & 0 & L_{3}\sin \theta _{3}\\ -\cos \theta _{3} & \sin \theta _{3} & 0 & -L_{3}\cos \theta _{3}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \end {align*}
From the above
\begin {align*} T_{3}^{0} & =A_{1}A_{2}A_{3}\\ & =\begin {pmatrix} -\sin \theta _{1} & 0 & \cos \theta _{1} & 0\\ \cos \theta _{1} & 0 & \sin \theta _{1} & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix}\begin {pmatrix} 1 & 0 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & -1 & 0 & L_{1}+L_{2}+d_{2}\\ 0 & 0 & 0 & 1 \end {pmatrix}\begin {pmatrix} \sin \theta _{3} & \cos \theta _{3} & 0 & L_{3}\sin \theta _{3}\\ -\cos \theta _{3} & \sin \theta _{3} & 0 & -L_{3}\cos \theta _{3}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \\ & = \text {\scriptsize $ \begin {pmatrix} \cos \theta _{1}\cos \theta _{3}-\sin \theta _{1}\sin \theta _{3} & -\cos \theta _{1}\sin \theta _{3}-\cos \theta _{3}\sin \theta _{1} & 0 & \cos \theta _{1} \left ( L_{1}+L_{2}+d_{2}\right ) +L_{3}\cos \theta _{1}\cos \theta _{3}-L_{3}\sin \theta _{1}\sin \theta _{3}\\ \cos \theta _{1}\sin \theta _{3}+\cos \theta _{3}\sin \theta _{1} & \cos \theta _{1}\cos \theta _{3}-\sin \theta _{1}\sin \theta _{3} & 0 & \sin \theta _{1} \left ( L_{1}+L_{2}+d_{2}\right ) +L_{3}\cos \theta _{1}\sin \theta _{3}+L_{3}\cos \theta _{3}\sin \theta _{1}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix}$ } \end {align*}
To verify the above, let \(\theta _{1}=0,\theta _{3}=360^{0},d_{2}=0\) then the fourth column of \(T_{3}^{0}\) gives the position vector of the end effector relative to the base when the manipulator is in a straight horizontal position\[ T_{3}^{0}=\begin {pmatrix} 1 & 0 & 0 & L_{1}+L_{2}+L_{3}\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \] The above results show that \(x_{3}=L_{1}+L_{2}+L_{3}\) which is the expected result.
The homogeneous transformation \(T_{i}^{i-1}\) was derived by inspection giving the following results
\begin {align*} T_{1}^{0} & =\begin {pmatrix} 0 & \sin q_{1} & \cos q_{1} & 0\\ 1 & 0 & 0 & L_{1}\\ 0 & \cos q_{1} & -\sin q_{1} & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} ,T_{2}^{1}=\begin {pmatrix} \cos q_{2} & 0 & \sin q_{2} & 0\\ 0 & 1 & 0 & 0\\ -\sin q_{2} & 0 & \cos q_{2} & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} ,T_{3}^{2}=\begin {pmatrix} \cos q_{3} & \sin q_{3} & 0 & 0\\ 0 & 0 & -1 & 0\\ -\sin q_{3} & \cos q_{3} & 0 & L_{2}\\ 0 & 0 & 0 & 1 \end {pmatrix} \\ T_{4}^{3} & =\begin {pmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & L_{3}+q_{4}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} ,T_{5}^{4}=\begin {pmatrix} \cos q_{5} & \sin q_{5} & 0 & 0\\ -\sin q_{5} & \cos q_{5} & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} ,T_{6}^{5}=\begin {pmatrix} -\cos q_{6} & \sin q_{6} & 0 & 0\\ 0 & 0 & 1 & L_{5}\\ \sin q_{6} & \cos q_{6} & 0 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \end {align*}
The following is plot of the \(x,y,z\) coordinates of the end effector \(E\)
The following Matlab script problem_6_part_1.m calculates the homogeneous transformation \(T_{6}^{0}\) and plots the above figures
%This scripts plots the x,y,z coordinates of the end effector E %for problem 5, HW1 , ME 739. %to run, type this script name on the Matlab console % problem_6_part_1 %The matlab path must include the ME 739 rendering software %Nasser M. Abbasi 2/16/15 clear all; close all; %define syms to use to build the T matrices syms q1 q2 q3 q4 q5 q6 L1 L2 L3 L5 t L1 = 3; L2 = 5; L3 = 3; L5 = 3; h6 = 1; q1 = -pi*sin(t); q2 = pi/4*(1-cos(t)); q3 = pi/4*sin(t); q4 = 1/2*L3*(1-cos(t)); q5 = -pi/4*sin(t); q6 = pi/4*sin(t); %define the 6 transformation matrices T01 to T56 in syms T01 = [0 sin(q1) cos(q1) 0; 1 0 0 L1; 0 cos(q1) -sin(q1) 0; 0 0 0 1]; T12 = [cos(q2) 0 sin(q2) 0; 0 1 0 0; -sin(q2) 0 cos(q2) 0; 0 0 0 1]; T23 = [cos(q3) sin(q3) 0 0; 0 0 -1 0; -sin(q3) cos(q3) 0 L2; 0 0 0 1]; T34 = [1 0 0 0; 0 1 0 L3+q4; 0 0 1 0; 0 0 0 1]; T45 = [cos(q5) sin(q5) 0 0; -sin(q5) cos(q5) 0 0; 0 0 1 0; 0 0 0 1]; T56 = [-cos(q6) sin(q6) 0 0; 0 0 1 L5; sin(q6) cos(q6) 0 0; 0 0 0 1]; %Now obtain T06 to allow finding the end effector coordinates T06 = T01*T12*T23*T34*T45*T56; %handle to function to evaluate T06 at each instance of time endPos = @(t0) subs(T06,t,t0) %set up time scale, and evaluate the end effector coordinates %saving result in a matrix for plotting later. timeScale = 0:.1:2*pi; coords = zeros(length(timeScale),3); %generate the coordinates of the end effector for i = 1:length(timeScale) p = endPos(timeScale(i))*[-h6 0 0 1]'; coords(i,:) = p(1:3); end %now plot the result. First col is the x-coordinates, %second col is y-coord, and third col is z-coordinate. subplot(1,3,1); plot(timeScale,coords(:,1), 'LineWidth',1.5); xlabel('t (sec)'); ylabel('x'); grid; axis square; xlim([0 2*pi]);ylim([-14 14]); set(gca, 'GridLineStyle', '-'); subplot(1,3,2); plot(timeScale,coords(:,2), 'LineWidth',1.5); xlabel('t (sec)'); ylabel('y'); grid; axis square; xlim([0 2*pi]);ylim([0 18]); %axis square; axis tight set(gca, 'GridLineStyle', '-'); subplot(1,3,3); plot(timeScale,coords(:,3),'LineWidth',1.5); xlabel('t (sec)'); ylabel('z'); grid; axis square; xlim([0 2*pi]);ylim([-13 15]); set(gca, 'GridLineStyle', '-'); %export_fig(gcf,'problem_6_part_1.pdf');
The following matlab script calculates \(T_{1}^{0},T_{2}^{0},T_{3}^{0},T_{4}^{0},T_{5}^{0},T_{6}^{0}\), where \(T_{2}^{0}=T_{1}^{0}T_{2}^{1},T_{3}^{0}=T_{2}^{0}T_{3}^{2},T_{4}^{0}=T_{3}^{0}T_{4}^{3},T_{5}^{0}=T_{4}^{0}T_{5}^{4},T_{6}^{0}=T_{5}^{0}T_{6}^{5}\)
For example, for \(T_{2}^{0}\) the result is
\begin {align*} T_{1}^{0} & =\begin {pmatrix} 0 & \sin q_{1} & \cos q_{1} & 0\\ 1 & 0 & 0 & L_{1}\\ 0 & \cos q_{1} & -\sin q_{1} & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \\ T_{2}^{0} & =T_{1}^{0}T_{2}^{1}=\begin {pmatrix} 0 & \sin q_{1} & \cos q_{1} & 0\\ 1 & 0 & 0 & L_{1}\\ 0 & \cos q_{1} & -\sin q_{1} & 0\\ 0 & 0 & 0 & 1 \end {pmatrix}\begin {pmatrix} \cos q_{2} & 0 & \sin q_{2} & 0\\ 0 & 1 & 0 & 0\\ -\sin q_{2} & 0 & \cos q_{2} & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} =\begin {pmatrix} -\cos q_{1}\sin q_{2} & \sin q_{1} & \cos q_{1}\cos q_{2} & 0\\ \cos q_{2} & 0 & \sin q_{2} & L_{1}\\ \sin q_{1}\sin q_{2} & \cos q_{1} & -\cos q_{2}\sin q_{1} & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \end {align*}
The complete calculation was done in the following Matlab script problem_6_part_2.m. The script is run by typing its name on the Matlab console.
%This calculates T01,T02,T03,T04,T05,T06 numerically %for problem 5, HW1 , ME 739. %to run, just type this script name on the Matlab console % problem_6_part_2 % %The matlab path must include the ME 739 rendering software %Nasser M. Abbasi 2/16/15 clear all; close all; %define syms to use to build the T matrices syms q1 q2 q3 q4 q5 q6 L1 L2 L3 L5 t L1 = 3; L2 = 5; L3 = 3; L5 = 3; h6 = 1; q1 = -pi*sin(t); q2 = pi/4*(1-cos(t)); q3 = pi/4*sin(t); q4 = 1/2*L3*(1-cos(t)); q5 = -pi/4*sin(t); q6 = pi/4*sin(t); %define the 6 transformation matrices T01 to T56 in syms %define the 6 transformation matrices T01 to T56 in syms T01 = [0 sin(q1) cos(q1) 0; 1 0 0 L1; 0 cos(q1) -sin(q1) 0; 0 0 0 1]; T12 = [cos(q2) 0 sin(q2) 0; 0 1 0 0; -sin(q2) 0 cos(q2) 0; 0 0 0 1]; T23 = [cos(q3) sin(q3) 0 0; 0 0 -1 0; -sin(q3) cos(q3) 0 L2; 0 0 0 1]; T34 = [1 0 0 0; 0 1 0 L3+q4; 0 0 1 0; 0 0 0 1]; T45 = [cos(q5) sin(q5) 0 0; -sin(q5) cos(q5) 0 0; 0 0 1 0; 0 0 0 1]; T56 = [-cos(q6) sin(q6) 0 0; 0 0 1 L5; sin(q6) cos(q6) 0 0; 0 0 0 1]; %Now calculate T02,T03,T04,T05,T06 T02 = T01*T12; T03 = T02*T23; T04 = T03*T34; T05 = T04*T45; T06 = T05*T56; %handle to function to evaluate each Tij at each instance of time calcT = @(T,t0) double(subs(T,t,t0)); %now calculate all the T's at some specific time. The problem %does not says what time instance to use, so we use t=0 for %illustration timeToShow = 1; %change this to different time as needed fprintf('T01 at t=1 is \n'); calcT(T01,timeToShow) fprintf('T02 at t=1 is \n'); calcT(T02,timeToShow) fprintf('T03 at t=1 is \n'); calcT(T03,timeToShow) fprintf('T04 at t=1 is \n'); calcT(T04,timeToShow) fprintf('T05 at t=1 is \n'); calcT(T05,timeToShow) fprintf('T06 at t=1 is \n'); calcT(T06,timeToShow)
The above script calculates numerically all the transformation matrices using the joint variable inputs given in the problem. At the end it prints each matrix. The problem did not indicate for which value of \(t\) to use to calculate the matrices, hence for illustration these are displayed for \(t=0\) and \(t=1\) second. A variable inside the script can be used to change the time instance. The following is the output from running the above script for illustration
At \(t=0\) the output is
T01 at t=0 is 0 0 1 0 1 0 0 3 0 1 0 0 0 0 0 1 T02 at t=0 is 0 0 1 0 1 0 0 3 0 1 0 0 0 0 0 1 T03 at t=0 is 0 1 0 5 1 0 0 3 0 0 -1 0 0 0 0 1 T04 at t=0 is 0 1 0 8 1 0 0 3 0 0 -1 0 0 0 0 1 T05 at t=0 is 0 1 0 8 1 0 0 3 0 0 -1 0 0 0 0 1 T06 at t=0 is 0 0 1 11 -1 0 0 3 0 -1 0 0 0 0 0 1
At \(t=1\) the output is
T01 at t=1 is 0 -0.4777 -0.87852 0 1 0 0 3 0 -0.87852 0.4777 0 0 0 0 1 T02 at t=1 is 0.31034 -0.4777 -0.82188 0 0.93553 0 0.35325 3 -0.16875 -0.87852 0.4469 0 0 0 0 1 T03 at t=1 is 0.74949 -0.45834 0.4777 -4.1094 0.52172 0.85312 0 4.7663 -0.40753 0.24922 0.87852 2.2345 0 0 0 1 T04 at t=1 is 0.74949 -0.45834 0.4777 -5.8005 0.52172 0.85312 0 7.9139 -0.40753 0.24922 0.87852 3.154 0 0 0 1 T05 at t=1 is 0.31034 -0.82188 0.4777 -5.8005 0.93553 0.35325 0 7.9139 -0.16875 0.4469 0.87852 3.154 0 0 0 1 T06 at t=1 is 0.048223 0.56761 -0.82188 -8.2661 -0.73855 0.57425 0.35325 8.9736 0.67247 0.58997 0.4469 4.4947 0 0 0 1
This script problem_6_part_3.m assumes the Matlab path is set to include the UW rendering software.
%This calculates T01,T02,T03,T04,T05,T06 numerically %for problem 5, HW1 , ME 739. %to run, just type this script name on the Matlab console % % problem_6_part_3 %The matlab path must include the ME 739 rendering software % %Nasser M. Abbasi 2/17/15 clear all; close all; clc; %----set rendering window view parameters f_handle = 1; % figure handle axis_limits = [-10 10 0 13 -10 10]; %needed little bit more space render_view = [-1 1 -1]; % camera position view_up = [0 1 0]; % vertical orientation % initialize rendering view SetRenderingViewParameters(axis_limits,render_view,view_up,f_handle); ADD_BASE = false; %set to TRUE to see base rendered, does not move. DO_MOVIE = false; %set true to make frames for movie ANIMATION_TIME = 10; %10 seconds to animation DEL_T = 0.05; %time betweeb each animation loop. smaller time %make it run slower but more accurate %define syms to use to build the T matrices syms q1 q2 q3 q4 q5 q6 L1 L2 L3 L5 t L1 = 3; L2 = 5; L3 = 3; L5 = 3; h6 = 1; q1 = -pi*sin(t); q2 = pi/4*(1-cos(t)); q3 = pi/4*sin(t); q4 = 1/2*L3*(1-cos(t)); q5 = -pi/4*sin(t); q6 = pi/4*sin(t); %define the 6 transformation matrices T01 to T56 in syms %define the 6 transformation matrices T01 to T56 in syms T01 = [0 sin(q1) cos(q1) 0; 1 0 0 L1; 0 cos(q1) -sin(q1) 0; 0 0 0 1]; T12 = [cos(q2) 0 sin(q2) 0; 0 1 0 0; -sin(q2) 0 cos(q2) 0; 0 0 0 1]; T23 = [cos(q3) sin(q3) 0 0; 0 0 -1 0; -sin(q3) cos(q3) 0 L2; 0 0 0 1]; T34 = [1 0 0 0; 0 1 0 L3+q4; 0 0 1 0; 0 0 0 1]; T45 = [cos(q5) sin(q5) 0 0; -sin(q5) cos(q5) 0 0; 0 0 1 0; 0 0 0 1]; T56 = [-cos(q6) sin(q6) 0 0; 0 0 1 L5; sin(q6) cos(q6) 0 0; 0 0 0 1]; %Now calculate T02,T03,T04,T05,T06 T02 = T01*T12; T03 = T02*T23; T04 = T03*T34; T05 = T04*T45; T06 = T05*T56; %handle to function to evaluate each Tij at each instance of time %this is called during running the animation calcT = @(T,t0) double(subs(T,t,t0)); %base, does not move if ADD_BASE linkColor = [0 0 0]; plotFrame=0; normalized_location=-1; nSides = 4; radius = 4; r = L1; axis_aligned = 2; d0 = CreateLinkRendering(r ,radius, nSides, axis_aligned ,normalized_location, ... linkColor,plotFrame, f_handle); end %now create all the links linkColor = [1 .1 0]; plotFrame=0; normalized_location=-1; nSides=20; radius=2; r=L1; axis_aligned = 1; d1 = CreateLinkRendering(r ,radius, nSides, axis_aligned ,normalized_location, ... linkColor,plotFrame, f_handle); linkColor = [.5 .2 1]; plotFrame=0; normalized_location=-1; nSides=4; r=L2; radius=1.2; axis_aligned=3; d2 = CreateLinkRendering(r ,radius, nSides,axis_aligned ,normalized_location, ... linkColor,plotFrame, f_handle); linkColor = [1 .1 1]; plotFrame=0; normalized_location=-1; nSides=4; r=L3; radius=1.15; axis_aligned=2; d3 = CreateLinkRendering(r ,radius, nSides,axis_aligned ,normalized_location,... linkColor,plotFrame,f_handle); linkColor = [0 0 1]; plotFrame=0; normalized_location=1; nSides=4; r=L5; radius=.9; axis_aligned=2; d4 = CreateLinkRendering(r ,radius, nSides,axis_aligned,normalized_location,... linkColor,plotFrame, f_handle); linkColor = [.3 .5 .3]; plotFrame=0; normalized_location=-1; nSides=4; r=L5; radius=.7; axis_aligned=2; d5 = CreateLinkRendering(r ,radius, nSides,axis_aligned ,normalized_location,... linkColor,plotFrame, f_handle); linkColor = [.5 1 .5]; plotFrame=0; normalized_location=-1; nSides=4; r=L5; radius=0.5; axis_aligned=3; d6 = CreateLinkRendering(r ,radius, nSides,axis_aligned,normalized_location,... linkColor,plotFrame, f_handle); %now start the animation timeScale = 0: DEL_T :ANIMATION_TIME; k = 0; %for screen shots counting, to make movie for i = 1:length(timeScale) T = calcT(T01,timeScale(i)); UpdateLink(d1,T); T = calcT(T02,timeScale(i)); UpdateLink(d2,T); T = calcT(T03,timeScale(i)); UpdateLink(d3,T); T = calcT(T04,timeScale(i)); UpdateLink(d4,T); T = calcT(T05,timeScale(i)); UpdateLink(d5,T); T = calcT(T06,timeScale(i)); UpdateLink(d6,T); title(sprintf('time = %3.3f (sec)',timeScale(i))); if DO_MOVIE k = k+1; I = getframe(gcf); imwrite(I.cdata, sprintf('frame%d.png',k)); end %p=T*[-h6 0 0 1]'; to show the end effector path if needed %plot3(p(1),p(2),p(3),'o'); drawnow end
The following is a movie of the first few seconds of the Matlab run. (posted as animation gif in my site).
