MATLAB Command Window Page 1

T54d =

[ cos(q5d), 0, -sin(q5d), L6*cos(q5d)]

[ sin(q5d), 0, cos(q5d), L6*sin(q5d)]
[ 0, -1, 0, 0]
[ 0, 0, 0, 1]

T43d =

[ cos(B - q4d), sin(B - q4d), 0, L5*cos(B - q4d)]

[ -sin(B - q4d), cos(B - q4d), 0, -L5*sin(B - q4d)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]

T32d =

[ cos(2*B + q3d), -sin(2*B + q3d), 0, L4*cos(2*B + q3d)]

[ sin(2*B + q3d), cos(2*B + q3d), 0, L4*sin(2*B + q3d)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]

T21d =

[ cos(B - q2d), 0, -sin(B - q2d), L3*cos(B - q2d)]

[ -sin(B - q2d), 0, -cos(B - q2d), -L3*sin(B - q2d)]
[ 0, 1, 0, 0]
[ 0, 0, 0, 1]

T10d =

[ cos(q1d), -sin(q1d), 0, 0]
[ sin(q1d), cos(q1d), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]

T50d =

[ cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d) - sin(q1d)*sin(q5d) - cos(q1d)*cos(q2d)

*cos(q5d)*sin(q3d)*sin(q4d) - cos(q1d)*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) - cos(q1d)*cos
(q4d)*cos(q5d)*sin(q2d)*sin(q3d), cos(q2d)*cos(q5d)*sin(q1d)*sin(q3d)*sin(q4d) - cos(q2d)
*cos(q3d)*cos(q4d)*cos(q5d)*sin(q1d) - cos(q1d)*sin(q5d) + cos(q3d)*cos(q5d)*sin(q1d)*sin
(q2d)*sin(q4d) + cos(q4d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q3d), sin(q2d + q3d + q4d + q5d)
/2 + sin(q2d + q3d + q4d - q5d)/2, (L4*cos(B + q3d + q4d + q5d))/2 + (L3*cos(q2d + q3d +
q4d + q5d))/2 + (L4*cos(B + q3d + q4d - q5d))/2 + L6*cos(q5d) + (L3*cos(q2d + q3d + q4d -
q5d))/2 + (L5*cos(B - q4d + q5d))/2 + (L5*cos(q4d - B + q5d))/2]
MATLAB Command Window Page 2

[ cos(q5d)*sin(q1d) + cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*sin(q5d) - cos(q1d)*cos(q2d)

*sin(q3d)*sin(q4d)*sin(q5d) - cos(q1d)*cos(q3d)*sin(q2d)*sin(q4d)*sin(q5d) - cos(q1d)*cos
(q4d)*sin(q2d)*sin(q3d)*sin(q5d), cos(q1d)*cos(q5d) - cos(q2d)*cos(q3d)*cos(q4d)*sin(q1d)
*sin(q5d) + cos(q2d)*sin(q1d)*sin(q3d)*sin(q4d)*sin(q5d) + cos(q3d)*sin(q1d)*sin(q2d)*sin
(q4d)*sin(q5d) + cos(q4d)*sin(q1d)*sin(q2d)*sin(q3d)*sin(q5d), cos(q2d + q3d + q4d - q5d)
/2 - cos(q2d + q3d + q4d + q5d)/2, (L5*sin(B - q4d + q5d))/2 + (L5*sin(q4d - B + q5d))/2
+ (L4*sin(B + q3d + q4d + q5d))/2 + (L3*sin(q2d + q3d + q4d + q5d))/2 - (L4*sin(B + q3d +
q4d - q5d))/2 + L6*sin(q5d) - (L3*sin(q2d + q3d + q4d - q5d))/2]
[
- sin(q1d + q2d + q3d + q4d)/2 - sin(q2d - q1d + q3d + q4d)/2,
cos(q2d - q1d + q3d + q4d)/2 - cos(q1d + q2d + q3d + q4d)/2,
cos(q2d + q3d + q4d),
L5*sin(B - q4d) - L4*sin(B + q3d + q4d) - L3*sin(q2d + q3d + q4d)]
[
0,
0, 0,
1]

T65d =

[ 1, 0, 0, L7]
[ 0, 1, 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]

T0Cd =

[ 1, 0, 0, L2]
[ 0, 0, -1, -L1]
[ 0, 1, 0, 0]
[ 0, 0, 0, 1]

T6Cd =

[ cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d) - sin(q1d)*sin(q5d) - cos(q1d)*cos(q2d)

