The document describes the kinematics of a 6 degree of freedom robotic arm. It presents the Denavit-Hartenberg parameters for defining each link of the robot. It then performs forward kinematics calculations to determine the transformation matrix relating the end effector to the base. It also performs inverse kinematics calculations to solve for the joint angles given a desired end effector pose. The forward and inverse kinematics solutions involve trigonometric functions of the joint angles and geometric parameters of the robot links.

1. Brazo robot de 6 grados de libertad
SOLUCIÓN DE LA CINEMÁTICA DIRECTA
Parámetros Denavit – Hartemberg:
i bi θi ai αi
1 0 θ1 0 -90°
2 0 θ2 a2 0
3 0 θ3 0 -90°
4 b4 θ4 0 90°
5 0 θ5 0 -90°
6 b6 θ6 0 0
Luego calculamos las matrices de transformación para cada eslabón, según la ecuación:
=
θ6
θ4
θ3
θ2
X5
X3
X1, X2
Z7
X7
X6
X4
Z6
Z4
Z1
Z5
Z3
Z2
θ1
θ5

2. = =
=
= =
= =

3. =
= =
=
Luego obtenemos la matriz de transformación final, según la ecuación:
Para lo cual multiplicaremos matrices de izquierda a derecha, asociándolas de 2 en 2:

4. Luego:
Haciendo uso de las propiedades trigonométricas, simplificamos los elementos de las matrices:
=
=
=
=
=
=
=
=
=
=
=
=
Luego realizamos el último producto de matrices

6. Agrupando términos que se repiten:

7. Donde:

8. SOLUCIÓN DE LA CINEMÁTICA INVERSA
Sea la matriz de transformación final, hallada anteriormente en cinemática directa:
[ - cos(theta6)*(sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) -
cos(theta4)*cos(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3))) -
sin(theta4)*sin(theta6)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)),
sin(theta6)*(sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) -
cos(theta4)*cos(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3))) -
cos(theta6)*sin(theta4)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)), -
cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) -
cos(theta4)*sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)), a2*cos(theta2) - b4*sin(theta2 +
theta3) - (b6*cos(theta2 + theta3)*sin(theta4 + theta5))/2 - b6*sin(theta2 + theta3)*cos(theta5) +
(b6*sin(theta4 - theta5)*cos(theta2 + theta3))/2]
[ cos(theta6)*(sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) +
cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))) -
sin(theta4)*sin(theta6)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)), -
sin(theta6)*(sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) +
cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))) -
cos(theta6)*sin(theta4)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),
cos(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) -
cos(theta4)*sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)), b4*cos(theta2 + theta3) +
a2*sin(theta2) - (b6*sin(theta2 + theta3)*sin(theta4 + theta5))/2 + b6*cos(theta2 + theta3)*cos(theta5) +
(b6*sin(theta4 - theta5)*sin(theta2 + theta3))/2]

