syms the1 the2 the3 the4 the5

Px = cos(the1)*(a3*cos(the2 + the3) + a2*cos(the2));

Py = sin(the1)*(a3*cos(the2 + the3) + a2*cos(the2));

Pz = a1 + a3*sin(the2 + the3) + a2*sin(the2);

px_new=vpa(subs(Px,[the1,the2,the3],[t1,t2,t3]));

py_new=vpa(subs(Py,[the1,the2,the3],[t1,t2,t3]));

pz_new=vpa(subs(Pz,[the1,the2,the3],[t1,t2,t3]));

[t1,t2,t3] = solve(Px==px_new,Py==py_new,Pz==pz_new,the1,the2,the3)

R03 = [cos(the2 + the3)*cos(the1) -sin(the2 + the3)*cos(the1) sin(the1);

cos(the2 + the3)*sin(the1) -sin(the2 + the3)*sin(the1) -cos(the1);

sin(the2 + the3) cos(the2 + the3) 0]

R46 = [cos(the4)*cos(the5) -cos(the4)*sin(the5) sin(the4);

cos(the5)*sin(the4) -sin(the4)*sin(the5) -cos(the4);

R03_1 = vpa(subs(R03,[the1,the2,the3],[t1(1),t2(1),t3(1)]))

R03_2 = vpa(subs(R03,[the1,the2,the3],[t1(2),t2(2),t3(2)]))

R03_3 = vpa(subs(R03,[the1,the2,the3],[t1(3),t2(3),t3(3)]))

R03_4 = vpa(subs(R03,[the1,the2,the3],[t1(4),t2(4),t3(4)]))

[t41,t51] = solve(sin(the4)==R46_1(1,3),sin(the5)==R46_1(3,1),the4,the5)

[t42,t52] = solve(sin(the4)==R46_2(1,3),sin(the5)==R46_2(3,1),the4,the5)

[t43,t53] = solve(sin(the4)==R46_3(1,3),sin(the5)==R46_3(3,1),the4,the5)

[t44,t54] = solve(sin(the4)==R46_4(1,3),sin(the5)==R46_4(3,1),the4,the5)

Theta4_1 = vpa((t41)*180/pi)

Theta5_1 = vpa((t51)*180/pi)

Theta4_2 = vpa((t42)*180/pi)

Theta5_2 = vpa((t52)*180/pi)

Theta4_3 = vpa((t43)*180/pi)

Theta5_3 = vpa((t53)*180/pi)

Theta4_4 = vpa((t44)*180/pi)

Theta5_4 = vpa((t54)*180/pi)