*cos(q5d)*sin(q3d)*sin(q4d) - cos(q1d)*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) - cos(q1d)*cos
(q4d)*cos(q5d)*sin(q2d)*sin(q3d), sin(q2d + q3d + q4d + q5d)/2 + sin(q2d + q3d + q4d -
q5d)/2, cos(q1d)*sin(q5d) + cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d)*sin(q1d) - cos(q2d)*cos
(q5d)*sin(q1d)*sin(q3d)*sin(q4d) - cos(q3d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q4d) - cos
(q4d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q3d), L7 + L6*cos(q5d) + L1*cos(q1d)*sin(q5d) -
L2*sin(q1d)*sin(q5d) + L5*sin(B)*cos(q5d)*sin(q4d) + L5*cos(B)*cos(q4d)*cos(q5d) - L3*cos
(q2d)*cos(q5d)*sin(q3d)*sin(q4d) - L3*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) - L3*cos(q4d)
*cos(q5d)*sin(q2d)*sin(q3d) + L4*cos(B)*cos(q3d)*cos(q4d)*cos(q5d) - L4*cos(B)*cos(q5d)
*sin(q3d)*sin(q4d) - L4*sin(B)*cos(q3d)*cos(q5d)*sin(q4d) - L4*sin(B)*cos(q4d)*cos(q5d)
*sin(q3d) + L3*cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d) - L1*cos(q2d)*cos(q5d)*sin(q1d)*sin
(q3d)*sin(q4d) - L1*cos(q3d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q4d) - L1*cos(q4d)*cos(q5d)
*sin(q1d)*sin(q2d)*sin(q3d) + L2*cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d) + L1*cos
(q2d)*cos(q3d)*cos(q4d)*cos(q5d)*sin(q1d) - L2*cos(q1d)*cos(q2d)*cos(q5d)*sin(q3d)*sin
MATLAB Command Window Page 3

(q4d) - L2*cos(q1d)*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) - L2*cos(q1d)*cos(q4d)*cos(q5d)

*sin(q2d)*sin(q3d)]
[ cos(q5d)*sin(q1d) + cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*sin(q5d) - cos(q1d)*cos(q2d)
*sin(q3d)*sin(q4d)*sin(q5d) - cos(q1d)*cos(q3d)*sin(q2d)*sin(q4d)*sin(q5d) - cos(q1d)*cos
(q4d)*sin(q2d)*sin(q3d)*sin(q5d), cos(q2d + q3d + q4d - q5d)/2 - cos(q2d + q3d + q4d +
q5d)/2, cos(q2d)*cos(q3d)*cos(q4d)*sin(q1d)*sin(q5d) - cos(q1d)*cos(q5d) - cos(q2d)*sin
(q1d)*sin(q3d)*sin(q4d)*sin(q5d) - cos(q3d)*sin(q1d)*sin(q2d)*sin(q4d)*sin(q5d) - cos
(q4d)*sin(q1d)*sin(q2d)*sin(q3d)*sin(q5d), L6*sin(q5d) - L1*cos(q1d)*cos(q5d) +
L2*cos(q5d)*sin(q1d) + L5*cos(B)*cos(q4d)*sin(q5d) + L5*sin(B)*sin(q4d)*sin(q5d) + L3*cos
(q2d)*cos(q3d)*cos(q4d)*sin(q5d) - L3*cos(q2d)*sin(q3d)*sin(q4d)*sin(q5d) - L3*cos(q3d)
*sin(q2d)*sin(q4d)*sin(q5d) - L3*cos(q4d)*sin(q2d)*sin(q3d)*sin(q5d) + L4*cos(B)*cos(q3d)
*cos(q4d)*sin(q5d) - L4*cos(B)*sin(q3d)*sin(q4d)*sin(q5d) - L4*sin(B)*cos(q3d)*sin(q4d)
*sin(q5d) - L4*sin(B)*cos(q4d)*sin(q3d)*sin(q5d) - L2*cos(q1d)*cos(q2d)*sin(q3d)*sin(q4d)
*sin(q5d) - L2*cos(q1d)*cos(q3d)*sin(q2d)*sin(q4d)*sin(q5d) - L2*cos(q1d)*cos(q4d)*sin
(q2d)*sin(q3d)*sin(q5d) - L1*cos(q2d)*sin(q1d)*sin(q3d)*sin(q4d)*sin(q5d) - L1*cos(q3d)
*sin(q1d)*sin(q2d)*sin(q4d)*sin(q5d) - L1*cos(q4d)*sin(q1d)*sin(q2d)*sin(q3d)*sin(q5d) +
L2*cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*sin(q5d) + L1*cos(q2d)*cos(q3d)*cos(q4d)*sin(q1d)
*sin(q5d)]
[
- sin(q1d + q2d + q3d + q4d)/2 - sin(q2d - q1d + q3d + q4d)/2,
cos(q2d + q3d + q4d),
cos(q1d + q2d + q3d + q4d)/2 - cos(q2d - q1d + q3d + q4d)/2,
L1*(cos(q1d + q2d + q3d + q4d)/2 - cos(q2d - q1d + q3d + q4d)/2) - L2*(sin(q1d + q2d +
q3d + q4d)/2 + sin(q2d - q1d + q3d + q4d)/2) + L5*sin(B - q4d) - L4*sin(B + q3d + q4d) -
L3*sin(q2d + q3d + q4d)]
[
0, 0,
0,
1]

T12i =

[ cos(q1i), 0, sin(q1i), L3*cos(q1i)]

[ sin(q1i), 0, -cos(q1i), L3*sin(q1i)]
[ 0, 1, 0, 0]
[ 0, 0, 0, 1]

T23i =

[ cos(B + q2i), -sin(B + q2i), 0, L4*cos(B + q2i)]