9. [ -
cos(theta4)*sin(theta6) - cos(theta5)*cos(theta6)*sin(theta4),
cos(theta5)*sin(theta4)*sin(theta6) - cos(theta4)*cos(theta6),
sin(theta4)*sin(theta5),
b6*sin(theta4)*sin(theta5)]
[
0,
0,
0,
1]
[ nx*cos(theta1)*cos(theta2) - nz*sin(theta2) + ny*cos(theta2)*sin(theta1), ox*cos(theta1)*cos(theta2) -
oz*sin(theta2) + oy*cos(theta2)*sin(theta1), ax*cos(theta1)*cos(theta2) - az*sin(theta2) +
ay*cos(theta2)*sin(theta1), px*cos(theta1)*cos(theta2) - pz*sin(theta2) - a2 + py*cos(theta2)*sin(theta1)]
[ - nz*cos(theta2) - nx*cos(theta1)*sin(theta2) - ny*sin(theta1)*sin(theta2), - oz*cos(theta2) -
ox*cos(theta1)*sin(theta2) - oy*sin(theta1)*sin(theta2), - az*cos(theta2) - ax*cos(theta1)*sin(theta2) -
ay*sin(theta1)*sin(theta2), - pz*cos(theta2) - px*cos(theta1)*sin(theta2) - py*sin(theta1)*sin(theta2)]
[ ny*cos(theta1) - nx*sin(theta1), oy*cos(theta1) -
ox*sin(theta1), ay*cos(theta1) - ax*sin(theta1),
py*cos(theta1) - px*sin(theta1)]
[ 0, 0,
0, 1]
[ - cos(theta6)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) -
cos(theta3)*sin(theta4)*sin(theta6), sin(theta6)*(sin(theta3)*sin(theta5) -
cos(theta3)*cos(theta4)*cos(theta5)) - cos(theta3)*cos(theta6)*sin(theta4), - cos(theta5)*sin(theta3) -
cos(theta3)*cos(theta4)*sin(theta5), - b6*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) -
b4*sin(theta3)]
[ cos(theta6)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3)) -
sin(theta3)*sin(theta4)*sin(theta6), - sin(theta6)*(cos(theta3)*sin(theta5) +
cos(theta4)*cos(theta5)*sin(theta3)) - cos(theta6)*sin(theta3)*sin(theta4), cos(theta3)*cos(theta5) -

10. cos(theta4)*sin(theta3)*sin(theta5), b6*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5)) +
b4*cos(theta3)]
[ - cos(theta4)*sin(theta6) - cos(theta5)*cos(theta6)*sin(theta4),
cos(theta5)*sin(theta4)*sin(theta6) - cos(theta4)*cos(theta6), sin(theta4)*sin(theta5),
b6*sin(theta4)*sin(theta5)]
[ 0,
0, 0, 1]
[ nx*cos(theta2 + theta3)*cos(theta1) - nz*sin(theta2 + theta3) + ny*cos(theta2 + theta3)*sin(theta1),
ox*cos(theta2 + theta3)*cos(theta1) - oz*sin(theta2 + theta3) + oy*cos(theta2 + theta3)*sin(theta1),
ax*cos(theta2 + theta3)*cos(theta1) - az*sin(theta2 + theta3) + ay*cos(theta2 + theta3)*sin(theta1),
px*cos(theta2 + theta3)*cos(theta1) - a2*cos(theta3) - pz*sin(theta2 + theta3) + py*cos(theta2 +
theta3)*sin(theta1)]
[ nx*sin(theta1) - ny*cos(theta1),
ox*sin(theta1) - oy*cos(theta1), ax*sin(theta1) - ay*cos(theta1),
px*sin(theta1) - py*cos(theta1)]
[ - nz*cos(theta2 + theta3) - nx*sin(theta2 + theta3)*cos(theta1) - ny*sin(theta2 + theta3)*sin(theta1), -
oz*cos(theta2 + theta3) - ox*sin(theta2 + theta3)*cos(theta1) - oy*sin(theta2 + theta3)*sin(theta1), -
az*cos(theta2 + theta3) - ax*sin(theta2 + theta3)*cos(theta1) - ay*sin(theta2 + theta3)*sin(theta1),
a2*sin(theta3) - pz*cos(theta2 + theta3) - px*sin(theta2 + theta3)*cos(theta1) - py*sin(theta2 +
theta3)*sin(theta1)]
[ 0,
0, 0,
1]
[ cos(theta4)*cos(theta5)*cos(theta6) - sin(theta4)*sin(theta6), - cos(theta6)*sin(theta4) -
cos(theta4)*cos(theta5)*sin(theta6), -cos(theta4)*sin(theta5), -b6*cos(theta4)*sin(theta5)]
[ cos(theta4)*sin(theta6) + cos(theta5)*cos(theta6)*sin(theta4), cos(theta4)*cos(theta6) -
cos(theta5)*sin(theta4)*sin(theta6), -sin(theta4)*sin(theta5), -b6*sin(theta4)*sin(theta5)]

