if 0<=c_q(i,2) && c_q(i,2)<=360
Th1(i,1) = pi+pi/2+a_h_step(i,2)+q0hr(i,2);
Th2(i,1) = -q0kr(i,2)-a_k_step(i,2);
Th3(i,1) = pi/2-(a_a_step(i,2)+q0ar(i,2));
-L1*sin(Th1(i,1))-L2*sin(Th1(i,1)+Th2(i,1))-L3*sin(Th1(i,1)+Th2(i,1)+Th3(i,1)), -L2*sin(Th1(i,1)+Th2(i,1))-L3*sin(Th1(i,1)+Th2(i,1)+Th3(i,1)), -L3*sin(Th1(i,1)+Th2(i,1)+Th3(i,1));
L1*cos(Th1(i,1))+L2*cos(Th1(i,1)+Th2(i,1))+L3*cos(Th1(i,1)+Th2(i,1)+Th3(i,1)), L2*cos(Th1(i,1)+Th2(i,1))+L3*cos(Th1(i,1)+Th2(i,1)+Th3(i,1)), L3*cos(Th1(i,1)+Th2(i,1)+Th3(i,1));