[ sin(B + q2i), cos(B + q2i), 0, L4*sin(B + q2i)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]

T34i =

[ cos(2*B - q3i), sin(2*B - q3i), 0, L5*cos(2*B - q3i)]

[ -sin(2*B - q3i), cos(2*B - q3i), 0, -L5*sin(2*B - q3i)]
MATLAB Command Window Page 4

[ 0, 0, 1, 0]
[ 0, 0, 0, 1]

T45i =

[ cos(B + q4i), 0, -sin(B + q4i), L6*cos(B + q4i)]

[ sin(B + q4i), 0, cos(B + q4i), L6*sin(B + q4i)]
[ 0, -1, 0, 0]
[ 0, 0, 0, 1]

T56i =

[ cos(q5i), -sin(q5i), 0, L7*cos(q5i)]

[ sin(q5i), cos(q5i), 0, L7*sin(q5i)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]

T16i =

[ cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i) - sin(q1i)*sin(q5i) - cos(q1i)*cos(q2i)

*cos(q5i)*sin(q3i)*sin(q4i) - cos(q1i)*cos(q3i)*cos(q5i)*sin(q2i)*sin(q4i) - cos(q1i)*cos
(q4i)*cos(q5i)*sin(q2i)*sin(q3i), cos(q1i)*cos(q2i)*sin(q3i)*sin(q4i)*sin(q5i) - cos(q1i)
*cos(q2i)*cos(q3i)*cos(q4i)*sin(q5i) - cos(q5i)*sin(q1i) + cos(q1i)*cos(q3i)*sin(q2i)*sin
(q4i)*sin(q5i) + cos(q1i)*cos(q4i)*sin(q2i)*sin(q3i)*sin(q5i), - sin(q1i + q2i + q3i +
q4i)/2 - sin(q2i - q1i + q3i + q4i)/2, L3*cos(q1i) - L7*sin(q1i)*sin(q5i) + L4*cos(B +
q2i)*cos(q1i) + L5*cos(2*B - q3i)*cos(B + q2i)*cos(q1i) + L6*cos(q2i - B + q3i)*cos(B +
q4i)*cos(q1i) + L5*sin(2*B - q3i)*sin(B + q2i)*cos(q1i) - L6*sin(q2i - B + q3i)*sin(B +
q4i)*cos(q1i) + L7*cos(q2i + q3i + q4i)*cos(q1i)*cos(q5i)]
[ cos(q1i)*sin(q5i) + cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin(q1i) - cos(q2i)*cos(q5i)
*sin(q1i)*sin(q3i)*sin(q4i) - cos(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) - cos(q4i)*cos
(q5i)*sin(q1i)*sin(q2i)*sin(q3i), cos(q1i)*cos(q5i) - cos(q2i)*cos(q3i)*cos(q4i)*sin(q1i)
*sin(q5i) + cos(q2i)*sin(q1i)*sin(q3i)*sin(q4i)*sin(q5i) + cos(q3i)*sin(q1i)*sin(q2i)*sin
(q4i)*sin(q5i) + cos(q4i)*sin(q1i)*sin(q2i)*sin(q3i)*sin(q5i), cos(q1i + q2i + q3i +
q4i)/2 - cos(q2i - q1i + q3i + q4i)/2, L3*sin(q1i) + L4*cos(B + q2i)*sin(q1i) + L7*cos
(q1i)*sin(q5i) + L5*cos(2*B - q3i)*cos(B + q2i)*sin(q1i) + L6*cos(q2i - B + q3i)*cos(B +
q4i)*sin(q1i) + L5*sin(2*B - q3i)*sin(B + q2i)*sin(q1i) - L6*sin(q2i - B + q3i)*sin(B +
q4i)*sin(q1i) + L7*cos(q2i + q3i + q4i)*cos(q5i)*sin(q1i)]
[
sin(q2i + q3i + q4i + q5i)/2 + sin(q2i + q3i + q4i - q5i)/2,
cos(q2i + q3i + q4i + q5i)/2 - cos(q2i + q3i + q4i - q5i)/2,
cos(q2i + q3i + q4i),
L5*sin(q2i - B + q3i) + L4*sin(B + q2i) + (L7*sin(q2i + q3i + q4i + q5i))/2 + (L7*sin(q2i
+ q3i + q4i - q5i))/2 + L6*sin(q2i + q3i + q4i)]
[
0,
0, 0,
1]
MATLAB Command Window Page 5

TC1i =

[ -1, 0, 0, -L2]
[ 0, 0, 1, 0]
[ 0, 1, 0, L1]
[ 0, 0, 0, 1]

TC6i =

[ sin(q1i)*sin(q5i) - cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i) + cos(q1i)*cos(q2i)

