JJ_e_fl.m

 
J_e_fl =[1, 0, 0, 0, 1, 0, 0, 0, 1, L_sh_f_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_sh_f_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)))) + 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_sh_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))) + 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 + 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))), L_sh_f_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_sh_f_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))) - 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_sh_f_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - L_th_f_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)), L_sh_f_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_sh_f_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)))) + 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_sh_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))) + 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 + 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_sh_f_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))) - 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_sh_f_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))) + 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))) + L_sh_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))) + 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 - (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)) - L_sh_f_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))) + L_sh_f_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_sh_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))) - 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 - (L_b*cos(beta_p)*cos(gamma_p))/2 - L_h_x*cos(beta_p)*cos(gamma_p), L_sh_f_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_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_sh_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)) - 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)) - 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_sh_f_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_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_sh_f_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_sh_f_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))) + (L_b*cos(gamma_p))/2 + 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_sh_f_y*(cos(alpha_p)*cos(q1_fl)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(q1_fl)) - L_th_f_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)) + L_sh_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)) + 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)) + 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_sh_f_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))) + L_sh_f_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_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_sh_f_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_sh_f_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)))) + 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_sh_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))) + 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))) + 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))), L_sh_f_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_sh_f_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))) - 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_sh_f_y*(cos(alpha_p)*cos(gamma_p)*sin(q1_fl) + cos(gamma_p)*cos(q1_fl)*sin(alpha_p)) - L_th_f_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)), L_sh_f_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_sh_f_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)))) + 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_sh_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))) + 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))) + 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)) + L_sh_f_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))) + L_sh_f_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))), L_sh_f_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_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_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_sh_f_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)))), 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)) + L_sh_f_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))) + L_sh_f_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))), L_sh_f_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))) + L_sh_f_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))), L_sh_f_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_sh_f_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)))), L_sh_f_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))) + L_sh_f_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];