11. [ cos(theta6)*sin(theta5), -sin(theta5)*sin(theta6), cos(theta5), b4
+ b6*cos(theta5)]
[ 0, 0, 0, 1]
[ nx*(sin(theta1)*sin(theta4) - cos(theta1)*(cos(theta4)*sin(theta2)*sin(theta3) -
cos(theta2)*cos(theta3)*cos(theta4))) - nz*(cos(theta2)*cos(theta4)*sin(theta3) +
cos(theta3)*cos(theta4)*sin(theta2)) - ny*(cos(theta1)*sin(theta4) +
sin(theta1)*(cos(theta4)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*cos(theta4))),
ox*(sin(theta1)*sin(theta4) - cos(theta1)*(cos(theta4)*sin(theta2)*sin(theta3) -
cos(theta2)*cos(theta3)*cos(theta4))) - oz*(cos(theta2)*cos(theta4)*sin(theta3) +
cos(theta3)*cos(theta4)*sin(theta2)) - oy*(cos(theta1)*sin(theta4) +
sin(theta1)*(cos(theta4)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*cos(theta4))),
ax*(sin(theta1)*sin(theta4) - cos(theta1)*(cos(theta4)*sin(theta2)*sin(theta3) -
cos(theta2)*cos(theta3)*cos(theta4))) - az*(cos(theta2)*cos(theta4)*sin(theta3) +
cos(theta3)*cos(theta4)*sin(theta2)) - ay*(cos(theta1)*sin(theta4) +
sin(theta1)*(cos(theta4)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*cos(theta4))),
px*(sin(theta1)*sin(theta4) - cos(theta1)*(cos(theta4)*sin(theta2)*sin(theta3) -
cos(theta2)*cos(theta3)*cos(theta4))) - pz*(cos(theta2)*cos(theta4)*sin(theta3) +
cos(theta3)*cos(theta4)*sin(theta2)) - py*(cos(theta1)*sin(theta4) +
sin(theta1)*(cos(theta4)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*cos(theta4))) -
a2*cos(theta3)*cos(theta4)]
[
- nz*cos(theta2 + theta3) - nx*sin(theta2 + theta3)*cos(theta1) - ny*sin(theta2 + theta3)*sin(theta1),
- oz*cos(theta2 + theta3) - ox*sin(theta2 + theta3)*cos(theta1) - oy*sin(theta2 + theta3)*sin(theta1),
- az*cos(theta2 + theta3) - ax*sin(theta2 + theta3)*cos(theta1) - ay*sin(theta2 + theta3)*sin(theta1),
a2*sin(theta3) - pz*cos(theta2 + theta3) - b4 - px*sin(theta2 + theta3)*cos(theta1) - py*sin(theta2 +
theta3)*sin(theta1)]
[ ny*(cos(theta1)*cos(theta4) - sin(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) -
cos(theta2)*cos(theta3)*sin(theta4))) - nx*(cos(theta4)*sin(theta1) +
cos(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) - cos(theta2)*cos(theta3)*sin(theta4))) -
nz*(cos(theta2)*sin(theta3)*sin(theta4) + cos(theta3)*sin(theta2)*sin(theta4)), oy*(cos(theta1)*cos(theta4) -
sin(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) - cos(theta2)*cos(theta3)*sin(theta4))) -
ox*(cos(theta4)*sin(theta1) + cos(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) -
cos(theta2)*cos(theta3)*sin(theta4))) - oz*(cos(theta2)*sin(theta3)*sin(theta4) +

12. cos(theta3)*sin(theta2)*sin(theta4)), ay*(cos(theta1)*cos(theta4) -
sin(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) - cos(theta2)*cos(theta3)*sin(theta4))) -
ax*(cos(theta4)*sin(theta1) + cos(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) -
cos(theta2)*cos(theta3)*sin(theta4))) - az*(cos(theta2)*sin(theta3)*sin(theta4) +
cos(theta3)*sin(theta2)*sin(theta4)), py*(cos(theta1)*cos(theta4) -
sin(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) - cos(theta2)*cos(theta3)*sin(theta4))) -
px*(cos(theta4)*sin(theta1) + cos(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) -
cos(theta2)*cos(theta3)*sin(theta4))) - pz*(cos(theta2)*sin(theta3)*sin(theta4) +
cos(theta3)*sin(theta2)*sin(theta4)) - a2*cos(theta3)*sin(theta4)]
[
0,
0,
0,
1]
[ cos(theta5)*cos(theta6), -cos(theta5)*sin(theta6), -sin(theta5), -b6*sin(theta5)]
[ cos(theta6)*sin(theta5), -sin(theta5)*sin(theta6), cos(theta5), b6*cos(theta5)]
[ -sin(theta6), -cos(theta6), 0, 0]
[ 0, 0, 0, 1]
[ - nx*(cos(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) +
sin(theta2)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3))) -
cos(theta5)*sin(theta1)*sin(theta4)) - ny*(sin(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) -
cos(theta3)*cos(theta4)*cos(theta5)) + sin(theta2)*(cos(theta3)*sin(theta5) +
cos(theta4)*cos(theta5)*sin(theta3))) + cos(theta1)*cos(theta5)*sin(theta4)) -
nz*(cos(theta2)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3)) -
sin(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5))), -
ox*(cos(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) +
sin(theta2)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3))) -
cos(theta5)*sin(theta1)*sin(theta4)) - oy*(sin(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) -
cos(theta3)*cos(theta4)*cos(theta5)) + sin(theta2)*(cos(theta3)*sin(theta5) +
cos(theta4)*cos(theta5)*sin(theta3))) + cos(theta1)*cos(theta5)*sin(theta4)) -
oz*(cos(theta2)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3)) -