*cos(q5i)*sin(q3i)*sin(q4i) + cos(q1i)*cos(q3i)*cos(q5i)*sin(q2i)*sin(q4i) + cos(q1i)*cos
(q4i)*cos(q5i)*sin(q2i)*sin(q3i), cos(q5i)*sin(q1i) + cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)
*sin(q5i) - cos(q1i)*cos(q2i)*sin(q3i)*sin(q4i)*sin(q5i) - cos(q1i)*cos(q3i)*sin(q2i)*sin
(q4i)*sin(q5i) - cos(q1i)*cos(q4i)*sin(q2i)*sin(q3i)*sin(q5i), sin(q1i + q2i + q3i + q4i)
/2 + sin(q2i - q1i + q3i + q4i)/2, L7*sin(q1i)*sin(q5i) - L3*cos(q1i) - L2 - L4*cos(B +
q2i)*cos(q1i) - L5*cos(2*B - q3i)*cos(B + q2i)*cos(q1i) - L6*cos(q2i - B + q3i)*cos(B +
q4i)*cos(q1i) - L5*sin(2*B - q3i)*sin(B + q2i)*cos(q1i) + L6*sin(q2i - B + q3i)*sin(B +
q4i)*cos(q1i) - L7*cos(q2i + q3i + q4i)*cos(q1i)*cos(q5i)]
[
sin(q2i + q3i + q4i + q5i)/2 + sin(q2i + q3i + q4i - q5i)/2,
cos(q2i + q3i + q4i + q5i)/2 - cos(q2i + q3i + q4i - q5i)/2,
cos(q2i + q3i + q4i),
L5*sin(q2i - B + q3i) + L4*sin(B + q2i) + (L7*sin(q2i + q3i + q4i + q5i))/2 + (L7*sin(q2i
+ q3i + q4i - q5i))/2 + L6*sin(q2i + q3i + q4i)]
[ cos(q1i)*sin(q5i) + cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin(q1i) - cos(q2i)*cos(q5i)
*sin(q1i)*sin(q3i)*sin(q4i) - cos(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) - cos(q4i)*cos
(q5i)*sin(q1i)*sin(q2i)*sin(q3i), cos(q1i)*cos(q5i) - cos(q2i)*cos(q3i)*cos(q4i)*sin(q1i)
*sin(q5i) + cos(q2i)*sin(q1i)*sin(q3i)*sin(q4i)*sin(q5i) + cos(q3i)*sin(q1i)*sin(q2i)*sin
(q4i)*sin(q5i) + cos(q4i)*sin(q1i)*sin(q2i)*sin(q3i)*sin(q5i), cos(q1i + q2i + q3i + q4i)
/2 - cos(q2i - q1i + q3i + q4i)/2, L1 + L3*sin(q1i) + L4*cos(B + q2i)*sin(q1i) + L7*cos
(q1i)*sin(q5i) + L5*cos(2*B - q3i)*cos(B + q2i)*sin(q1i) + L6*cos(q2i - B + q3i)*cos(B +
q4i)*sin(q1i) + L5*sin(2*B - q3i)*sin(B + q2i)*sin(q1i) - L6*sin(q2i - B + q3i)*sin(B +
q4i)*sin(q1i) + L7*cos(q2i + q3i + q4i)*cos(q5i)*sin(q1i)]
[
0,
0, 0,
1]

PIE FLOTANTE RESPECTO AL PIE DE APOYO

TPFPA =

