Problema UR5 Cinemàtica Directa
1. Matrius de Transformació Homogènia
1.1. \(\mathbf{T}_1\)
\[
\mathbf{T}_1 =
\begin{pmatrix}
0.707 & 0 & 0.707 & 0 \\
0.707 & 0 & -0.707 & 0 \\
0 & 1 & 0 & 0.089 \\
0 & 0 & 0 & 1
\end{pmatrix}
\]
1.2. \(\mathbf{T}_2\)
\[
\mathbf{T}_2 =
\begin{pmatrix}
0.866 & 0.5 & 0 & 0.368 \\
-0.5 & 0.866 & 0 & -0.212 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{pmatrix}
\]
1.3. \(\mathbf{T}_3\)
\[
\mathbf{T}_3 =
\begin{pmatrix}
0.5 & -0.866 & 0 & 0.196 \\
0.866 & 0.5 & 0 & 0.339 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{pmatrix}
\]
1.4. \(\mathbf{T}_4\)
\[
\mathbf{T}_4 =
\begin{pmatrix}
0.707 & 0 & -0.707 & 0 \\
-0.707 & 0 & -0.707 & 0 \\
0 & 1 & 0 & 0.109 \\
0 & 0 & 0 & 1
\end{pmatrix}
\]
1.5. \(\mathbf{T}_5\)
\[
\mathbf{T}_5 =
\begin{pmatrix}
0.866 & 0 & 0.5 & 0 \\
0.5 & 0 & -0.866 & 0 \\
0 & 1 & 0 & 0.094 \\
0 & 0 & 0 & 1
\end{pmatrix}
\]
1.6. \(\mathbf{T}_6\)
\[
\mathbf{T}_6 =
\begin{pmatrix}
0.5 & -0.866 & 0 & 0 \\
0.866 & 0.5 & 0 & 0 \\
0 & 0 & 1 & 0.082 \\
0 & 0 & 0 & 1
\end{pmatrix}
\]
2. Multiplicació de les Matrius de Transformació
2.1. Multiplicació de \(\mathbf{T}_1\) i \(\mathbf{T}_2\)
\[
\mathbf{T}_{12} = \mathbf{T}_1 \cdot \mathbf{T}_2 =
\begin{pmatrix}
0.612 & 0.612 & 0.707 & 0.260 \\
-0.612 & 0.612 & -0.707 & -0.504 \\
0 & 0 & 1 & 0.089 \\
0 & 0 & 0 & 1
\end{pmatrix}
\]
2.2. Multiplicació de \(\mathbf{T}_{12}\) i \(\mathbf{T}_3\)
\[
\mathbf{T}_{123} = \mathbf{T}_{12} \cdot \mathbf{T}_3 =
\begin{pmatrix}
0.104 & -0.958 & 0.707 & 0.427 \\
-0.814 & -0.286 & -0.707 & -0.452 \\
0 & 0 & 1 & 0.183 \\
0 & 0 & 0 & 1
\end{pmatrix}
\]
2.3. Multiplicació de \(\mathbf{T}_{123}\) i \(\mathbf{T}_4\)
\[
\mathbf{T}_{1234} = \mathbf{T}_{123} \cdot \mathbf{T}_4 =
\begin{pmatrix}
-0.5 & 0.5 & -0.707 & 0.1 \\
0.5 & 0.5 & 0.707 & 0.0 \\
0 & 1 & 0 & 0.094 \\
0 & 0 & 0 & 1
\end{pmatrix}
\]
2.4. Multiplicació de \(\mathbf{T}_{1234}\) i \(\mathbf{T}_5\)
\[
\mathbf{T}_{12345} = \mathbf{T}_{1234} \cdot \mathbf{T}_5 =
\begin{pmatrix}
-0.866 & 0.5 & -0.707 & -0.259 \\
0.5 & 0.866 & 0.707 & 0.066 \\
0 & 0 & 1 & 0.094 \\
0 & 0 & 0 & 1
\end{pmatrix}
\]
2.5. Multiplicació de \(\mathbf{T}_{12345}\) i \(\mathbf{T}_6\)
\[
\mathbf{T}_{total} = \mathbf{T}_{12345} \cdot \mathbf{T}_6 =
\begin{pmatrix}
-0.258 & -0.965 & 0 & 0.814 \\
0.966 & -0.259 & 0 & 0.201 \\
0 & 0 & 1 & 0.275 \\
0 & 0 & 0 & 1
\end{pmatrix}
\]
3. Resultat Final
3.1. Posició Final
La posició final \((x_f, y_f, z_f)\) és la quarta columna de \(\mathbf{T}_{total}\):
- x_f \(\approx 0.814\) m
- y_f \(\approx 0.201\) m
- z_f \(\approx 0.275\) m
3.2. Orientació Final
L'orientació està representada per la submatriu 3x3 superior esquerra de \(\mathbf{T}_{total}\).