JJ_Vc.m

 
J_Vc =[1, 0, 0, 0, 1, 0, 0, 0, 1, d_b_y*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - d_b_z*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)), - d_b_z*cos(alpha_p)*cos(gamma_p) - d_b_y*cos(gamma_p)*sin(alpha_p), d_b_y*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - d_b_z*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)), d_b_y*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + d_b_z*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - d_b_x*cos(gamma_p)*sin(beta_p), 0, - d_b_y*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) - d_b_z*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - d_b_x*cos(beta_p)*cos(gamma_p), d_b_z*cos(beta_p)*cos(gamma_p)*sin(alpha_p) - d_b_y*cos(alpha_p)*cos(beta_p)*cos(gamma_p) - d_b_x*cos(beta_p)*sin(gamma_p), d_b_x*cos(gamma_p) - d_b_y*cos(alpha_p)*sin(gamma_p) + d_b_z*sin(alpha_p)*sin(gamma_p), d_b_x*sin(beta_p)*sin(gamma_p) + d_b_y*cos(alpha_p)*cos(gamma_p)*sin(beta_p) - d_b_z*cos(gamma_p)*sin(alpha_p)*sin(beta_p), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, d_hip_fr_y*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - (conj(W_b)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 - d_hip_fr_z*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), (conj(W_b)*cos(gamma_p)*sin(alpha_p))/2 - d_hip_fr_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) - d_hip_fr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)), d_hip_fr_y*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - (conj(W_b)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 - d_hip_fr_z*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), d_hip_fr_y*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - (conj(W_b)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_hip_fr_z*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_hip_fr_x*cos(gamma_p)*sin(beta_p) - (conj(L_b)*cos(gamma_p)*sin(beta_p))/2, 0, (conj(W_b)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 - d_hip_fr_y*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - d_hip_fr_z*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_hip_fr_x*cos(beta_p)*cos(gamma_p) - (conj(L_b)*cos(beta_p)*cos(gamma_p))/2, d_hip_fr_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fr) + cos(beta_p)*cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) - d_hip_fr_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_fr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) - d_hip_fr_x*cos(beta_p)*sin(gamma_p) - (conj(L_b)*cos(beta_p)*sin(gamma_p))/2 + (conj(W_b)*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, d_hip_fr_x*cos(gamma_p) + (conj(L_b)*cos(gamma_p))/2 - d_hip_fr_y*(cos(alpha_p)*cos(q1_fr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_fr)) + d_hip_fr_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_fr) + cos(q1_fr)*sin(alpha_p)*sin(gamma_p)) + (conj(W_b)*cos(alpha_p)*sin(gamma_p))/2, d_hip_fr_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_fr)) - d_hip_fr_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)*sin(beta_p)) + d_hip_fr_x*sin(beta_p)*sin(gamma_p) + (conj(L_b)*sin(beta_p)*sin(gamma_p))/2 - (conj(W_b)*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, d_hip_fr_y*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_hip_fr_z*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), - d_hip_fr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) - d_hip_fr_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)), d_hip_fr_y*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_hip_fr_z*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, L_h_z*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_h_y*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - (conj(W_b)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 + d_th_fr_y*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_th_fr_z*cos(q2_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + d_th_fr_x*sin(q2_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) - d_th_fr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) - d_th_fr_z*cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) + d_th_fr_x*sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) + (conj(W_b)*cos(gamma_p)*sin(alpha_p))/2, L_h_z*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_y*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - (conj(W_b)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_th_fr_y*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_th_fr_z*cos(q2_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_fr_x*sin(q2_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), d_th_fr_z*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fr)) - d_th_fr_x*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fr)*sin(beta_p)) - (conj(W_b)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 - L_h_y*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_z*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_fr_y*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_x*cos(gamma_p)*sin(beta_p) - (conj(L_b)*cos(gamma_p)*sin(beta_p))/2, 0, (conj(W_b)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 + d_th_fr_x*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fr)) - d_th_fr_z*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fr)) + L_h_y*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_h_z*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_th_fr_y*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_h_x*cos(beta_p)*cos(gamma_p) - (conj(L_b)*cos(beta_p)*cos(gamma_p))/2, L_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_fr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) - L_h_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fr) + cos(beta_p)*cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) - d_th_fr_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_fr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) - d_th_fr_x*(sin(q2_fr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fr) + cos(beta_p)*cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) + cos(beta_p)*cos(q2_fr)*sin(gamma_p)) + d_th_fr_z*(cos(q2_fr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fr) + cos(beta_p)*cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_fr)) - L_h_x*cos(beta_p)*sin(gamma_p) - (conj(L_b)*cos(beta_p)*sin(gamma_p))/2 + (conj(W_b)*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, L_h_x*cos(gamma_p) - d_th_fr_x*(sin(q2_fr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fr) + cos(q1_fr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_fr)) + d_th_fr_z*(cos(gamma_p)*sin(q2_fr) + cos(q2_fr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fr) + cos(q1_fr)*sin(alpha_p)*sin(gamma_p))) + L_h_y*(cos(alpha_p)*cos(q1_fr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_fr)) - L_h_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_fr) + cos(q1_fr)*sin(alpha_p)*sin(gamma_p)) + (conj(L_b)*cos(gamma_p))/2 - d_th_fr_y*(cos(alpha_p)*cos(q1_fr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_fr)) + (conj(W_b)*cos(alpha_p)*sin(gamma_p))/2, L_h_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)*sin(beta_p)) - L_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_fr)) + d_th_fr_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_fr)) + d_th_fr_x*(sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)*sin(beta_p)) + cos(q2_fr)*sin(beta_p)*sin(gamma_p)) - d_th_fr_z*(cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_fr)) + L_h_x*sin(beta_p)*sin(gamma_p) + (conj(L_b)*sin(beta_p)*sin(gamma_p))/2 - (conj(W_b)*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, L_h_z*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_h_y*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + d_th_fr_y*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_th_fr_z*cos(q2_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + d_th_fr_x*sin(q2_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) - d_th_fr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) - d_th_fr_z*cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) + d_th_fr_x*sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)), L_h_z*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_y*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_fr_y*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_th_fr_z*cos(q2_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_fr_x*sin(q2_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), - d_th_fr_x*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fr)) - d_th_fr_z*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fr)), d_th_fr_z*(cos(q2_fr)*sin(gamma_p) + sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p))) - d_th_fr_x*(sin(gamma_p)*sin(q2_fr) - cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p))), - d_th_fr_x*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fr)) - d_th_fr_z*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fr)*sin(beta_p)), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, d_sh_fr_x*(cos(q2_fr)*sin(q3_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_fr)*sin(q2_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - (conj(W_b)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 - d_sh_fr_z*(cos(q2_fr)*cos(q3_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_fr)*sin(q3_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - L_h_y*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_f_y*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + d_sh_fr_y*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_f_z*cos(q2_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_th_f_x*sin(q2_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), d_sh_fr_x*(cos(q2_fr)*sin(q3_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) + cos(q3_fr)*sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr))) - d_sh_fr_z*(cos(q2_fr)*cos(q3_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) - sin(q2_fr)*sin(q3_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr))) + L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) + L_th_f_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) - d_sh_fr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) + L_th_f_z*cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) + L_th_f_x*sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) + (conj(W_b)*cos(gamma_p)*sin(alpha_p))/2, d_sh_fr_x*(cos(q2_fr)*sin(q3_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_fr)*sin(q2_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - (conj(W_b)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 - d_sh_fr_z*(cos(q2_fr)*cos(q3_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_fr)*sin(q3_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - L_h_y*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_f_y*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_sh_fr_y*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_f_z*cos(q2_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_f_x*sin(q2_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), d_sh_fr_z*(cos(q3_fr)*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fr)) - sin(q3_fr)*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fr)*sin(beta_p))) - L_th_f_x*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fr)*sin(beta_p)) - L_th_f_z*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fr)) - L_h_y*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_z*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_f_y*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_sh_fr_x*(cos(q3_fr)*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fr)*sin(beta_p)) + sin(q3_fr)*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fr))) - (conj(W_b)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_sh_fr_y*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_x*cos(gamma_p)*sin(beta_p) - (conj(L_b)*cos(gamma_p)*sin(beta_p))/2, 0, L_th_f_x*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fr)) + L_th_f_z*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fr)) + (conj(W_b)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 + d_sh_fr_x*(cos(q3_fr)*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fr)) + sin(q3_fr)*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fr))) - d_sh_fr_z*(cos(q3_fr)*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fr)) - sin(q3_fr)*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fr))) + L_h_y*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_h_z*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_f_y*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - d_sh_fr_y*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_h_x*cos(beta_p)*cos(gamma_p) - (conj(L_b)*cos(beta_p)*cos(gamma_p))/2, L_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_fr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) - L_h_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fr) + cos(beta_p)*cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) + L_th_f_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_fr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) - d_sh_fr_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_fr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) - d_sh_fr_x*(cos(q3_fr)*(sin(q2_fr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fr) + cos(beta_p)*cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) + cos(beta_p)*cos(q2_fr)*sin(gamma_p)) + sin(q3_fr)*(cos(q2_fr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fr) + cos(beta_p)*cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_fr))) + d_sh_fr_z*(cos(q3_fr)*(cos(q2_fr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fr) + cos(beta_p)*cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_fr)) - sin(q3_fr)*(sin(q2_fr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fr) + cos(beta_p)*cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) + cos(beta_p)*cos(q2_fr)*sin(gamma_p))) - L_th_f_x*(sin(q2_fr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fr) + cos(beta_p)*cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) + cos(beta_p)*cos(q2_fr)*sin(gamma_p)) - L_th_f_z*(cos(q2_fr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fr) + cos(beta_p)*cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_fr)) - L_h_x*cos(beta_p)*sin(gamma_p) - (conj(L_b)*cos(beta_p)*sin(gamma_p))/2 + (conj(W_b)*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, L_h_x*cos(gamma_p) - d_sh_fr_z*(sin(q3_fr)*(sin(q2_fr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fr) + cos(q1_fr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_fr)) - cos(q3_fr)*(cos(gamma_p)*sin(q2_fr) + cos(q2_fr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fr) + cos(q1_fr)*sin(alpha_p)*sin(gamma_p)))) - L_th_f_x*(sin(q2_fr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fr) + cos(q1_fr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_fr)) - L_th_f_z*(cos(gamma_p)*sin(q2_fr) + cos(q2_fr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fr) + cos(q1_fr)*sin(alpha_p)*sin(gamma_p))) - d_sh_fr_x*(sin(q3_fr)*(cos(gamma_p)*sin(q2_fr) + cos(q2_fr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fr) + cos(q1_fr)*sin(alpha_p)*sin(gamma_p))) + cos(q3_fr)*(sin(q2_fr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fr) + cos(q1_fr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_fr))) + L_h_y*(cos(alpha_p)*cos(q1_fr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_fr)) - L_h_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_fr) + cos(q1_fr)*sin(alpha_p)*sin(gamma_p)) + L_th_f_y*(cos(alpha_p)*cos(q1_fr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_fr)) + (conj(L_b)*cos(gamma_p))/2 - d_sh_fr_y*(cos(alpha_p)*cos(q1_fr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_fr)) + (conj(W_b)*cos(alpha_p)*sin(gamma_p))/2, d_sh_fr_x*(cos(q3_fr)*(sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)*sin(beta_p)) + cos(q2_fr)*sin(beta_p)*sin(gamma_p)) + sin(q3_fr)*(cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_fr))) - d_sh_fr_z*(cos(q3_fr)*(cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_fr)) - sin(q3_fr)*(sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)*sin(beta_p)) + cos(q2_fr)*sin(beta_p)*sin(gamma_p))) - L_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_fr)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)*sin(beta_p)) - L_th_f_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_fr)) + d_sh_fr_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_fr)) + L_th_f_x*(sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)*sin(beta_p)) + cos(q2_fr)*sin(beta_p)*sin(gamma_p)) + L_th_f_z*(cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_fr)) + L_h_x*sin(beta_p)*sin(gamma_p) + (conj(L_b)*sin(beta_p)*sin(gamma_p))/2 - (conj(W_b)*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, d_sh_fr_x*(cos(q2_fr)*sin(q3_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_fr)*sin(q2_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - d_sh_fr_z*(cos(q2_fr)*cos(q3_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_fr)*sin(q3_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - L_h_y*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_f_y*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + d_sh_fr_y*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_f_z*cos(q2_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_th_f_x*sin(q2_fr)*(cos(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), d_sh_fr_x*(cos(q2_fr)*sin(q3_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) + cos(q3_fr)*sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr))) - d_sh_fr_z*(cos(q2_fr)*cos(q3_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) - sin(q2_fr)*sin(q3_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr))) + L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) + L_th_f_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) - d_sh_fr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)) + L_th_f_z*cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)) + L_th_f_x*sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fr) - cos(gamma_p)*sin(alpha_p)*sin(q1_fr)), d_sh_fr_x*(cos(q2_fr)*sin(q3_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_fr)*sin(q2_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - d_sh_fr_z*(cos(q2_fr)*cos(q3_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_fr)*sin(q3_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - L_h_y*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_f_y*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_sh_fr_y*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_f_z*cos(q2_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_f_x*sin(q2_fr)*(cos(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), L_th_f_z*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fr)) - L_th_f_x*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fr)) - d_sh_fr_x*(cos(q3_fr)*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fr)) - sin(q3_fr)*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fr))) - d_sh_fr_z*(cos(q3_fr)*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fr)) + sin(q3_fr)*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fr))), d_sh_fr_z*(cos(q3_fr)*(cos(q2_fr)*sin(gamma_p) + sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p))) - sin(q3_fr)*(sin(gamma_p)*sin(q2_fr) - cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)))) - d_sh_fr_x*(cos(q3_fr)*(sin(gamma_p)*sin(q2_fr) - cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p))) + sin(q3_fr)*(cos(q2_fr)*sin(gamma_p) + sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)))) - L_th_f_x*(sin(gamma_p)*sin(q2_fr) - cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p))) - L_th_f_z*(cos(q2_fr)*sin(gamma_p) + sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p))), L_th_f_z*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fr)*sin(beta_p)) - L_th_f_x*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fr)) - d_sh_fr_x*(cos(q3_fr)*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fr)) - sin(q3_fr)*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fr)*sin(beta_p))) - d_sh_fr_z*(cos(q3_fr)*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fr)*sin(beta_p)) + sin(q3_fr)*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fr))), - d_sh_fr_x*(cos(q3_fr)*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fr)) - sin(q3_fr)*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fr))) - d_sh_fr_z*(cos(q3_fr)*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fr)) + sin(q3_fr)*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fr))), d_sh_fr_z*(cos(q3_fr)*(cos(q2_fr)*sin(gamma_p) + sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p))) - sin(q3_fr)*(sin(gamma_p)*sin(q2_fr) - cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)))) - d_sh_fr_x*(cos(q3_fr)*(sin(gamma_p)*sin(q2_fr) - cos(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p))) + sin(q3_fr)*(cos(q2_fr)*sin(gamma_p) + sin(q2_fr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fr) + cos(gamma_p)*cos(q1_fr)*sin(alpha_p)))), - d_sh_fr_x*(cos(q3_fr)*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fr)) - sin(q3_fr)*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fr)*sin(beta_p))) - d_sh_fr_z*(cos(q3_fr)*(sin(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fr)*sin(beta_p)) + sin(q3_fr)*(cos(q2_fr)*(cos(q1_fr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fr))), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, (W_b*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 + d_hip_fl_y*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_hip_fl_z*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), - d_hip_fl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - d_hip_fl_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) - (W_b*cos(gamma_p)*sin(alpha_p))/2, (W_b*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_hip_fl_y*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_hip_fl_z*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), (W_b*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_hip_fl_y*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_hip_fl_z*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - (L_b*cos(gamma_p)*sin(beta_p))/2 - d_hip_fl_x*cos(gamma_p)*sin(beta_p), 0, - (W_b*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 - d_hip_fl_y*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - d_hip_fl_z*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - (L_b*cos(beta_p)*cos(gamma_p))/2 - d_hip_fl_x*cos(beta_p)*cos(gamma_p), d_hip_fl_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fl) + cos(beta_p)*cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - d_hip_fl_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_fl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) - (L_b*cos(beta_p)*sin(gamma_p))/2 - d_hip_fl_x*cos(beta_p)*sin(gamma_p) - (W_b*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, (L_b*cos(gamma_p))/2 + d_hip_fl_x*cos(gamma_p) - d_hip_fl_y*(cos(alpha_p)*cos(q1_fl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_fl)) + d_hip_fl_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_fl) + cos(q1_fl)*sin(alpha_p)*sin(gamma_p)) - (W_b*cos(alpha_p)*sin(gamma_p))/2, d_hip_fl_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_fl)) - d_hip_fl_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)*sin(beta_p)) + (L_b*sin(beta_p)*sin(gamma_p))/2 + d_hip_fl_x*sin(beta_p)*sin(gamma_p) + (W_b*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, d_hip_fl_y*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_hip_fl_z*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), - d_hip_fl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - d_hip_fl_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)), d_hip_fl_y*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_hip_fl_z*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, L_h_y*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + (W_b*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 + d_th_fl_y*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_th_fl_z*cos(q2_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + d_th_fl_x*sin(q2_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) - L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - d_th_fl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - (W_b*cos(gamma_p)*sin(alpha_p))/2 - d_th_fl_z*cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) + d_th_fl_x*sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)), L_h_y*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + (W_b*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_th_fl_y*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_th_fl_z*cos(q2_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_fl_x*sin(q2_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), d_th_fl_z*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fl)) - d_th_fl_x*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fl)*sin(beta_p)) + L_h_y*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_z*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + (W_b*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_th_fl_y*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - (L_b*cos(gamma_p)*sin(beta_p))/2 - L_h_x*cos(gamma_p)*sin(beta_p), 0, d_th_fl_x*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fl)) - d_th_fl_z*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fl)) - L_h_y*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_h_z*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - (W_b*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 - d_th_fl_y*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - (L_b*cos(beta_p)*cos(gamma_p))/2 - L_h_x*cos(beta_p)*cos(gamma_p), d_th_fl_z*(cos(q2_fl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fl) + cos(beta_p)*cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_fl)) - L_h_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fl) + cos(beta_p)*cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - d_th_fl_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_fl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) - d_th_fl_x*(sin(q2_fl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fl) + cos(beta_p)*cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) + cos(beta_p)*cos(q2_fl)*sin(gamma_p)) - L_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_fl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) - (L_b*cos(beta_p)*sin(gamma_p))/2 - L_h_x*cos(beta_p)*sin(gamma_p) - (W_b*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, (L_b*cos(gamma_p))/2 + L_h_x*cos(gamma_p) - d_th_fl_x*(sin(q2_fl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fl) + cos(q1_fl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_fl)) + d_th_fl_z*(cos(gamma_p)*sin(q2_fl) + cos(q2_fl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fl) + cos(q1_fl)*sin(alpha_p)*sin(gamma_p))) - L_h_y*(cos(alpha_p)*cos(q1_fl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_fl)) - L_h_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_fl) + cos(q1_fl)*sin(alpha_p)*sin(gamma_p)) - d_th_fl_y*(cos(alpha_p)*cos(q1_fl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_fl)) - (W_b*cos(alpha_p)*sin(gamma_p))/2, L_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_fl)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)*sin(beta_p)) + d_th_fl_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_fl)) + d_th_fl_x*(sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)*sin(beta_p)) + cos(q2_fl)*sin(beta_p)*sin(gamma_p)) - d_th_fl_z*(cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_fl)) + (L_b*sin(beta_p)*sin(gamma_p))/2 + L_h_x*sin(beta_p)*sin(gamma_p) + (W_b*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, L_h_y*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + d_th_fl_y*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_th_fl_z*cos(q2_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + d_th_fl_x*sin(q2_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) - L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - d_th_fl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - d_th_fl_z*cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) + d_th_fl_x*sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)), L_h_y*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_fl_y*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_th_fl_z*cos(q2_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_fl_x*sin(q2_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), - d_th_fl_x*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fl)) - d_th_fl_z*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fl)), d_th_fl_z*(cos(q2_fl)*sin(gamma_p) + sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p))) - d_th_fl_x*(sin(gamma_p)*sin(q2_fl) - cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p))), - d_th_fl_x*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fl)) - d_th_fl_z*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fl)*sin(beta_p)), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, d_sh_fl_x*(cos(q2_fl)*sin(q3_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_fl)*sin(q2_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - d_sh_fl_z*(cos(q2_fl)*cos(q3_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_fl)*sin(q3_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + L_h_y*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_th_f_y*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + (W_b*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 + d_sh_fl_y*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_f_z*cos(q2_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_th_f_x*sin(q2_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), d_sh_fl_x*(cos(q2_fl)*sin(q3_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) + cos(q3_fl)*sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl))) - d_sh_fl_z*(cos(q2_fl)*cos(q3_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) - sin(q2_fl)*sin(q3_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl))) - L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) - L_th_f_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - d_sh_fl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - (W_b*cos(gamma_p)*sin(alpha_p))/2 + L_th_f_z*cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) + L_th_f_x*sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)), d_sh_fl_x*(cos(q2_fl)*sin(q3_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_fl)*sin(q2_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - d_sh_fl_z*(cos(q2_fl)*cos(q3_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_fl)*sin(q3_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + L_h_y*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_f_y*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + (W_b*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_sh_fl_y*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_f_z*cos(q2_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_f_x*sin(q2_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), L_h_y*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_f_z*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fl)) - L_th_f_x*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fl)*sin(beta_p)) - L_h_z*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_f_y*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + (W_b*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 - d_sh_fl_x*(cos(q3_fl)*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fl)*sin(beta_p)) + sin(q3_fl)*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fl))) + d_sh_fl_z*(cos(q3_fl)*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fl)) - sin(q3_fl)*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fl)*sin(beta_p))) + d_sh_fl_y*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - (L_b*cos(gamma_p)*sin(beta_p))/2 - L_h_x*cos(gamma_p)*sin(beta_p), 0, L_th_f_x*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fl)) + L_th_f_z*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fl)) + d_sh_fl_x*(cos(q3_fl)*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fl)) + sin(q3_fl)*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fl))) - d_sh_fl_z*(cos(q3_fl)*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fl)) - sin(q3_fl)*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fl))) - L_h_y*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_h_z*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - L_th_f_y*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - (W_b*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 - d_sh_fl_y*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - (L_b*cos(beta_p)*cos(gamma_p))/2 - L_h_x*cos(beta_p)*cos(gamma_p), d_sh_fl_z*(cos(q3_fl)*(cos(q2_fl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fl) + cos(beta_p)*cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_fl)) - sin(q3_fl)*(sin(q2_fl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fl) + cos(beta_p)*cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) + cos(beta_p)*cos(q2_fl)*sin(gamma_p))) - L_h_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fl) + cos(beta_p)*cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - L_th_f_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_fl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) - d_sh_fl_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_fl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) - d_sh_fl_x*(cos(q3_fl)*(sin(q2_fl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fl) + cos(beta_p)*cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) + cos(beta_p)*cos(q2_fl)*sin(gamma_p)) + sin(q3_fl)*(cos(q2_fl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fl) + cos(beta_p)*cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_fl))) - L_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_fl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) - L_th_f_x*(sin(q2_fl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fl) + cos(beta_p)*cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) + cos(beta_p)*cos(q2_fl)*sin(gamma_p)) - L_th_f_z*(cos(q2_fl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_fl) + cos(beta_p)*cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_fl)) - (L_b*cos(beta_p)*sin(gamma_p))/2 - L_h_x*cos(beta_p)*sin(gamma_p) - (W_b*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, (L_b*cos(gamma_p))/2 - d_sh_fl_z*(sin(q3_fl)*(sin(q2_fl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fl) + cos(q1_fl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_fl)) - cos(q3_fl)*(cos(gamma_p)*sin(q2_fl) + cos(q2_fl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fl) + cos(q1_fl)*sin(alpha_p)*sin(gamma_p)))) - L_th_f_x*(sin(q2_fl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fl) + cos(q1_fl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_fl)) - L_th_f_z*(cos(gamma_p)*sin(q2_fl) + cos(q2_fl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fl) + cos(q1_fl)*sin(alpha_p)*sin(gamma_p))) - d_sh_fl_x*(sin(q3_fl)*(cos(gamma_p)*sin(q2_fl) + cos(q2_fl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fl) + cos(q1_fl)*sin(alpha_p)*sin(gamma_p))) + cos(q3_fl)*(sin(q2_fl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_fl) + cos(q1_fl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_fl))) + L_h_x*cos(gamma_p) - L_h_y*(cos(alpha_p)*cos(q1_fl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_fl)) - L_h_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_fl) + cos(q1_fl)*sin(alpha_p)*sin(gamma_p)) - L_th_f_y*(cos(alpha_p)*cos(q1_fl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_fl)) - d_sh_fl_y*(cos(alpha_p)*cos(q1_fl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_fl)) - (W_b*cos(alpha_p)*sin(gamma_p))/2, d_sh_fl_x*(cos(q3_fl)*(sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)*sin(beta_p)) + cos(q2_fl)*sin(beta_p)*sin(gamma_p)) + sin(q3_fl)*(cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_fl))) - d_sh_fl_z*(cos(q3_fl)*(cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_fl)) - sin(q3_fl)*(sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)*sin(beta_p)) + cos(q2_fl)*sin(beta_p)*sin(gamma_p))) + L_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_fl)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)*sin(beta_p)) + L_th_f_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_fl)) + d_sh_fl_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_fl)) + L_th_f_x*(sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)*sin(beta_p)) + cos(q2_fl)*sin(beta_p)*sin(gamma_p)) + L_th_f_z*(cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_fl)) + (L_b*sin(beta_p)*sin(gamma_p))/2 + L_h_x*sin(beta_p)*sin(gamma_p) + (W_b*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, d_sh_fl_x*(cos(q2_fl)*sin(q3_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_fl)*sin(q2_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - d_sh_fl_z*(cos(q2_fl)*cos(q3_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_fl)*sin(q3_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + L_h_y*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_th_f_y*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + d_sh_fl_y*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_f_z*cos(q2_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_th_f_x*sin(q2_fl)*(cos(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), d_sh_fl_x*(cos(q2_fl)*sin(q3_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) + cos(q3_fl)*sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl))) - d_sh_fl_z*(cos(q2_fl)*cos(q3_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) - sin(q2_fl)*sin(q3_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl))) - L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) - L_th_f_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - d_sh_fl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) + L_th_f_z*cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)) + L_th_f_x*sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_fl) - cos(gamma_p)*sin(alpha_p)*sin(q1_fl)), d_sh_fl_x*(cos(q2_fl)*sin(q3_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_fl)*sin(q2_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - d_sh_fl_z*(cos(q2_fl)*cos(q3_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_fl)*sin(q3_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + L_h_y*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_f_y*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_sh_fl_y*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_f_z*cos(q2_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_f_x*sin(q2_fl)*(cos(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), L_th_f_z*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fl)) - L_th_f_x*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fl)) - d_sh_fl_x*(cos(q3_fl)*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fl)) - sin(q3_fl)*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fl))) - d_sh_fl_z*(cos(q3_fl)*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fl)) + sin(q3_fl)*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fl))), d_sh_fl_z*(cos(q3_fl)*(cos(q2_fl)*sin(gamma_p) + sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p))) - sin(q3_fl)*(sin(gamma_p)*sin(q2_fl) - cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)))) - d_sh_fl_x*(cos(q3_fl)*(sin(gamma_p)*sin(q2_fl) - cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p))) + sin(q3_fl)*(cos(q2_fl)*sin(gamma_p) + sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)))) - L_th_f_x*(sin(gamma_p)*sin(q2_fl) - cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p))) - L_th_f_z*(cos(q2_fl)*sin(gamma_p) + sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p))), L_th_f_z*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fl)*sin(beta_p)) - L_th_f_x*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fl)) - d_sh_fl_x*(cos(q3_fl)*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fl)) - sin(q3_fl)*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fl)*sin(beta_p))) - d_sh_fl_z*(cos(q3_fl)*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fl)*sin(beta_p)) + sin(q3_fl)*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fl))), - d_sh_fl_x*(cos(q3_fl)*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fl)) - sin(q3_fl)*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fl))) - d_sh_fl_z*(cos(q3_fl)*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_fl)) + sin(q3_fl)*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_fl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_fl))), d_sh_fl_z*(cos(q3_fl)*(cos(q2_fl)*sin(gamma_p) + sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p))) - sin(q3_fl)*(sin(gamma_p)*sin(q2_fl) - cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)))) - d_sh_fl_x*(cos(q3_fl)*(sin(gamma_p)*sin(q2_fl) - cos(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p))) + sin(q3_fl)*(cos(q2_fl)*sin(gamma_p) + sin(q2_fl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)))), - d_sh_fl_x*(cos(q3_fl)*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fl)) - sin(q3_fl)*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fl)*sin(beta_p))) - d_sh_fl_z*(cos(q3_fl)*(sin(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_fl)*sin(beta_p)) + sin(q3_fl)*(cos(q2_fl)*(cos(q1_fl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_fl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_fl))), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, d_hip_hr_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - (W_b*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 - d_hip_hr_z*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), (W_b*cos(gamma_p)*sin(alpha_p))/2 - d_hip_hr_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - d_hip_hr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)), d_hip_hr_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - (W_b*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 - d_hip_hr_z*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), d_hip_hr_y*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - (W_b*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_hip_hr_z*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + (L_b*cos(gamma_p)*sin(beta_p))/2 - d_hip_hr_x*cos(gamma_p)*sin(beta_p), 0, (W_b*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 - d_hip_hr_y*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - d_hip_hr_z*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + (L_b*cos(beta_p)*cos(gamma_p))/2 - d_hip_hr_x*cos(beta_p)*cos(gamma_p), d_hip_hr_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - d_hip_hr_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + (L_b*cos(beta_p)*sin(gamma_p))/2 - d_hip_hr_x*cos(beta_p)*sin(gamma_p) + (W_b*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, d_hip_hr_x*cos(gamma_p) - (L_b*cos(gamma_p))/2 - d_hip_hr_y*(cos(alpha_p)*cos(q1_hr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hr)) + d_hip_hr_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) + (W_b*cos(alpha_p)*sin(gamma_p))/2, d_hip_hr_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hr)) - d_hip_hr_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - (L_b*sin(beta_p)*sin(gamma_p))/2 + d_hip_hr_x*sin(beta_p)*sin(gamma_p) - (W_b*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, d_hip_hr_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_hip_hr_z*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), - d_hip_hr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - d_hip_hr_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)), d_hip_hr_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_hip_hr_z*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, L_h_z*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_h_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - (W_b*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 + d_th_hr_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_th_hr_z*cos(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + d_th_hr_x*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - d_th_hr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + (W_b*cos(gamma_p)*sin(alpha_p))/2 - d_th_hr_z*cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + d_th_hr_x*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)), L_h_z*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - (W_b*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_th_hr_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_th_hr_z*cos(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_hr_x*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), d_th_hr_z*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - d_th_hr_x*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) - L_h_y*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_z*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - (W_b*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_th_hr_y*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + (L_b*cos(gamma_p)*sin(beta_p))/2 + L_h_x*cos(gamma_p)*sin(beta_p), 0, d_th_hr_x*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) - d_th_hr_z*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) + L_h_y*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_h_z*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + (W_b*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 - d_th_hr_y*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + (L_b*cos(beta_p)*cos(gamma_p))/2 + L_h_x*cos(beta_p)*cos(gamma_p), L_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - L_h_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - d_th_hr_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - d_th_hr_x*(sin(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + cos(beta_p)*cos(q2_hr)*sin(gamma_p)) + d_th_hr_z*(cos(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hr)) + (L_b*cos(beta_p)*sin(gamma_p))/2 + L_h_x*cos(beta_p)*sin(gamma_p) + (W_b*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, d_th_hr_z*(cos(gamma_p)*sin(q2_hr) + cos(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p))) - L_h_x*cos(gamma_p) - d_th_hr_x*(sin(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hr)) - (L_b*cos(gamma_p))/2 + L_h_y*(cos(alpha_p)*cos(q1_hr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hr)) - L_h_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) - d_th_hr_y*(cos(alpha_p)*cos(q1_hr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hr)) + (W_b*cos(alpha_p)*sin(gamma_p))/2, L_h_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - L_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hr)) + d_th_hr_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hr)) + d_th_hr_x*(sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) + cos(q2_hr)*sin(beta_p)*sin(gamma_p)) - d_th_hr_z*(cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hr)) - (L_b*sin(beta_p)*sin(gamma_p))/2 - L_h_x*sin(beta_p)*sin(gamma_p) - (W_b*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, L_h_z*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_h_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + d_th_hr_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_th_hr_z*cos(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + d_th_hr_x*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - d_th_hr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - d_th_hr_z*cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + d_th_hr_x*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)), L_h_z*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_hr_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_th_hr_z*cos(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_hr_x*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), - d_th_hr_x*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - d_th_hr_z*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)), d_th_hr_z*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) - d_th_hr_x*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))), - d_th_hr_x*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - d_th_hr_z*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, d_th_mid_hr_x*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - d_th_mid_hr_z*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - L_h_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_h_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - (W_b*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 + d_th_mid_hr_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), d_th_mid_hr_x*(cos(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + cos(q3_hr)*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr))) - d_th_mid_hr_z*(cos(q2_hr)*cos(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - sin(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr))) + L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + L_th_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - d_th_mid_hr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + (W_b*cos(gamma_p)*sin(alpha_p))/2 + L_th_h_z*cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - L_th_h_x*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)), d_th_mid_hr_x*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - d_th_mid_hr_z*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - L_h_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - (W_b*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_th_mid_hr_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), L_th_h_x*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) - L_th_h_z*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - L_h_y*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_z*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_y*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - (W_b*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 - d_th_mid_hr_x*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr))) + d_th_mid_hr_z*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p))) + d_th_mid_hr_y*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + (L_b*cos(gamma_p)*sin(beta_p))/2 + L_h_x*cos(gamma_p)*sin(beta_p), 0, L_th_h_z*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - L_th_h_x*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + d_th_mid_hr_x*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr))) - d_th_mid_hr_z*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr))) + L_h_y*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_h_z*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_h_y*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + (W_b*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 - d_th_mid_hr_y*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + (L_b*cos(beta_p)*cos(gamma_p))/2 + L_h_x*cos(beta_p)*cos(gamma_p), L_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - L_h_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + L_th_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - d_th_mid_hr_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - d_th_mid_hr_x*(cos(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + cos(beta_p)*cos(q2_hr)*sin(gamma_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hr))) + d_th_mid_hr_z*(cos(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + cos(beta_p)*cos(q2_hr)*sin(gamma_p))) + L_th_h_x*(sin(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + cos(beta_p)*cos(q2_hr)*sin(gamma_p)) - L_th_h_z*(cos(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hr)) + (L_b*cos(beta_p)*sin(gamma_p))/2 + L_h_x*cos(beta_p)*sin(gamma_p) + (W_b*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, L_th_h_x*(sin(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hr)) - d_th_mid_hr_z*(sin(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hr)) - cos(q3_hr)*(cos(gamma_p)*sin(q2_hr) + cos(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)))) - d_th_mid_hr_x*(sin(q3_hr)*(cos(gamma_p)*sin(q2_hr) + cos(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hr))) - L_th_h_z*(cos(gamma_p)*sin(q2_hr) + cos(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p))) - (L_b*cos(gamma_p))/2 - L_h_x*cos(gamma_p) + L_h_y*(cos(alpha_p)*cos(q1_hr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hr)) - L_h_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) + L_th_h_y*(cos(alpha_p)*cos(q1_hr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hr)) - d_th_mid_hr_y*(cos(alpha_p)*cos(q1_hr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hr)) + (W_b*cos(alpha_p)*sin(gamma_p))/2, d_th_mid_hr_x*(cos(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) + cos(q2_hr)*sin(beta_p)*sin(gamma_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hr))) - d_th_mid_hr_z*(cos(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) + cos(q2_hr)*sin(beta_p)*sin(gamma_p))) - L_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hr)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - L_th_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hr)) + d_th_mid_hr_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hr)) - L_th_h_x*(sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) + cos(q2_hr)*sin(beta_p)*sin(gamma_p)) + L_th_h_z*(cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hr)) - (L_b*sin(beta_p)*sin(gamma_p))/2 - L_h_x*sin(beta_p)*sin(gamma_p) - (W_b*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, d_th_mid_hr_x*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - d_th_mid_hr_z*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - L_h_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_h_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + d_th_mid_hr_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), d_th_mid_hr_x*(cos(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + cos(q3_hr)*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr))) - d_th_mid_hr_z*(cos(q2_hr)*cos(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - sin(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr))) + L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + L_th_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - d_th_mid_hr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + L_th_h_z*cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - L_th_h_x*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)), d_th_mid_hr_x*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - d_th_mid_hr_z*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - L_h_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_mid_hr_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), L_th_h_x*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) + L_th_h_z*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) - d_th_mid_hr_x*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr))) - d_th_mid_hr_z*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr))), d_th_mid_hr_z*(cos(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) - sin(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))) - d_th_mid_hr_x*(cos(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) + sin(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))) + L_th_h_x*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) - L_th_h_z*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))), L_th_h_x*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) + L_th_h_z*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) - d_th_mid_hr_x*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p))) - d_th_mid_hr_z*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr))), - d_th_mid_hr_x*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr))) - d_th_mid_hr_z*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr))), d_th_mid_hr_z*(cos(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) - sin(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))) - d_th_mid_hr_x*(cos(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) + sin(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))), - d_th_mid_hr_x*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p))) - d_th_mid_hr_z*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr))), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, d_sh_hr_x*(cos(q4_hr)*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + sin(q4_hr)*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))))) - d_sh_hr_z*(cos(q4_hr)*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - sin(q4_hr)*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))))) - L_th_h_mid_x*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + L_th_h_mid_z*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - L_h_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_h_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - L_th_h_mid_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - (W_b*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 + d_sh_hr_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), L_th_h_mid_z*(cos(q2_hr)*cos(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - sin(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr))) - L_th_h_mid_x*(cos(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + cos(q3_hr)*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr))) + L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + L_th_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + L_th_h_mid_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - d_sh_hr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + d_sh_hr_x*(cos(q4_hr)*(cos(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + cos(q3_hr)*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr))) + sin(q4_hr)*(cos(q2_hr)*cos(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - sin(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)))) - d_sh_hr_z*(cos(q4_hr)*(cos(q2_hr)*cos(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - sin(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr))) - sin(q4_hr)*(cos(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + cos(q3_hr)*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)))) + (W_b*cos(gamma_p)*sin(alpha_p))/2 + L_th_h_z*cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - L_th_h_x*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)), d_sh_hr_x*(cos(q4_hr)*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + sin(q4_hr)*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))))) - d_sh_hr_z*(cos(q4_hr)*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - sin(q4_hr)*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))))) - L_th_h_mid_x*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + L_th_h_mid_z*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - L_h_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_mid_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - (W_b*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_sh_hr_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), L_th_h_x*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) - L_th_h_z*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) + L_th_h_mid_x*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr))) - L_th_h_mid_z*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p))) - L_h_y*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_z*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_y*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_mid_y*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - (W_b*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_sh_hr_y*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_sh_hr_x*(cos(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr))) + sin(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)))) + d_sh_hr_z*(cos(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p))) - sin(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)))) + (L_b*cos(gamma_p)*sin(beta_p))/2 + L_h_x*cos(gamma_p)*sin(beta_p), 0, L_th_h_z*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - L_th_h_x*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + d_sh_hr_x*(cos(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr))) + sin(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)))) - d_sh_hr_z*(cos(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr))) - sin(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)))) - L_th_h_mid_x*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr))) + L_th_h_mid_z*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr))) + L_h_y*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_h_z*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_h_y*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_th_h_mid_y*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + (W_b*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 - d_sh_hr_y*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + (L_b*cos(beta_p)*cos(gamma_p))/2 + L_h_x*cos(beta_p)*cos(gamma_p), L_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - L_h_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + L_th_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + L_th_h_mid_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - d_sh_hr_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hr) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - d_sh_hr_x*(cos(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + cos(beta_p)*cos(q2_hr)*sin(gamma_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hr))) + sin(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + cos(beta_p)*cos(q2_hr)*sin(gamma_p)))) + d_sh_hr_z*(cos(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + cos(beta_p)*cos(q2_hr)*sin(gamma_p))) - sin(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + cos(beta_p)*cos(q2_hr)*sin(gamma_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hr)))) + L_th_h_mid_x*(cos(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + cos(beta_p)*cos(q2_hr)*sin(gamma_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hr))) - L_th_h_mid_z*(cos(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + cos(beta_p)*cos(q2_hr)*sin(gamma_p))) + L_th_h_x*(sin(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + cos(beta_p)*cos(q2_hr)*sin(gamma_p)) - L_th_h_z*(cos(q2_hr)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hr) + cos(beta_p)*cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hr)) + (L_b*cos(beta_p)*sin(gamma_p))/2 + L_h_x*cos(beta_p)*sin(gamma_p) + (W_b*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, L_th_h_mid_x*(sin(q3_hr)*(cos(gamma_p)*sin(q2_hr) + cos(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hr))) + L_th_h_mid_z*(sin(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hr)) - cos(q3_hr)*(cos(gamma_p)*sin(q2_hr) + cos(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)))) - d_sh_hr_x*(cos(q4_hr)*(sin(q3_hr)*(cos(gamma_p)*sin(q2_hr) + cos(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hr))) - sin(q4_hr)*(sin(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hr)) - cos(q3_hr)*(cos(gamma_p)*sin(q2_hr) + cos(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p))))) - d_sh_hr_z*(cos(q4_hr)*(sin(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hr)) - cos(q3_hr)*(cos(gamma_p)*sin(q2_hr) + cos(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)))) + sin(q4_hr)*(sin(q3_hr)*(cos(gamma_p)*sin(q2_hr) + cos(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hr)))) + L_th_h_x*(sin(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hr)) - L_th_h_z*(cos(gamma_p)*sin(q2_hr) + cos(q2_hr)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p))) - (L_b*cos(gamma_p))/2 - L_h_x*cos(gamma_p) + L_h_y*(cos(alpha_p)*cos(q1_hr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hr)) - L_h_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_hr) + cos(q1_hr)*sin(alpha_p)*sin(gamma_p)) + L_th_h_y*(cos(alpha_p)*cos(q1_hr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hr)) + L_th_h_mid_y*(cos(alpha_p)*cos(q1_hr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hr)) - d_sh_hr_y*(cos(alpha_p)*cos(q1_hr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hr)) + (W_b*cos(alpha_p)*sin(gamma_p))/2, d_sh_hr_x*(cos(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) + cos(q2_hr)*sin(beta_p)*sin(gamma_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hr))) + sin(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) + cos(q2_hr)*sin(beta_p)*sin(gamma_p)))) - d_sh_hr_z*(cos(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) + cos(q2_hr)*sin(beta_p)*sin(gamma_p))) - sin(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) + cos(q2_hr)*sin(beta_p)*sin(gamma_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hr)))) - L_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hr)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - L_th_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hr)) - L_th_h_mid_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hr)) + d_sh_hr_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hr)) - L_th_h_x*(sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) + cos(q2_hr)*sin(beta_p)*sin(gamma_p)) + L_th_h_z*(cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hr)) - L_th_h_mid_x*(cos(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) + cos(q2_hr)*sin(beta_p)*sin(gamma_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hr))) + L_th_h_mid_z*(cos(q3_hr)*(cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)*sin(beta_p)) + cos(q2_hr)*sin(beta_p)*sin(gamma_p))) - (L_b*sin(beta_p)*sin(gamma_p))/2 - L_h_x*sin(beta_p)*sin(gamma_p) - (W_b*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, d_sh_hr_x*(cos(q4_hr)*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + sin(q4_hr)*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))))) - d_sh_hr_z*(cos(q4_hr)*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - sin(q4_hr)*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))))) - L_th_h_mid_x*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + L_th_h_mid_z*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - L_h_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_h_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - L_th_h_mid_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + d_sh_hr_y*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hr)*(cos(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), L_th_h_mid_z*(cos(q2_hr)*cos(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - sin(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr))) - L_th_h_mid_x*(cos(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + cos(q3_hr)*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr))) + L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + L_th_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + L_th_h_mid_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) - d_sh_hr_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + d_sh_hr_x*(cos(q4_hr)*(cos(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + cos(q3_hr)*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr))) + sin(q4_hr)*(cos(q2_hr)*cos(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - sin(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)))) - d_sh_hr_z*(cos(q4_hr)*(cos(q2_hr)*cos(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - sin(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr))) - sin(q4_hr)*(cos(q2_hr)*sin(q3_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) + cos(q3_hr)*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)))) + L_th_h_z*cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)) - L_th_h_x*sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hr) - cos(gamma_p)*sin(alpha_p)*sin(q1_hr)), d_sh_hr_x*(cos(q4_hr)*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + sin(q4_hr)*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))))) - d_sh_hr_z*(cos(q4_hr)*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - sin(q4_hr)*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))))) - L_th_h_mid_x*(cos(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hr)*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + L_th_h_mid_z*(cos(q2_hr)*cos(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hr)*sin(q3_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - L_h_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_mid_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_sh_hr_y*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hr)*(cos(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), L_th_h_x*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) + L_th_h_z*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) - d_sh_hr_x*(cos(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr))) - sin(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)))) - d_sh_hr_z*(cos(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr))) + sin(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)))) + L_th_h_mid_x*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr))) + L_th_h_mid_z*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr))), d_sh_hr_z*(cos(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) - sin(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))) - sin(q4_hr)*(cos(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) + sin(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))))) - d_sh_hr_x*(cos(q4_hr)*(cos(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) + sin(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))) + sin(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) - sin(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))))) + L_th_h_x*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) - L_th_h_z*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) + L_th_h_mid_x*(cos(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) + sin(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))) - L_th_h_mid_z*(cos(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) - sin(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))), L_th_h_x*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) + L_th_h_z*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + L_th_h_mid_x*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p))) + L_th_h_mid_z*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr))) - d_sh_hr_x*(cos(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p))) - sin(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)))) - d_sh_hr_z*(cos(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr))) + sin(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)))), L_th_h_mid_x*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr))) - d_sh_hr_z*(cos(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr))) + sin(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)))) - d_sh_hr_x*(cos(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr))) - sin(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)))) + L_th_h_mid_z*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr))), d_sh_hr_z*(cos(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) - sin(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))) - sin(q4_hr)*(cos(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) + sin(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))))) - d_sh_hr_x*(cos(q4_hr)*(cos(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) + sin(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))) + sin(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) - sin(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))))) + L_th_h_mid_x*(cos(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) + sin(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))) - L_th_h_mid_z*(cos(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) - sin(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))), L_th_h_mid_x*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p))) + L_th_h_mid_z*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr))) - d_sh_hr_x*(cos(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p))) - sin(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)))) - d_sh_hr_z*(cos(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr))) + sin(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)))), - d_sh_hr_x*(cos(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr))) - sin(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)))) - d_sh_hr_z*(cos(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr))) + sin(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hr)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hr)))), d_sh_hr_z*(cos(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) - sin(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))) - sin(q4_hr)*(cos(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) + sin(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))))) - d_sh_hr_x*(cos(q4_hr)*(cos(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) + sin(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)))) + sin(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*sin(gamma_p) + sin(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))) - sin(q3_hr)*(sin(gamma_p)*sin(q2_hr) - cos(q2_hr)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p))))), - d_sh_hr_x*(cos(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p))) - sin(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)))) - d_sh_hr_z*(cos(q4_hr)*(cos(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)) + sin(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr))) + sin(q4_hr)*(cos(q3_hr)*(cos(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hr)) - sin(q3_hr)*(sin(q2_hr)*(cos(q1_hr)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hr)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hr)*sin(beta_p)))), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, (W_b*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 + d_hip_hl_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_hip_hl_z*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), - d_hip_hl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - d_hip_hl_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - (W_b*cos(gamma_p)*sin(alpha_p))/2, (W_b*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_hip_hl_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_hip_hl_z*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), (W_b*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_hip_hl_y*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_hip_hl_z*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + (L_b*cos(gamma_p)*sin(beta_p))/2 - d_hip_hl_x*cos(gamma_p)*sin(beta_p), 0, (L_b*cos(beta_p)*cos(gamma_p))/2 - d_hip_hl_y*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - d_hip_hl_z*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - (W_b*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 - d_hip_hl_x*cos(beta_p)*cos(gamma_p), d_hip_hl_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - d_hip_hl_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + (L_b*cos(beta_p)*sin(gamma_p))/2 - d_hip_hl_x*cos(beta_p)*sin(gamma_p) - (W_b*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, d_hip_hl_x*cos(gamma_p) - (L_b*cos(gamma_p))/2 - d_hip_hl_y*(cos(alpha_p)*cos(q1_hl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hl)) + d_hip_hl_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - (W_b*cos(alpha_p)*sin(gamma_p))/2, d_hip_hl_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hl)) - d_hip_hl_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) - (L_b*sin(beta_p)*sin(gamma_p))/2 + d_hip_hl_x*sin(beta_p)*sin(gamma_p) + (W_b*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, d_hip_hl_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_hip_hl_z*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), - d_hip_hl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - d_hip_hl_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)), d_hip_hl_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_hip_hl_z*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, L_h_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + (W_b*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 + d_th_hl_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_th_hl_z*cos(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + d_th_hl_x*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - d_th_hl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - (W_b*cos(gamma_p)*sin(alpha_p))/2 - d_th_hl_z*cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + d_th_hl_x*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)), L_h_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + (W_b*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_th_hl_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_th_hl_z*cos(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_hl_x*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), d_th_hl_z*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - d_th_hl_x*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + L_h_y*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_z*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + (W_b*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_th_hl_y*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + (L_b*cos(gamma_p)*sin(beta_p))/2 + L_h_x*cos(gamma_p)*sin(beta_p), 0, d_th_hl_x*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) - d_th_hl_z*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - L_h_y*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - (W_b*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 - d_th_hl_y*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + (L_b*cos(beta_p)*cos(gamma_p))/2 + L_h_x*cos(beta_p)*cos(gamma_p), d_th_hl_z*(cos(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hl)) - L_h_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - d_th_hl_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - d_th_hl_x*(sin(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + cos(beta_p)*cos(q2_hl)*sin(gamma_p)) - L_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + (L_b*cos(beta_p)*sin(gamma_p))/2 + L_h_x*cos(beta_p)*sin(gamma_p) - (W_b*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, d_th_hl_z*(cos(gamma_p)*sin(q2_hl) + cos(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p))) - L_h_x*cos(gamma_p) - d_th_hl_x*(sin(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hl)) - (L_b*cos(gamma_p))/2 - L_h_y*(cos(alpha_p)*cos(q1_hl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hl)) - L_h_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - d_th_hl_y*(cos(alpha_p)*cos(q1_hl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hl)) - (W_b*cos(alpha_p)*sin(gamma_p))/2, L_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hl)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + d_th_hl_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hl)) + d_th_hl_x*(sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + cos(q2_hl)*sin(beta_p)*sin(gamma_p)) - d_th_hl_z*(cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hl)) - (L_b*sin(beta_p)*sin(gamma_p))/2 - L_h_x*sin(beta_p)*sin(gamma_p) + (W_b*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, L_h_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + d_th_hl_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - d_th_hl_z*cos(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + d_th_hl_x*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - d_th_hl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - d_th_hl_z*cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + d_th_hl_x*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)), L_h_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_hl_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_th_hl_z*cos(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_hl_x*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), - d_th_hl_x*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - d_th_hl_z*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)), d_th_hl_z*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) - d_th_hl_x*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))), - d_th_hl_x*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - d_th_hl_z*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)), 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, d_th_mid_hl_x*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - d_th_mid_hl_z*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + L_h_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_th_h_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + (W_b*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 + d_th_mid_hl_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), d_th_mid_hl_x*(cos(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + cos(q3_hl)*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl))) - d_th_mid_hl_z*(cos(q2_hl)*cos(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - sin(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl))) - L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - L_th_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - d_th_mid_hl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - (W_b*cos(gamma_p)*sin(alpha_p))/2 + L_th_h_z*cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - L_th_h_x*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)), d_th_mid_hl_x*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - d_th_mid_hl_z*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + L_h_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + (W_b*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_th_mid_hl_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), L_th_h_x*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) - L_th_h_z*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) + L_h_y*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_z*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_y*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + (W_b*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 - d_th_mid_hl_x*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl))) + d_th_mid_hl_z*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p))) + d_th_mid_hl_y*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + (L_b*cos(gamma_p)*sin(beta_p))/2 + L_h_x*cos(gamma_p)*sin(beta_p), 0, L_th_h_z*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - L_th_h_x*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + d_th_mid_hl_x*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl))) - d_th_mid_hl_z*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl))) - L_h_y*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - L_th_h_y*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - (W_b*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 - d_th_mid_hl_y*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + (L_b*cos(beta_p)*cos(gamma_p))/2 + L_h_x*cos(beta_p)*cos(gamma_p), d_th_mid_hl_z*(cos(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + cos(beta_p)*cos(q2_hl)*sin(gamma_p))) - L_h_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - L_th_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - d_th_mid_hl_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - d_th_mid_hl_x*(cos(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + cos(beta_p)*cos(q2_hl)*sin(gamma_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hl))) - L_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + L_th_h_x*(sin(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + cos(beta_p)*cos(q2_hl)*sin(gamma_p)) - L_th_h_z*(cos(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hl)) + (L_b*cos(beta_p)*sin(gamma_p))/2 + L_h_x*cos(beta_p)*sin(gamma_p) - (W_b*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, L_th_h_x*(sin(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hl)) - d_th_mid_hl_z*(sin(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hl)) - cos(q3_hl)*(cos(gamma_p)*sin(q2_hl) + cos(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)))) - d_th_mid_hl_x*(sin(q3_hl)*(cos(gamma_p)*sin(q2_hl) + cos(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hl))) - L_th_h_z*(cos(gamma_p)*sin(q2_hl) + cos(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p))) - (L_b*cos(gamma_p))/2 - L_h_x*cos(gamma_p) - L_h_y*(cos(alpha_p)*cos(q1_hl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hl)) - L_h_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - L_th_h_y*(cos(alpha_p)*cos(q1_hl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hl)) - d_th_mid_hl_y*(cos(alpha_p)*cos(q1_hl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hl)) - (W_b*cos(alpha_p)*sin(gamma_p))/2, d_th_mid_hl_x*(cos(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + cos(q2_hl)*sin(beta_p)*sin(gamma_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hl))) - d_th_mid_hl_z*(cos(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + cos(q2_hl)*sin(beta_p)*sin(gamma_p))) + L_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hl)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + L_th_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hl)) + d_th_mid_hl_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hl)) - L_th_h_x*(sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + cos(q2_hl)*sin(beta_p)*sin(gamma_p)) + L_th_h_z*(cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hl)) - (L_b*sin(beta_p)*sin(gamma_p))/2 - L_h_x*sin(beta_p)*sin(gamma_p) + (W_b*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, d_th_mid_hl_x*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - d_th_mid_hl_z*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + L_h_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_th_h_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + d_th_mid_hl_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), d_th_mid_hl_x*(cos(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + cos(q3_hl)*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl))) - d_th_mid_hl_z*(cos(q2_hl)*cos(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - sin(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl))) - L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - L_th_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - d_th_mid_hl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + L_th_h_z*cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - L_th_h_x*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)), d_th_mid_hl_x*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - d_th_mid_hl_z*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + L_h_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_th_mid_hl_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), L_th_h_x*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) + L_th_h_z*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) - d_th_mid_hl_x*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl))) - d_th_mid_hl_z*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl))), d_th_mid_hl_z*(cos(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) - sin(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))) - d_th_mid_hl_x*(cos(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) + sin(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))) + L_th_h_x*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) - L_th_h_z*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))), L_th_h_x*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) + L_th_h_z*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) - d_th_mid_hl_x*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p))) - d_th_mid_hl_z*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl))), - d_th_mid_hl_x*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl))) - d_th_mid_hl_z*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl))), d_th_mid_hl_z*(cos(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) - sin(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))) - d_th_mid_hl_x*(cos(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) + sin(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))), - d_th_mid_hl_x*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p))) - d_th_mid_hl_z*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl))), 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, d_sh_hl_x*(cos(q4_hl)*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + sin(q4_hl)*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))))) - d_sh_hl_z*(cos(q4_hl)*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - sin(q4_hl)*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))))) - L_th_h_mid_x*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + L_th_h_mid_z*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + L_h_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_th_h_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_h_mid_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + (W_b*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 + d_sh_hl_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), L_th_h_mid_z*(cos(q2_hl)*cos(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - sin(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl))) - L_th_h_mid_x*(cos(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + cos(q3_hl)*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl))) - L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - L_th_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - L_th_h_mid_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - d_sh_hl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + d_sh_hl_x*(cos(q4_hl)*(cos(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + cos(q3_hl)*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl))) + sin(q4_hl)*(cos(q2_hl)*cos(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - sin(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)))) - d_sh_hl_z*(cos(q4_hl)*(cos(q2_hl)*cos(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - sin(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl))) - sin(q4_hl)*(cos(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + cos(q3_hl)*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)))) - (W_b*cos(gamma_p)*sin(alpha_p))/2 + L_th_h_z*cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - L_th_h_x*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)), d_sh_hl_x*(cos(q4_hl)*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + sin(q4_hl)*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))))) - d_sh_hl_z*(cos(q4_hl)*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - sin(q4_hl)*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))))) - L_th_h_mid_x*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + L_th_h_mid_z*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + L_h_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_mid_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + (W_b*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_sh_hl_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), L_th_h_x*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) - L_th_h_z*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) + L_th_h_mid_x*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl))) - L_th_h_mid_z*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p))) + L_h_y*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_h_z*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_y*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_mid_y*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + (W_b*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + d_sh_hl_y*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - d_sh_hl_x*(cos(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl))) + sin(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)))) + d_sh_hl_z*(cos(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p))) - sin(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)))) + (L_b*cos(gamma_p)*sin(beta_p))/2 + L_h_x*cos(gamma_p)*sin(beta_p), 0, L_th_h_z*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - L_th_h_x*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + d_sh_hl_x*(cos(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl))) + sin(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)))) - d_sh_hl_z*(cos(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl))) - sin(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)))) - L_th_h_mid_x*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl))) + L_th_h_mid_z*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl))) - L_h_y*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - L_th_h_y*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_h_mid_y*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - (W_b*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 - d_sh_hl_y*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + (L_b*cos(beta_p)*cos(gamma_p))/2 + L_h_x*cos(beta_p)*cos(gamma_p), d_sh_hl_z*(cos(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + cos(beta_p)*cos(q2_hl)*sin(gamma_p))) - sin(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + cos(beta_p)*cos(q2_hl)*sin(gamma_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hl)))) - L_h_z*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - L_th_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - L_th_h_mid_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - d_sh_hl_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - d_sh_hl_x*(cos(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + cos(beta_p)*cos(q2_hl)*sin(gamma_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hl))) + sin(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + cos(beta_p)*cos(q2_hl)*sin(gamma_p)))) - L_h_y*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(q1_hl) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + L_th_h_mid_x*(cos(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + cos(beta_p)*cos(q2_hl)*sin(gamma_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hl))) - L_th_h_mid_z*(cos(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + cos(beta_p)*cos(q2_hl)*sin(gamma_p))) + L_th_h_x*(sin(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + cos(beta_p)*cos(q2_hl)*sin(gamma_p)) - L_th_h_z*(cos(q2_hl)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(q1_hl) + cos(beta_p)*cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - cos(beta_p)*sin(gamma_p)*sin(q2_hl)) + (L_b*cos(beta_p)*sin(gamma_p))/2 + L_h_x*cos(beta_p)*sin(gamma_p) - (W_b*cos(alpha_p)*cos(beta_p)*cos(gamma_p))/2, L_th_h_mid_x*(sin(q3_hl)*(cos(gamma_p)*sin(q2_hl) + cos(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hl))) + L_th_h_mid_z*(sin(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hl)) - cos(q3_hl)*(cos(gamma_p)*sin(q2_hl) + cos(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)))) - d_sh_hl_x*(cos(q4_hl)*(sin(q3_hl)*(cos(gamma_p)*sin(q2_hl) + cos(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hl))) - sin(q4_hl)*(sin(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hl)) - cos(q3_hl)*(cos(gamma_p)*sin(q2_hl) + cos(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p))))) - d_sh_hl_z*(cos(q4_hl)*(sin(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hl)) - cos(q3_hl)*(cos(gamma_p)*sin(q2_hl) + cos(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)))) + sin(q4_hl)*(sin(q3_hl)*(cos(gamma_p)*sin(q2_hl) + cos(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hl)))) + L_th_h_x*(sin(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - cos(gamma_p)*cos(q2_hl)) - L_th_h_z*(cos(gamma_p)*sin(q2_hl) + cos(q2_hl)*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p))) - (L_b*cos(gamma_p))/2 - L_h_x*cos(gamma_p) - L_h_y*(cos(alpha_p)*cos(q1_hl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hl)) - L_h_z*(cos(alpha_p)*sin(gamma_p)*sin(q1_hl) + cos(q1_hl)*sin(alpha_p)*sin(gamma_p)) - L_th_h_y*(cos(alpha_p)*cos(q1_hl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hl)) - L_th_h_mid_y*(cos(alpha_p)*cos(q1_hl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hl)) - d_sh_hl_y*(cos(alpha_p)*cos(q1_hl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hl)) - (W_b*cos(alpha_p)*sin(gamma_p))/2, d_sh_hl_x*(cos(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + cos(q2_hl)*sin(beta_p)*sin(gamma_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hl))) + sin(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + cos(q2_hl)*sin(beta_p)*sin(gamma_p)))) - d_sh_hl_z*(cos(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + cos(q2_hl)*sin(beta_p)*sin(gamma_p))) - sin(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + cos(q2_hl)*sin(beta_p)*sin(gamma_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hl)))) + L_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hl)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + L_th_h_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hl)) + L_th_h_mid_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hl)) + d_sh_hl_y*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(q1_hl)) - L_th_h_x*(sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + cos(q2_hl)*sin(beta_p)*sin(gamma_p)) + L_th_h_z*(cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hl)) - L_th_h_mid_x*(cos(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + cos(q2_hl)*sin(beta_p)*sin(gamma_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hl))) + L_th_h_mid_z*(cos(q3_hl)*(cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) - sin(beta_p)*sin(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)*sin(beta_p)) + cos(q2_hl)*sin(beta_p)*sin(gamma_p))) - (L_b*sin(beta_p)*sin(gamma_p))/2 - L_h_x*sin(beta_p)*sin(gamma_p) + (W_b*cos(alpha_p)*cos(gamma_p)*sin(beta_p))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, d_sh_hl_x*(cos(q4_hl)*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + sin(q4_hl)*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))))) - d_sh_hl_z*(cos(q4_hl)*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) - sin(q4_hl)*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))))) - L_th_h_mid_x*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + L_th_h_mid_z*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))) + L_h_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + L_th_h_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_h_mid_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + d_sh_hl_y*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hl)*(cos(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))), L_th_h_mid_z*(cos(q2_hl)*cos(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - sin(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl))) - L_th_h_mid_x*(cos(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + cos(q3_hl)*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl))) - L_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + L_h_z*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - L_th_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - L_th_h_mid_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) - d_sh_hl_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)) + d_sh_hl_x*(cos(q4_hl)*(cos(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + cos(q3_hl)*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl))) + sin(q4_hl)*(cos(q2_hl)*cos(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - sin(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)))) - d_sh_hl_z*(cos(q4_hl)*(cos(q2_hl)*cos(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - sin(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl))) - sin(q4_hl)*(cos(q2_hl)*sin(q3_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) + cos(q3_hl)*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)))) + L_th_h_z*cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)) - L_th_h_x*sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*cos(q1_hl) - cos(gamma_p)*sin(alpha_p)*sin(q1_hl)), d_sh_hl_x*(cos(q4_hl)*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + sin(q4_hl)*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))))) - d_sh_hl_z*(cos(q4_hl)*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) - sin(q4_hl)*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))))) - L_th_h_mid_x*(cos(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(q3_hl)*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + L_th_h_mid_z*(cos(q2_hl)*cos(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - sin(q2_hl)*sin(q3_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))) + L_h_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_h_z*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_mid_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + d_sh_hl_y*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + L_th_h_z*cos(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - L_th_h_x*sin(q2_hl)*(cos(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))), L_th_h_x*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) + L_th_h_z*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) - d_sh_hl_x*(cos(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl))) - sin(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)))) - d_sh_hl_z*(cos(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl))) + sin(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)))) + L_th_h_mid_x*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl))) + L_th_h_mid_z*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl))), d_sh_hl_z*(cos(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) - sin(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))) - sin(q4_hl)*(cos(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) + sin(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))))) - d_sh_hl_x*(cos(q4_hl)*(cos(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) + sin(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))) + sin(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) - sin(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))))) + L_th_h_x*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) - L_th_h_z*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) + L_th_h_mid_x*(cos(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) + sin(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))) - L_th_h_mid_z*(cos(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) - sin(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))), L_th_h_x*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) + L_th_h_z*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + L_th_h_mid_x*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p))) + L_th_h_mid_z*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl))) - d_sh_hl_x*(cos(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p))) - sin(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)))) - d_sh_hl_z*(cos(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl))) + sin(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)))), L_th_h_mid_x*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl))) - d_sh_hl_z*(cos(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl))) + sin(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)))) - d_sh_hl_x*(cos(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl))) - sin(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)))) + L_th_h_mid_z*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl))), d_sh_hl_z*(cos(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) - sin(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))) - sin(q4_hl)*(cos(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) + sin(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))))) - d_sh_hl_x*(cos(q4_hl)*(cos(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) + sin(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))) + sin(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) - sin(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))))) + L_th_h_mid_x*(cos(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) + sin(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))) - L_th_h_mid_z*(cos(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) - sin(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))), L_th_h_mid_x*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p))) + L_th_h_mid_z*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl))) - d_sh_hl_x*(cos(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p))) - sin(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)))) - d_sh_hl_z*(cos(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl))) + sin(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)))), - d_sh_hl_x*(cos(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl))) - sin(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)))) - d_sh_hl_z*(cos(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl))) + sin(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(q1_hl)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*cos(q2_hl)))), d_sh_hl_z*(cos(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) - sin(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))) - sin(q4_hl)*(cos(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) + sin(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))))) - d_sh_hl_x*(cos(q4_hl)*(cos(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) + sin(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p)))) + sin(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*sin(gamma_p) + sin(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))) - sin(q3_hl)*(sin(gamma_p)*sin(q2_hl) - cos(q2_hl)*(cos(alpha_p)*cos(gamma_p)*sin(q1_hl) + cos(gamma_p)*cos(q1_hl)*sin(alpha_p))))), - d_sh_hl_x*(cos(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p))) - sin(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)))) - d_sh_hl_z*(cos(q4_hl)*(cos(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p)) + sin(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl))) + sin(q4_hl)*(cos(q3_hl)*(cos(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*sin(beta_p)*sin(q2_hl)) - sin(q3_hl)*(sin(q2_hl)*(cos(q1_hl)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(q1_hl)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*cos(q2_hl)*sin(beta_p))))];