[ (sin(q2d + q3d + q4d + q5d)/2 + sin(q2d + q3d + q4d - q5d)/2)*(sin(q2i + q3i + q4i +
q5i)/2 + sin(q2i + q3i + q4i - q5i)/2) - (sin(q1d)*sin(q5d) - cos(q1d)*cos(q2d)*cos(q3d)
*cos(q4d)*cos(q5d) + cos(q1d)*cos(q2d)*cos(q5d)*sin(q3d)*sin(q4d) + cos(q1d)*cos(q3d)*cos
(q5d)*sin(q2d)*sin(q4d) + cos(q1d)*cos(q4d)*cos(q5d)*sin(q2d)*sin(q3d))*(sin(q1i)*sin
(q5i) - cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i) + cos(q1i)*cos(q2i)*cos(q5i)*sin
(q3i)*sin(q4i) + cos(q1i)*cos(q3i)*cos(q5i)*sin(q2i)*sin(q4i) + cos(q1i)*cos(q4i)*cos
(q5i)*sin(q2i)*sin(q3i)) + (cos(q2d)*cos(q5d)*sin(q1d)*sin(q3d)*sin(q4d) - cos(q2d)*cos
MATLAB Command Window Page 6

(q3d)*cos(q4d)*cos(q5d)*sin(q1d) - cos(q1d)*sin(q5d) + cos(q3d)*cos(q5d)*sin(q1d)*sin

(q2d)*sin(q4d) + cos(q4d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q3d))*(cos(q2i)*cos(q5i)*sin
(q1i)*sin(q3i)*sin(q4i) - cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin(q1i) - cos(q1i)*sin
(q5i) + cos(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) + cos(q4i)*cos(q5i)*sin(q1i)*sin
(q2i)*sin(q3i)), (cos(q1i)*cos(q2i)*sin(q3i)*sin(q4i)*sin(q5i) - cos(q1i)*cos(q2i)*cos
(q3i)*cos(q4i)*sin(q5i) - cos(q5i)*sin(q1i) + cos(q1i)*cos(q3i)*sin(q2i)*sin(q4i)*sin
(q5i) + cos(q1i)*cos(q4i)*sin(q2i)*sin(q3i)*sin(q5i))*(sin(q1d)*sin(q5d) - cos(q1d)*cos
(q2d)*cos(q3d)*cos(q4d)*cos(q5d) + cos(q1d)*cos(q2d)*cos(q5d)*sin(q3d)*sin(q4d) + cos
(q1d)*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) + cos(q1d)*cos(q4d)*cos(q5d)*sin(q2d)*sin(q3d))
+ (cos(q2i + q3i + q4i + q5i)/2 - cos(q2i + q3i + q4i - q5i)/2)*(sin(q2d + q3d + q4d +
q5d)/2 + sin(q2d + q3d + q4d - q5d)/2) - (cos(q2d)*cos(q5d)*sin(q1d)*sin(q3d)*sin(q4d) -
cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d)*sin(q1d) - cos(q1d)*sin(q5d) + cos(q3d)*cos(q5d)*sin
(q1d)*sin(q2d)*sin(q4d) + cos(q4d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q3d))*(cos(q1i)*cos
(q5i) - cos(q2i)*cos(q3i)*cos(q4i)*sin(q1i)*sin(q5i) + cos(q2i)*sin(q1i)*sin(q3i)*sin
(q4i)*sin(q5i) + cos(q3i)*sin(q1i)*sin(q2i)*sin(q4i)*sin(q5i) + cos(q4i)*sin(q1i)*sin
(q2i)*sin(q3i)*sin(q5i)), cos(q2i + q3i + q4i)*(sin(q2d + q3d + q4d + q5d)/2 + sin(q2d
+ q3d + q4d - q5d)/2) - (cos(q1i + q2i + q3i + q4i)/2 - cos(q2i - q1i + q3i + q4i)/2)*
(cos(q2d)*cos(q5d)*sin(q1d)*sin(q3d)*sin(q4d) - cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d)*sin
(q1d) - cos(q1d)*sin(q5d) + cos(q3d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q4d) + cos(q4d)*cos
(q5d)*sin(q1d)*sin(q2d)*sin(q3d)) - (sin(q1i + q2i + q3i + q4i)/2 + sin(q2i - q1i + q3i +
q4i)/2)*(sin(q1d)*sin(q5d) - cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d) + cos(q1d)*cos
(q2d)*cos(q5d)*sin(q3d)*sin(q4d) + cos(q1d)*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) + cos
(q1d)*cos(q4d)*cos(q5d)*sin(q2d)*sin(q3d)), L7 + (sin(q2d + q3d + q4d + q5d)/2 + sin(q2d
+ q3d + q4d - q5d)/2)*(L5*sin(q2i - B + q3i) + L4*sin(B + q2i) + (L7*sin(q2i + q3i + q4i
+ q5i))/2 + (L7*sin(q2i + q3i + q4i - q5i))/2 + L6*sin(q2i + q3i + q4i)) + (sin(q1d)*sin
(q5d) - cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d) + cos(q1d)*cos(q2d)*cos(q5d)*sin
(q3d)*sin(q4d) + cos(q1d)*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) + cos(q1d)*cos(q4d)*cos
(q5d)*sin(q2d)*sin(q3d))*(L2 + L3*cos(q1i) - L7*sin(q1i)*sin(q5i) + L4*cos(B + q2i)*cos
(q1i) + L5*cos(2*B - q3i)*cos(B + q2i)*cos(q1i) + L6*cos(q2i - B + q3i)*cos(B + q4i)*cos
(q1i) + L5*sin(2*B - q3i)*sin(B + q2i)*cos(q1i) - L6*sin(q2i - B + q3i)*sin(B + q4i)*cos
(q1i) + L7*cos(q2i + q3i + q4i)*cos(q1i)*cos(q5i)) + L6*cos(q5d) - (cos(q2d)*cos(q5d)*sin
(q1d)*sin(q3d)*sin(q4d) - cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d)*sin(q1d) - cos(q1d)*sin
(q5d) + cos(q3d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q4d) + cos(q4d)*cos(q5d)*sin(q1d)*sin
(q2d)*sin(q3d))*(L1 + L3*sin(q1i) + L4*cos(B + q2i)*sin(q1i) + L7*cos(q1i)*sin(q5i) +
L5*cos(2*B - q3i)*cos(B + q2i)*sin(q1i) + L6*cos(q2i - B + q3i)*cos(B + q4i)*sin(q1i) +
L5*sin(2*B - q3i)*sin(B + q2i)*sin(q1i) - L6*sin(q2i - B + q3i)*sin(B + q4i)*sin(q1i) +
L7*cos(q2i + q3i + q4i)*cos(q5i)*sin(q1i)) + L1*cos(q1d)*sin(q5d) - L2*sin(q1d)*sin(q5d)
+ L5*sin(B)*cos(q5d)*sin(q4d) + L5*cos(B)*cos(q4d)*cos(q5d) - L3*cos(q2d)*cos(q5d)*sin
(q3d)*sin(q4d) - L3*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) - L3*cos(q4d)*cos(q5d)*sin(q2d)
*sin(q3d) + L4*cos(B)*cos(q3d)*cos(q4d)*cos(q5d) - L4*cos(B)*cos(q5d)*sin(q3d)*sin(q4d) -
L4*sin(B)*cos(q3d)*cos(q5d)*sin(q4d) - L4*sin(B)*cos(q4d)*cos(q5d)*sin(q3d) + L3*cos(q2d)
*cos(q3d)*cos(q4d)*cos(q5d) - L1*cos(q2d)*cos(q5d)*sin(q1d)*sin(q3d)*sin(q4d) - L1*cos
(q3d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q4d) - L1*cos(q4d)*cos(q5d)*sin(q1d)*sin(q2d)*sin
(q3d) + L2*cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d) + L1*cos(q2d)*cos(q3d)*cos(q4d)
*cos(q5d)*sin(q1d) - L2*cos(q1d)*cos(q2d)*cos(q5d)*sin(q3d)*sin(q4d) - L2*cos(q1d)*cos
(q3d)*cos(q5d)*sin(q2d)*sin(q4d) - L2*cos(q1d)*cos(q4d)*cos(q5d)*sin(q2d)*sin(q3d)]
[ (cos(q2i)*cos(q5i)*sin(q1i)*sin(q3i)*sin(q4i) - cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin
(q1i) - cos(q1i)*sin(q5i) + cos(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) + cos(q4i)*cos
(q5i)*sin(q1i)*sin(q2i)*sin(q3i))*(cos(q1d)*cos(q5d) - cos(q2d)*cos(q3d)*cos(q4d)*sin
(q1d)*sin(q5d) + cos(q2d)*sin(q1d)*sin(q3d)*sin(q4d)*sin(q5d) + cos(q3d)*sin(q1d)*sin
(q2d)*sin(q4d)*sin(q5d) + cos(q4d)*sin(q1d)*sin(q2d)*sin(q3d)*sin(q5d)) - (cos(q2d + q3d
MATLAB Command Window Page 7

+ q4d + q5d)/2 - cos(q2d + q3d + q4d - q5d)/2)*(sin(q2i + q3i + q4i + q5i)/2 + sin(q2i +
q3i + q4i - q5i)/2) - (cos(q1d)*cos(q2d)*sin(q3d)*sin(q4d)*sin(q5d) - cos(q1d)*cos(q2d)
*cos(q3d)*cos(q4d)*sin(q5d) - cos(q5d)*sin(q1d) + cos(q1d)*cos(q3d)*sin(q2d)*sin(q4d)*sin
(q5d) + cos(q1d)*cos(q4d)*sin(q2d)*sin(q3d)*sin(q5d))*(sin(q1i)*sin(q5i) - cos(q1i)*cos
(q2i)*cos(q3i)*cos(q4i)*cos(q5i) + cos(q1i)*cos(q2i)*cos(q5i)*sin(q3i)*sin(q4i) + cos
(q1i)*cos(q3i)*cos(q5i)*sin(q2i)*sin(q4i) + cos(q1i)*cos(q4i)*cos(q5i)*sin(q2i)*sin
(q3i)), (cos(q1d)*cos(q2d)*sin(q3d)*sin(q4d)*sin(q5d) - cos(q1d)*cos(q2d)*cos(q3d)*cos
(q4d)*sin(q5d) - cos(q5d)*sin(q1d) + cos(q1d)*cos(q3d)*sin(q2d)*sin(q4d)*sin(q5d) + cos
(q1d)*cos(q4d)*sin(q2d)*sin(q3d)*sin(q5d))*(cos(q1i)*cos(q2i)*sin(q3i)*sin(q4i)*sin(q5i)
- cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*sin(q5i) - cos(q5i)*sin(q1i) + cos(q1i)*cos(q3i)
*sin(q2i)*sin(q4i)*sin(q5i) + cos(q1i)*cos(q4i)*sin(q2i)*sin(q3i)*sin(q5i)) - (cos(q2d +
q3d + q4d + q5d)/2 - cos(q2d + q3d + q4d - q5d)/2)*(cos(q2i + q3i + q4i + q5i)/2 - cos
(q2i + q3i + q4i - q5i)/2) - (cos(q1d)*cos(q5d) - cos(q2d)*cos(q3d)*cos(q4d)*sin(q1d)*sin
(q5d) + cos(q2d)*sin(q1d)*sin(q3d)*sin(q4d)*sin(q5d) + cos(q3d)*sin(q1d)*sin(q2d)*sin
(q4d)*sin(q5d) + cos(q4d)*sin(q1d)*sin(q2d)*sin(q3d)*sin(q5d))*(cos(q1i)*cos(q5i) - cos
(q2i)*cos(q3i)*cos(q4i)*sin(q1i)*sin(q5i) + cos(q2i)*sin(q1i)*sin(q3i)*sin(q4i)*sin(q5i)
+ cos(q3i)*sin(q1i)*sin(q2i)*sin(q4i)*sin(q5i) + cos(q4i)*sin(q1i)*sin(q2i)*sin(q3i)*sin
(q5i)), - cos(q2i + q3i + q4i)*(cos(q2d + q3d + q4d + q5d)/2 - cos(q2d + q3d + q4d - q5d)
/2) - (sin(q1i + q2i + q3i + q4i)/2 + sin(q2i - q1i + q3i + q4i)/2)*(cos(q1d)*cos(q2d)
*sin(q3d)*sin(q4d)*sin(q5d) - cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*sin(q5d) - cos(q5d)*sin
(q1d) + cos(q1d)*cos(q3d)*sin(q2d)*sin(q4d)*sin(q5d) + cos(q1d)*cos(q4d)*sin(q2d)*sin
(q3d)*sin(q5d)) - (cos(q1i + q2i + q3i + q4i)/2 - cos(q2i - q1i + q3i + q4i)/2)*(cos(q1d)
*cos(q5d) - cos(q2d)*cos(q3d)*cos(q4d)*sin(q1d)*sin(q5d) + cos(q2d)*sin(q1d)*sin(q3d)*sin
(q4d)*sin(q5d) + cos(q3d)*sin(q1d)*sin(q2d)*sin(q4d)*sin(q5d) + cos(q4d)*sin(q1d)*sin
(q2d)*sin(q3d)*sin(q5d)), L6*sin(q5d) - (cos(q2d + q3d + q4d + q5d)/2 - cos(q2d +
q3d + q4d - q5d)/2)*(L5*sin(q2i - B + q3i) + L4*sin(B + q2i) + (L7*sin(q2i + q3i + q4i +
q5i))/2 + (L7*sin(q2i + q3i + q4i - q5i))/2 + L6*sin(q2i + q3i + q4i)) - (cos(q1d)*cos
(q5d) - cos(q2d)*cos(q3d)*cos(q4d)*sin(q1d)*sin(q5d) + cos(q2d)*sin(q1d)*sin(q3d)*sin
(q4d)*sin(q5d) + cos(q3d)*sin(q1d)*sin(q2d)*sin(q4d)*sin(q5d) + cos(q4d)*sin(q1d)*sin
(q2d)*sin(q3d)*sin(q5d))*(L1 + L3*sin(q1i) + L4*cos(B + q2i)*sin(q1i) + L7*cos(q1i)*sin
(q5i) + L5*cos(2*B - q3i)*cos(B + q2i)*sin(q1i) + L6*cos(q2i - B + q3i)*cos(B + q4i)*sin
(q1i) + L5*sin(2*B - q3i)*sin(B + q2i)*sin(q1i) - L6*sin(q2i - B + q3i)*sin(B + q4i)*sin
(q1i) + L7*cos(q2i + q3i + q4i)*cos(q5i)*sin(q1i)) + (cos(q1d)*cos(q2d)*sin(q3d)*sin(q4d)
*sin(q5d) - cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*sin(q5d) - cos(q5d)*sin(q1d) + cos(q1d)
*cos(q3d)*sin(q2d)*sin(q4d)*sin(q5d) + cos(q1d)*cos(q4d)*sin(q2d)*sin(q3d)*sin(q5d))*(L2
+ L3*cos(q1i) - L7*sin(q1i)*sin(q5i) + L4*cos(B + q2i)*cos(q1i) + L5*cos(2*B - q3i)*cos(B
+ q2i)*cos(q1i) + L6*cos(q2i - B + q3i)*cos(B + q4i)*cos(q1i) + L5*sin(2*B - q3i)*sin(B +
q2i)*cos(q1i) - L6*sin(q2i - B + q3i)*sin(B + q4i)*cos(q1i) + L7*cos(q2i + q3i + q4i)*cos
(q1i)*cos(q5i)) - L1*cos(q1d)*cos(q5d) + L2*cos(q5d)*sin(q1d) + L5*cos(B)*cos(q4d)*sin
(q5d) + L5*sin(B)*sin(q4d)*sin(q5d) + L3*cos(q2d)*cos(q3d)*cos(q4d)*sin(q5d) - L3*cos
(q2d)*sin(q3d)*sin(q4d)*sin(q5d) - L3*cos(q3d)*sin(q2d)*sin(q4d)*sin(q5d) - L3*cos(q4d)
*sin(q2d)*sin(q3d)*sin(q5d) + L4*cos(B)*cos(q3d)*cos(q4d)*sin(q5d) - L4*cos(B)*sin(q3d)
*sin(q4d)*sin(q5d) - L4*sin(B)*cos(q3d)*sin(q4d)*sin(q5d) - L4*sin(B)*cos(q4d)*sin(q3d)
*sin(q5d) - L2*cos(q1d)*cos(q2d)*sin(q3d)*sin(q4d)*sin(q5d) - L2*cos(q1d)*cos(q3d)*sin
(q2d)*sin(q4d)*sin(q5d) - L2*cos(q1d)*cos(q4d)*sin(q2d)*sin(q3d)*sin(q5d) - L1*cos(q2d)
*sin(q1d)*sin(q3d)*sin(q4d)*sin(q5d) - L1*cos(q3d)*sin(q1d)*sin(q2d)*sin(q4d)*sin(q5d) -
L1*cos(q4d)*sin(q1d)*sin(q2d)*sin(q3d)*sin(q5d) + L2*cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)
*sin(q5d) + L1*cos(q2d)*cos(q3d)*cos(q4d)*sin(q1d)*sin(q5d)]
[
cos(q2d + q3d + q4d)*(sin(q2i + q3i + q4i + q5i)/2 + sin(q2i + q3i + q4i - q5i)/2) - (cos
(q1d + q2d + q3d + q4d)/2 - cos(q2d - q1d + q3d + q4d)/2)*(cos(q2i)*cos(q5i)*sin(q1i)*sin
MATLAB Command Window Page 8

(q3i)*sin(q4i) - cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin(q1i) - cos(q1i)*sin(q5i) + cos

(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) + cos(q4i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q3i))
- (sin(q1d + q2d + q3d + q4d)/2 + sin(q2d - q1d + q3d + q4d)/2)*(sin(q1i)*sin(q5i) - cos
(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i) + cos(q1i)*cos(q2i)*cos(q5i)*sin(q3i)*sin(q4i)
+ cos(q1i)*cos(q3i)*cos(q5i)*sin(q2i)*sin(q4i) + cos(q1i)*cos(q4i)*cos(q5i)*sin(q2i)*sin
(q3i)),
cos(q2d + q3d + q4d)*(cos(q2i + q3i + q4i + q5i)/2 - cos(q2i + q3i + q4i - q5i)/2) + (sin
(q1d + q2d + q3d + q4d)/2 + sin(q2d - q1d + q3d + q4d)/2)*(cos(q1i)*cos(q2i)*sin(q3i)*sin
(q4i)*sin(q5i) - cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*sin(q5i) - cos(q5i)*sin(q1i) + cos
(q1i)*cos(q3i)*sin(q2i)*sin(q4i)*sin(q5i) + cos(q1i)*cos(q4i)*sin(q2i)*sin(q3i)*sin(q5i))
+ (cos(q1d + q2d + q3d + q4d)/2 - cos(q2d - q1d + q3d + q4d)/2)*(cos(q1i)*cos(q5i) - cos
(q2i)*cos(q3i)*cos(q4i)*sin(q1i)*sin(q5i) + cos(q2i)*sin(q1i)*sin(q3i)*sin(q4i)*sin(q5i)
+ cos(q3i)*sin(q1i)*sin(q2i)*sin(q4i)*sin(q5i) + cos(q4i)*sin(q1i)*sin(q2i)*sin(q3i)*sin
(q5i)),
(cos(q1d + q2d + q3d + q4d)/2 - cos(q2d - q1d + q3d + q4d)/2)*(cos(q1i + q2i + q3i + q4i)
/2 - cos(q2i - q1i + q3i + q4i)/2) + cos(q2d + q3d + q4d)*cos(q2i + q3i + q4i) - (sin(q1d
+ q2d + q3d + q4d)/2 + sin(q2d - q1d + q3d + q4d)/2)*(sin(q1i + q2i + q3i + q4i)/2 + sin
(q2i - q1i + q3i + q4i)/2),
(sin(q1d + q2d + q3d + q4d)/2 + sin(q2d - q1d + q3d + q4d)/2)*(L2 + L3*cos(q1i) - L7*sin
(q1i)*sin(q5i) + L4*cos(B + q2i)*cos(q1i) + L5*cos(2*B - q3i)*cos(B + q2i)*cos(q1i) +
L6*cos(q2i - B + q3i)*cos(B + q4i)*cos(q1i) + L5*sin(2*B - q3i)*sin(B + q2i)*cos(q1i) -
L6*sin(q2i - B + q3i)*sin(B + q4i)*cos(q1i) + L7*cos(q2i + q3i + q4i)*cos(q1i)*cos(q5i))
+ L1*(cos(q1d + q2d + q3d + q4d)/2 - cos(q2d - q1d + q3d + q4d)/2) - L2*(sin(q1d + q2d +
q3d + q4d)/2 + sin(q2d - q1d + q3d + q4d)/2) + (cos(q1d + q2d + q3d + q4d)/2 - cos(q2d -
q1d + q3d + q4d)/2)*(L1 + L3*sin(q1i) + L4*cos(B + q2i)*sin(q1i) + L7*cos(q1i)*sin(q5i) +
L5*cos(2*B - q3i)*cos(B + q2i)*sin(q1i) + L6*cos(q2i - B + q3i)*cos(B + q4i)*sin(q1i) +
L5*sin(2*B - q3i)*sin(B + q2i)*sin(q1i) - L6*sin(q2i - B + q3i)*sin(B + q4i)*sin(q1i) +
L7*cos(q2i + q3i + q4i)*cos(q5i)*sin(q1i)) + L5*sin(B - q4d) - L4*sin(B + q3d + q4d) +
cos(q2d + q3d + q4d)*(L5*sin(q2i - B + q3i) + L4*sin(B + q2i) + (L7*sin(q2i + q3i + q4i +
q5i))/2 + (L7*sin(q2i + q3i + q4i - q5i))/2 + L6*sin(q2i + q3i + q4i)) - L3*sin(q2d + q3d
+ q4d)]
[
0,
0,
0,
1]

>>