S23*S5 - C23*C4*C5 = sx*C1*S6 - ny*C6*S1 - nx*C1*C6 + sy*S1*S6

C23*S4 = sx*C1*S6 - ny*C6*S1 - nx*C1*C6 + sy*S1*S6

C5*S23 + C23*C4*S5= ay*S1 + ax*C1

C23*S5 + S23*C4*C5 = nz*C6 -sz*S6

S23*S4 = -sz*C6 - nz*S6

S23*C4*S5 - C23*C5 - az=0

C5*S4 = nx*C6*S1 - ny*C1*C6 + sy*C1*S6 - sx*S1*S6

S4*S5 + ay*C1 - ax*S1=0

d3 + d5*C4 + py*C1 - px*S1=0

C4 = sx*C6*S1 - ny*C1*S6 - sy*C1*C6 + nx*S1*S6

C23 + az*C5 + (sz*S6 - nz*C6)*sin(q5)

cos(q2 + q3) + az*cos(q5) - nz*C6*sin(q5) + sz*sin(q5)*S6,