13. sin(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5))), -
ax*(cos(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) +
sin(theta2)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3))) -
cos(theta5)*sin(theta1)*sin(theta4)) - ay*(sin(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) -
cos(theta3)*cos(theta4)*cos(theta5)) + sin(theta2)*(cos(theta3)*sin(theta5) +
cos(theta4)*cos(theta5)*sin(theta3))) + cos(theta1)*cos(theta5)*sin(theta4)) -
az*(cos(theta2)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3)) -
sin(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5))), a2*(sin(theta3)*sin(theta5) -
cos(theta3)*cos(theta4)*cos(theta5)) - px*(cos(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) -
cos(theta3)*cos(theta4)*cos(theta5)) + sin(theta2)*(cos(theta3)*sin(theta5) +
cos(theta4)*cos(theta5)*sin(theta3))) - cos(theta5)*sin(theta1)*sin(theta4)) -
py*(sin(theta1)*(cos(theta2)*(sin(theta3)*sin(theta5) - cos(theta3)*cos(theta4)*cos(theta5)) +
sin(theta2)*(cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3))) +
cos(theta1)*cos(theta5)*sin(theta4)) - pz*(cos(theta2)*(cos(theta3)*sin(theta5) +
cos(theta4)*cos(theta5)*sin(theta3)) - sin(theta2)*(sin(theta3)*sin(theta5) -
cos(theta3)*cos(theta4)*cos(theta5))) - b4*sin(theta5)]
[
nz*(cos(theta2)*sin(theta3)*sin(theta4) + cos(theta3)*sin(theta2)*sin(theta4)) + nx*(cos(theta4)*sin(theta1) +
cos(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) - cos(theta2)*cos(theta3)*sin(theta4))) -
ny*(cos(theta1)*cos(theta4) - sin(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) -
cos(theta2)*cos(theta3)*sin(theta4))),
oz*(cos(theta2)*sin(theta3)*sin(theta4) + cos(theta3)*sin(theta2)*sin(theta4)) + ox*(cos(theta4)*sin(theta1) +
cos(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) - cos(theta2)*cos(theta3)*sin(theta4))) -
oy*(cos(theta1)*cos(theta4) - sin(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) -
cos(theta2)*cos(theta3)*sin(theta4))),
az*(cos(theta2)*sin(theta3)*sin(theta4) + cos(theta3)*sin(theta2)*sin(theta4)) + ax*(cos(theta4)*sin(theta1) +
cos(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) - cos(theta2)*cos(theta3)*sin(theta4))) -
ay*(cos(theta1)*cos(theta4) - sin(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) -
cos(theta2)*cos(theta3)*sin(theta4))),
pz*(cos(theta2)*sin(theta3)*sin(theta4) + cos(theta3)*sin(theta2)*sin(theta4)) + px*(cos(theta4)*sin(theta1) +
cos(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) - cos(theta2)*cos(theta3)*sin(theta4))) -
py*(cos(theta1)*cos(theta4) - sin(theta1)*(sin(theta2)*sin(theta3)*sin(theta4) -
cos(theta2)*cos(theta3)*sin(theta4))) + a2*cos(theta3)*sin(theta4)]
[ - nx*(cos(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) +
sin(theta2)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5))) +
sin(theta1)*sin(theta4)*sin(theta5)) - ny*(sin(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) +
cos(theta3)*cos(theta4)*sin(theta5)) + sin(theta2)*(cos(theta3)*cos(theta5) -

14. cos(theta4)*sin(theta3)*sin(theta5))) - cos(theta1)*sin(theta4)*sin(theta5)) -
nz*(cos(theta2)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5)) -
sin(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5))), -
ox*(cos(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) +
sin(theta2)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5))) +
sin(theta1)*sin(theta4)*sin(theta5)) - oy*(sin(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) +
cos(theta3)*cos(theta4)*sin(theta5)) + sin(theta2)*(cos(theta3)*cos(theta5) -
cos(theta4)*sin(theta3)*sin(theta5))) - cos(theta1)*sin(theta4)*sin(theta5)) -
oz*(cos(theta2)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5)) -
sin(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5))), -
ax*(cos(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) +
sin(theta2)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5))) +
sin(theta1)*sin(theta4)*sin(theta5)) - ay*(sin(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) +
cos(theta3)*cos(theta4)*sin(theta5)) + sin(theta2)*(cos(theta3)*cos(theta5) -
cos(theta4)*sin(theta3)*sin(theta5))) - cos(theta1)*sin(theta4)*sin(theta5)) -
az*(cos(theta2)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5)) -
sin(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5))), a2*(cos(theta5)*sin(theta3) +
cos(theta3)*cos(theta4)*sin(theta5)) - px*(cos(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) +
cos(theta3)*cos(theta4)*sin(theta5)) + sin(theta2)*(cos(theta3)*cos(theta5) -
cos(theta4)*sin(theta3)*sin(theta5))) + sin(theta1)*sin(theta4)*sin(theta5)) -
py*(sin(theta1)*(cos(theta2)*(cos(theta5)*sin(theta3) + cos(theta3)*cos(theta4)*sin(theta5)) +
sin(theta2)*(cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5))) -
cos(theta1)*sin(theta4)*sin(theta5)) - pz*(cos(theta2)*(cos(theta3)*cos(theta5) -
cos(theta4)*sin(theta3)*sin(theta5)) - sin(theta2)*(cos(theta5)*sin(theta3) +
cos(theta3)*cos(theta4)*sin(theta5))) - b4*cos(theta5)]
[
0,
0,
0,
1]
[ cos(theta6), -sin(theta6), 0, 0]
[ sin(theta6), cos(theta6), 0, 0]
[ 0, 0, 1, b6]
[ 0, 0, 0, 1]