JJ_e_hr.m

 
J_e_hr =[1, 0, 0, 0, 1, 0, 0, 0, 1, L_sh_h_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_sh_h_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))))) - 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_sh_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_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 + 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_sh_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + 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)) - L_sh_h_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)))) + L_sh_h_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)), L_sh_h_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_sh_h_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))))) - 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_sh_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_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 + 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_sh_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_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))) + L_sh_h_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)))) - L_sh_h_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)))) - (W_b*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + (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)) - L_sh_h_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)))) + L_sh_h_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_sh_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_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 + (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_sh_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_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)) + L_sh_h_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)))) - L_sh_h_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)))) + L_sh_h_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))))) + L_sh_h_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_sh_h_y*(cos(alpha_p)*cos(q1_hr)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_hr)) + 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)) + (W_b*cos(alpha_p)*sin(gamma_p))/2, L_sh_h_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_sh_h_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)))) - 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_sh_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_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)) - 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, L_sh_h_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_sh_h_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))))) - 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_sh_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_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))) + 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_sh_h_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_hr) + cos(gamma_p)*cos(q1_hr)*sin(alpha_p)) + 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)) - L_sh_h_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)))) + L_sh_h_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)), L_sh_h_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_sh_h_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))))) - 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_sh_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_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))) + 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)) + L_sh_h_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_sh_h_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))), L_sh_h_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_sh_h_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))))) + 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))) + L_sh_h_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)))) + L_sh_h_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_sh_h_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_sh_h_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))), L_sh_h_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_sh_h_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))))) + 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))) + L_sh_h_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)))) + L_sh_h_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_sh_h_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_sh_h_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_sh_h_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_sh_h_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))))), L_sh_h_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)))) + L_sh_h_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];