-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathangle_jacob.m
141 lines (116 loc) · 154 KB
/
angle_jacob.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
function dq=angle_jacob(P)
%size(P)
p=P(1:18);
dp=P(19:36);
ddp=P(37:54);
%% Input
r_p = p(1:3); % pelvis position
x_p = r_p(1);
y_p = r_p(2);
z_p = r_p(3);
q_p = p(4:6); % pelvis rotation
alpha_p = q_p(1);
beta_p = q_p(2);
gamma_p = q_p(3);
r_fr = p(7:9); % Right ankle position
x_fr = r_fr(1);
y_fr = r_fr(2);
z_fr = r_fr(3);
r_fl = p(10:12); % Left ankle position
x_fl = r_fl(1);
y_fl = r_fl(2);
z_fl = r_fl(3);
r_hr = p(13:15); % Right ankle rotation
x_hr = r_hr(1);
y_hr = r_hr(2);
z_hr = r_hr(3);
r_hl = p(16:18); % Left ankle rotation
x_hl = r_hl(1);
y_hl = r_hl(2);
z_hl = r_hl(3);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dr_p = dp(1:3); % pelvis position
dx_p = dr_p(1);
dy_p = dr_p(2);
dz_p = dr_p(3);
dq_p = dp(4:6); % pelvis rotation
dalpha_p = dq_p(1);
dbeta_p = dq_p(2);
dgamma_p = dq_p(3);
dr_fr = dp(7:9); % Right ankle position
dx_fr = dr_fr(1);
dy_fr = dr_fr(2);
dz_fr = dr_fr(3);
dr_fl = dp(10:12); % Left ankle position
dx_fl = dr_fl(1);
dy_fl = dr_fl(2);
dz_fl = dr_fl(3);
dr_hr = dp(13:15); % Right ankle rotation
dx_hr = dr_hr(1);
dy_hr = dr_hr(2);
dz_hr = dr_hr(3);
dr_hl = dp(16:18); % Left ankle rotation
dx_hl = dr_hl(1);
dy_hl = dr_hl(2);
dz_hl = dr_hl(3);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
q=P(55:66);
t=P(67);
%% Global Variables
global L_sh L_th L_inth L_off L_pl
global omega
global Step_Time
global SCR
%% Angle Joints
qr=q(1:6);ql=q(7:12);
q1_r=qr(1); q2_r=qr(2); q3_r=qr(3); q4_r=qr(4); q5_r=qr(5); q6_r=qr(6);
q1_l=ql(1); q2_l=ql(2); q3_l=ql(3); q4_l=ql(4); q5_l=ql(5); q6_l=ql(6);
%% Jacobian Matrix
J_v_r =[
1, 0, 0, L_th*(cos(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_r)*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_r)*sin(q3_r)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) + L_inth*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + L_sh*(cos(q4_r)*(cos(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_r)*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_r)*sin(q3_r)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q4_r)*(sin(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_r)*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) + cos(q2_r)*sin(q3_r)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)))) + L_off*sin(q3_r)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (L_pl*cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(2*(cos(omega)^2 + sin(omega)^2)) - (L_pl*sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(2*(cos(omega)^2 + sin(omega)^2)), L_sh*(sin(q4_r)*(sin(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) - cos(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p))) - cos(q4_r)*(cos(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) + sin(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p)))) - L_inth*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - L_th*(cos(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) + sin(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p))) + L_off*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p)) - (L_pl*cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(2*(cos(omega)^2 + sin(omega)^2)) + (L_pl*sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(2*(cos(omega)^2 + sin(omega)^2)), L_sh*(cos(q4_r)*(sin(q2_r)*(sin(q3_r)*((cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(q3_r)*sin(gamma_p)) - cos(q2_r)*((cos(q3_r)*((cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*sin(gamma_p)*sin(q3_r))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)) - (cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(beta_p)*cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2)))) + sin(q4_r)*(cos(q2_r)*(sin(q3_r)*((cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(q3_r)*sin(gamma_p)) + sin(q2_r)*((cos(q3_r)*((cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*sin(gamma_p)*sin(q3_r))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)) - (cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(beta_p)*cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))))) - L_off*(sin(q3_r)*((cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(q3_r)*sin(gamma_p)) + L_th*(sin(q2_r)*(sin(q3_r)*((cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(q3_r)*sin(gamma_p)) - cos(q2_r)*((cos(q3_r)*((cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*sin(gamma_p)*sin(q3_r))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)) - (cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(beta_p)*cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2)))) + L_inth*((cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(beta_p)*cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2)) + (L_pl*cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega))/(2*(cos(omega)^2 + sin(omega)^2)) + (L_pl*cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega))/(2*(cos(omega)^2 + sin(omega)^2)), L_sh*(cos(q2_r)*cos(q4_r)*((cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + (cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r))) - sin(q2_r)*sin(q4_r)*((cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + (cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)))) + L_th*cos(q2_r)*((cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + (cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r))), L_th*(sin(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r))) - cos(q2_r)*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r))) + L_sh*(cos(q4_r)*(sin(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r))) - cos(q2_r)*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r))) + sin(q4_r)*(cos(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r))) + sin(q2_r)*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r)))), L_off*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) - L_sh*(cos(q4_r)*(sin(q2_r)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) + cos(q2_r)*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r))) + sin(q4_r)*(cos(q2_r)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) - sin(q2_r)*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r)))) - L_th*(sin(q2_r)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) + cos(q2_r)*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r))), L_sh*(cos(q4_r)*(sin(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r))) - cos(q2_r)*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r))) + sin(q4_r)*(cos(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r))) + sin(q2_r)*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r)))), 0, 0;
0, 1, 0, L_inth*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)) + L_sh*(cos(q4_r)*(cos(q2_r)*(((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r)) + cos(q3_r)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) - sin(q2_r)*sin(q3_r)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))) - sin(q4_r)*(sin(q2_r)*(((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r)) + cos(q3_r)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) + cos(q2_r)*sin(q3_r)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2)))) + L_th*(cos(q2_r)*(((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r)) + cos(q3_r)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) - sin(q2_r)*sin(q3_r)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))) + L_off*sin(q3_r)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2)) - (L_pl*cos(alpha_p)*cos(gamma_p)*sin(omega))/(2*(cos(omega)^2 + sin(omega)^2)) + (L_pl*cos(gamma_p)*cos(omega)*sin(alpha_p))/(2*(cos(omega)^2 + sin(omega)^2)), 0, L_inth*((cos(alpha_p)*sin(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(omega)*sin(alpha_p)*sin(gamma_p))/(cos(omega)^2 + sin(omega)^2)) - L_sh*(cos(q4_r)*(cos(q2_r)*((cos(gamma_p)*sin(q3_r) + cos(q3_r)*((cos(alpha_p)*cos(omega)*sin(gamma_p))/(cos(omega)^2 + sin(omega)^2) + (sin(alpha_p)*sin(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)) - ((cos(alpha_p)*sin(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(omega)*sin(alpha_p)*sin(gamma_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))) - sin(q2_r)*(sin(q3_r)*((cos(alpha_p)*cos(omega)*sin(gamma_p))/(cos(omega)^2 + sin(omega)^2) + (sin(alpha_p)*sin(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r))) - sin(q4_r)*(sin(q2_r)*((cos(gamma_p)*sin(q3_r) + cos(q3_r)*((cos(alpha_p)*cos(omega)*sin(gamma_p))/(cos(omega)^2 + sin(omega)^2) + (sin(alpha_p)*sin(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)) - ((cos(alpha_p)*sin(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(omega)*sin(alpha_p)*sin(gamma_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))) + cos(q2_r)*(sin(q3_r)*((cos(alpha_p)*cos(omega)*sin(gamma_p))/(cos(omega)^2 + sin(omega)^2) + (sin(alpha_p)*sin(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)))) - L_th*(cos(q2_r)*((cos(gamma_p)*sin(q3_r) + cos(q3_r)*((cos(alpha_p)*cos(omega)*sin(gamma_p))/(cos(omega)^2 + sin(omega)^2) + (sin(alpha_p)*sin(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)) - ((cos(alpha_p)*sin(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(omega)*sin(alpha_p)*sin(gamma_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))) - sin(q2_r)*(sin(q3_r)*((cos(alpha_p)*cos(omega)*sin(gamma_p))/(cos(omega)^2 + sin(omega)^2) + (sin(alpha_p)*sin(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r))) - L_off*(sin(q3_r)*((cos(alpha_p)*cos(omega)*sin(gamma_p))/(cos(omega)^2 + sin(omega)^2) + (sin(alpha_p)*sin(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)) + (L_pl*cos(alpha_p)*cos(omega)*sin(gamma_p))/(2*(cos(omega)^2 + sin(omega)^2)) + (L_pl*sin(alpha_p)*sin(gamma_p)*sin(omega))/(2*(cos(omega)^2 + sin(omega)^2)), - L_sh*(cos(q2_r)*cos(q4_r)*((sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r)) - ((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) - sin(q2_r)*sin(q4_r)*((sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r)) - ((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)))) - L_th*cos(q2_r)*((sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r)) - ((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))), L_th*(sin(q2_r)*((sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)) + ((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))) - cos(q2_r)*(cos(q3_r)*sin(gamma_p) + sin(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))) + L_sh*(cos(q4_r)*(sin(q2_r)*((sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)) + ((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))) - cos(q2_r)*(cos(q3_r)*sin(gamma_p) + sin(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))) + sin(q4_r)*(sin(q2_r)*(cos(q3_r)*sin(gamma_p) + sin(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(q2_r)*((sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)) + ((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))))), L_th*(sin(q2_r)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_r)*(cos(q3_r)*sin(gamma_p) + sin(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) + L_sh*(cos(q4_r)*(sin(q2_r)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_r)*(cos(q3_r)*sin(gamma_p) + sin(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) + sin(q4_r)*(cos(q2_r)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_r)*(cos(q3_r)*sin(gamma_p) + sin(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)))) - L_off*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))), L_sh*(cos(q4_r)*(sin(q2_r)*((sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)) + ((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))) - cos(q2_r)*(cos(q3_r)*sin(gamma_p) + sin(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))) + sin(q4_r)*(sin(q2_r)*(cos(q3_r)*sin(gamma_p) + sin(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(q2_r)*((sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)) + ((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))))), 0, 0;
0, 0, 1, L_th*(cos(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_r)*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_r)*sin(q3_r)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) + L_inth*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + L_sh*(cos(q4_r)*(cos(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_r)*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_r)*sin(q3_r)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q4_r)*(sin(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_r)*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) + cos(q2_r)*sin(q3_r)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)))) + L_off*sin(q3_r)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (L_pl*cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(2*(cos(omega)^2 + sin(omega)^2)) - (L_pl*sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(2*(cos(omega)^2 + sin(omega)^2)), L_inth*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + L_th*(cos(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r))) + sin(q2_r)*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r))) - L_off*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r)) + L_sh*(cos(q4_r)*(cos(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r))) + sin(q2_r)*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r))) - sin(q4_r)*(sin(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r))) - cos(q2_r)*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r)))) + (L_pl*cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(2*(cos(omega)^2 + sin(omega)^2)) - (L_pl*sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(2*(cos(omega)^2 + sin(omega)^2)), L_off*(sin(q3_r)*((cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p))/(cos(omega)^2 + sin(omega)^2) + (cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_r)*sin(beta_p)*sin(gamma_p)) - L_sh*(cos(q4_r)*(cos(q2_r)*(((cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p)*sin(beta_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r)) - (cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(cos(q3_r)*((cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p))/(cos(omega)^2 + sin(omega)^2) + (cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - sin(beta_p)*sin(gamma_p)*sin(q3_r))) + sin(q2_r)*(sin(q3_r)*((cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p))/(cos(omega)^2 + sin(omega)^2) + (cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_r)*sin(beta_p)*sin(gamma_p))) - sin(q4_r)*(sin(q2_r)*(((cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p)*sin(beta_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r)) - (cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(cos(q3_r)*((cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p))/(cos(omega)^2 + sin(omega)^2) + (cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - sin(beta_p)*sin(gamma_p)*sin(q3_r))) - cos(q2_r)*(sin(q3_r)*((cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p))/(cos(omega)^2 + sin(omega)^2) + (cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_r)*sin(beta_p)*sin(gamma_p)))) - L_th*(cos(q2_r)*(((cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p)*sin(beta_p))/(cos(omega)^2 + sin(omega)^2))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r)) - (cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*(cos(q3_r)*((cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p))/(cos(omega)^2 + sin(omega)^2) + (cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - sin(beta_p)*sin(gamma_p)*sin(q3_r))) + sin(q2_r)*(sin(q3_r)*((cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p))/(cos(omega)^2 + sin(omega)^2) + (cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_r)*sin(beta_p)*sin(gamma_p))) - L_inth*((cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p)*sin(beta_p))/(cos(omega)^2 + sin(omega)^2)) - (L_pl*cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p))/(2*(cos(omega)^2 + sin(omega)^2)) - (L_pl*cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega))/(2*(cos(omega)^2 + sin(omega)^2)), L_th*cos(q2_r)*((cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + (cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))) - L_sh*(sin(q2_r)*sin(q4_r)*((cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + (cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))) - cos(q2_r)*cos(q4_r)*((cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + (cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r))*(cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r)))), L_sh*(sin(q4_r)*(cos(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) + sin(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p))) + cos(q4_r)*(sin(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) - cos(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p)))) + L_th*(sin(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) - cos(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p))), L_off*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) - L_sh*(cos(q4_r)*(sin(q2_r)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) + cos(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) + sin(q4_r)*(cos(q2_r)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) - sin(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega)))) - L_th*(sin(q2_r)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) + cos(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))), L_sh*(sin(q4_r)*(cos(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) + sin(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p))) + cos(q4_r)*(sin(q2_r)*((cos(omega)*cos(q1_r) - sin(omega)*sin(q1_r))*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r))*(cos(omega)*sin(q1_r) + cos(q1_r)*sin(omega))) - cos(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p)))), 0, 0];
J_v_l =[
1, 0, 0, L_sh*(cos(q4_l)*(cos(q2_l)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_l)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_l)*sin(q3_l)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))) - sin(q4_l)*(sin(q2_l)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_l)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(q2_l)*sin(q3_l)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))) + L_th*(cos(q2_l)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_l)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_l)*sin(q3_l)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))) + L_inth*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + (L_pl*cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2 - (L_pl*sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 + L_off*sin(q3_l)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))), L_off*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p)) - L_sh*(cos(q4_l)*(cos(q2_l)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p))) - sin(q4_l)*(sin(q2_l)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p)))) - L_th*(cos(q2_l)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p))) - L_inth*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))) + (L_pl*cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + (L_pl*sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2, L_th*(sin(q2_l)*(sin(q3_l)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega)) + cos(beta_p)*cos(q3_l)*sin(gamma_p)) - cos(q2_l)*((cos(q3_l)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega)) - cos(beta_p)*sin(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + (cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(omega) + cos(beta_p)*cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)))) - L_inth*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(omega) + cos(beta_p)*cos(gamma_p)*cos(omega)*sin(alpha_p)) + L_sh*(cos(q4_l)*(sin(q2_l)*(sin(q3_l)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega)) + cos(beta_p)*cos(q3_l)*sin(gamma_p)) - cos(q2_l)*((cos(q3_l)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega)) - cos(beta_p)*sin(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + (cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(omega) + cos(beta_p)*cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)))) + sin(q4_l)*(cos(q2_l)*(sin(q3_l)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega)) + cos(beta_p)*cos(q3_l)*sin(gamma_p)) + sin(q2_l)*((cos(q3_l)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega)) - cos(beta_p)*sin(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + (cos(alpha_p)*cos(beta_p)*cos(gamma_p)*sin(omega) + cos(beta_p)*cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2))))) - L_off*(sin(q3_l)*(cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega) - cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega)) + cos(beta_p)*cos(q3_l)*sin(gamma_p)) - (L_pl*cos(alpha_p)*cos(beta_p)*cos(gamma_p)*cos(omega))/2 + (L_pl*cos(beta_p)*cos(gamma_p)*sin(alpha_p)*sin(omega))/2, L_sh*(cos(q2_l)*cos(q4_l)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + (cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_l)*sin(q4_l)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + (cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)))) + L_th*cos(q2_l)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + (cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2))), L_sh*(cos(q4_l)*(sin(q2_l)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l))) + sin(q4_l)*(cos(q2_l)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l)))) + L_th*(sin(q2_l)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l))), L_off*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)) - L_sh*(cos(q4_l)*(sin(q2_l)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)) + cos(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q4_l)*(cos(q2_l)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)) - sin(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)))) - L_th*(sin(q2_l)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)) + cos(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))), L_sh*(cos(q4_l)*(sin(q2_l)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l))) + sin(q4_l)*(cos(q2_l)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l)))), 0, 0;
0, 1, 0, L_th*(cos(q2_l)*((cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p))) + L_inth*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)) + L_sh*(cos(q4_l)*(cos(q2_l)*((cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p))) - sin(q4_l)*(sin(q2_l)*((cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))) - L_off*sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)) - (L_pl*cos(alpha_p)*cos(gamma_p)*sin(omega))/2 - (L_pl*cos(gamma_p)*cos(omega)*sin(alpha_p))/2, 0, L_th*(sin(q2_l)*(sin(q3_l)*(cos(alpha_p)*cos(omega)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(omega)) - cos(gamma_p)*cos(q3_l)) - cos(q2_l)*((cos(gamma_p)*sin(q3_l) + cos(q3_l)*(cos(alpha_p)*cos(omega)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(omega)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + (cos(alpha_p)*sin(gamma_p)*sin(omega) + cos(omega)*sin(alpha_p)*sin(gamma_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)))) - L_off*(sin(q3_l)*(cos(alpha_p)*cos(omega)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(omega)) - cos(gamma_p)*cos(q3_l)) + L_sh*(cos(q4_l)*(sin(q2_l)*(sin(q3_l)*(cos(alpha_p)*cos(omega)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(omega)) - cos(gamma_p)*cos(q3_l)) - cos(q2_l)*((cos(gamma_p)*sin(q3_l) + cos(q3_l)*(cos(alpha_p)*cos(omega)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(omega)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + (cos(alpha_p)*sin(gamma_p)*sin(omega) + cos(omega)*sin(alpha_p)*sin(gamma_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)))) + sin(q4_l)*(sin(q2_l)*((cos(gamma_p)*sin(q3_l) + cos(q3_l)*(cos(alpha_p)*cos(omega)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(omega)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + (cos(alpha_p)*sin(gamma_p)*sin(omega) + cos(omega)*sin(alpha_p)*sin(gamma_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2))) + cos(q2_l)*(sin(q3_l)*(cos(alpha_p)*cos(omega)*sin(gamma_p) - sin(alpha_p)*sin(gamma_p)*sin(omega)) - cos(gamma_p)*cos(q3_l)))) - L_inth*(cos(alpha_p)*sin(gamma_p)*sin(omega) + cos(omega)*sin(alpha_p)*sin(gamma_p)) - (L_pl*cos(alpha_p)*cos(omega)*sin(gamma_p))/2 + (L_pl*sin(alpha_p)*sin(gamma_p)*sin(omega))/2, - L_sh*(cos(q2_l)*cos(q4_l)*((sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) + (cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_l)*sin(q4_l)*((sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) + (cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)))) - L_th*cos(q2_l)*((sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) + (cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))), L_sh*(cos(q4_l)*(sin(q2_l)*((sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - (cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(cos(q3_l)*sin(gamma_p) + sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))) + sin(q4_l)*(cos(q2_l)*((sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - (cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(cos(q3_l)*sin(gamma_p) + sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))) + L_th*(sin(q2_l)*((sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - (cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(cos(q3_l)*sin(gamma_p) + sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))), L_th*(sin(q2_l)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))) - cos(q2_l)*(cos(q3_l)*sin(gamma_p) + sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - L_off*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))) + L_sh*(cos(q4_l)*(sin(q2_l)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))) - cos(q2_l)*(cos(q3_l)*sin(gamma_p) + sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q4_l)*(cos(q2_l)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))) + sin(q2_l)*(cos(q3_l)*sin(gamma_p) + sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)))), L_sh*(cos(q4_l)*(sin(q2_l)*((sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - (cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(cos(q3_l)*sin(gamma_p) + sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))) + sin(q4_l)*(cos(q2_l)*((sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) - (cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(cos(q3_l)*sin(gamma_p) + sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))), 0, 0;
0, 0, 1, L_sh*(cos(q4_l)*(cos(q2_l)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_l)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_l)*sin(q3_l)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))) - sin(q4_l)*(sin(q2_l)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_l)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(q2_l)*sin(q3_l)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))) + L_th*(cos(q2_l)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) + cos(q3_l)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_l)*sin(q3_l)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))) + L_inth*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + (L_pl*cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 - (L_pl*sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/2 + L_off*sin(q3_l)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))), L_sh*(cos(q4_l)*(cos(q2_l)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l))) - sin(q4_l)*(sin(q2_l)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l)))) - L_off*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l)) + L_inth*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))) + L_th*(cos(q2_l)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l))) - (L_pl*cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/2 - (L_pl*sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/2, L_inth*(cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)*sin(beta_p)) + L_sh*(cos(q4_l)*(cos(q2_l)*((cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)*sin(beta_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) + (cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega)) - sin(beta_p)*sin(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_l)*(sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega)) + cos(q3_l)*sin(beta_p)*sin(gamma_p))) - sin(q4_l)*(sin(q2_l)*((cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)*sin(beta_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) + (cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega)) - sin(beta_p)*sin(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(q2_l)*(sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega)) + cos(q3_l)*sin(beta_p)*sin(gamma_p)))) + L_th*(cos(q2_l)*((cos(alpha_p)*cos(gamma_p)*sin(beta_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)*sin(beta_p))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) + (cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega)) - sin(beta_p)*sin(gamma_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_l)*(sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega)) + cos(q3_l)*sin(beta_p)*sin(gamma_p))) + L_off*(sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p) - cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega)) + cos(q3_l)*sin(beta_p)*sin(gamma_p)) - (L_pl*cos(gamma_p)*sin(alpha_p)*sin(beta_p)*sin(omega))/2 + (L_pl*cos(alpha_p)*cos(gamma_p)*cos(omega)*sin(beta_p))/2, L_th*cos(q2_l)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + (cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2))) - L_sh*(sin(q2_l)*sin(q4_l)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + (cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*cos(q4_l)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)) + (cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)))), L_th*(sin(q2_l)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p))) + L_sh*(cos(q4_l)*(sin(q2_l)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p))) + sin(q4_l)*(cos(q2_l)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p)))), L_off*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)) - L_sh*(cos(q4_l)*(sin(q2_l)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)) + cos(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q4_l)*(cos(q2_l)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)) - sin(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2)))) - L_th*(sin(q2_l)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)) + cos(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))), L_sh*(cos(q4_l)*(sin(q2_l)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p))) + sin(q4_l)*(cos(q2_l)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))*((cos(omega)*cos(q1_l))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2)) - (cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l))*((cos(omega)*sin(q1_l))/(cos(omega)^2 + sin(omega)^2) - (cos(q1_l)*sin(omega))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p)))), 0, 0];
J_w_r =[
0, 0, 0, cos(beta_p)*cos(gamma_p), 0, sin(beta_p), sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r), cos(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))), (cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2), cos(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))), cos(q4_r)*(cos(q2_r)*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r)) + sin(q2_r)*(cos(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) + sin(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))))) - sin(q4_r)*(sin(q2_r)*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r)) - cos(q2_r)*(cos(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) + sin(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))))), sin(q5_r)*(cos(q4_r)*(sin(q2_r)*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r)) - cos(q2_r)*(cos(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) + sin(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))))) + sin(q4_r)*(cos(q2_r)*(sin(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(beta_p)*cos(gamma_p)*cos(q3_r)) + sin(q2_r)*(cos(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) + sin(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)))))) + cos(q5_r)*(cos(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(beta_p)*cos(gamma_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))));
0, 0, 0, sin(gamma_p), 1, 0, cos(q3_r)*sin(gamma_p) + sin(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2)), sin(q1_r)*(sin(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))) - cos(q1_r)*(cos(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))), (cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2), sin(q1_r)*(sin(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))) - cos(q1_r)*(cos(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))), cos(q4_r)*(cos(q2_r)*(cos(q3_r)*sin(gamma_p) + sin(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_r)*(cos(q1_r)*(sin(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))) + sin(q1_r)*(cos(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))))) - sin(q4_r)*(sin(q2_r)*(cos(q3_r)*sin(gamma_p) + sin(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(q2_r)*(cos(q1_r)*(sin(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))) + sin(q1_r)*(cos(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))))), sin(q5_r)*(cos(q4_r)*(sin(q2_r)*(cos(q3_r)*sin(gamma_p) + sin(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(q2_r)*(cos(q1_r)*(sin(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))) + sin(q1_r)*(cos(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))))) + sin(q4_r)*(cos(q2_r)*(cos(q3_r)*sin(gamma_p) + sin(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_r)*(cos(q1_r)*(sin(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))) + sin(q1_r)*(cos(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2)))))) - cos(q5_r)*(cos(q1_r)*(cos(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) - sin(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))) - sin(q1_r)*(sin(omega)*(sin(gamma_p)*sin(q3_r) - cos(q3_r)*((cos(gamma_p)*sin(alpha_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) + (cos(alpha_p)*cos(gamma_p)*cos(omega))/(cos(omega)^2 + sin(omega)^2))) + cos(omega)*((cos(alpha_p)*cos(gamma_p)*sin(omega))/(cos(omega)^2 + sin(omega)^2) - (cos(gamma_p)*cos(omega)*sin(alpha_p))/(cos(omega)^2 + sin(omega)^2))));
0, 0, 0, -cos(gamma_p)*sin(beta_p), 0, cos(beta_p), sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p), cos(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))), (cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2), cos(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))), cos(q4_r)*(sin(q2_r)*(cos(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) + sin(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)))) + cos(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p))) + sin(q4_r)*(cos(q2_r)*(cos(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) + sin(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)))) - sin(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p))), cos(q5_r)*(cos(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)))) - sin(q5_r)*(cos(q4_r)*(cos(q2_r)*(cos(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) + sin(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)))) - sin(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p))) - sin(q4_r)*(sin(q2_r)*(cos(q1_r)*(sin(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) - cos(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2))) + sin(q1_r)*(cos(omega)*(cos(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) + cos(gamma_p)*sin(beta_p)*sin(q3_r)) + sin(omega)*((cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) + (sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)))) + cos(q2_r)*(sin(q3_r)*((cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)))/(cos(omega)^2 + sin(omega)^2)) - cos(gamma_p)*cos(q3_r)*sin(beta_p))))];
J_w_l =[
0, 0, 0, cos(beta_p)*cos(gamma_p), 0, sin(beta_p), sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l), sin(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)) - cos(q1_l)*((sin(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2) - (cos(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2)), cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)), sin(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)) - cos(q1_l)*((sin(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2) - (cos(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2)), - cos(q4_l)*(sin(q2_l)*(cos(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)) + sin(q1_l)*((sin(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2) - (cos(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l))) - sin(q4_l)*(cos(q2_l)*(cos(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)) + sin(q1_l)*((sin(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2) - (cos(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l))), sin(q5_l)*(cos(q4_l)*(cos(q2_l)*(cos(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)) + sin(q1_l)*((sin(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2) - (cos(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l))) - sin(q4_l)*(sin(q2_l)*(cos(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)) + sin(q1_l)*((sin(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2) - (cos(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(sin(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) + cos(beta_p)*cos(gamma_p)*cos(q3_l)))) - cos(q5_l)*(cos(q1_l)*((sin(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2) - (cos(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2)) - sin(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p))) - cos(beta_p)*cos(gamma_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*sin(beta_p) + cos(beta_p)*sin(alpha_p)*sin(gamma_p)) - sin(omega)*(sin(alpha_p)*sin(beta_p) - cos(alpha_p)*cos(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)));
0, 0, 0, sin(gamma_p), 1, 0, cos(q3_l)*sin(gamma_p) + sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)), - cos(q1_l)*((cos(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2)) - sin(q1_l)*((sin(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2)), - cos(alpha_p)*cos(gamma_p)*sin(omega) - cos(gamma_p)*cos(omega)*sin(alpha_p), - cos(q1_l)*((cos(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2)) - sin(q1_l)*((sin(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2)), cos(q4_l)*(sin(q2_l)*(cos(q1_l)*((sin(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2)) - sin(q1_l)*((cos(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2))) + cos(q2_l)*(cos(q3_l)*sin(gamma_p) + sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))) + sin(q4_l)*(cos(q2_l)*(cos(q1_l)*((sin(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2)) - sin(q1_l)*((cos(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_l)*(cos(q3_l)*sin(gamma_p) + sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))), - sin(q5_l)*(cos(q4_l)*(cos(q2_l)*(cos(q1_l)*((sin(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2)) - sin(q1_l)*((cos(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2))) - sin(q2_l)*(cos(q3_l)*sin(gamma_p) + sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega)))) - sin(q4_l)*(sin(q2_l)*(cos(q1_l)*((sin(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2)) - sin(q1_l)*((cos(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2))) + cos(q2_l)*(cos(q3_l)*sin(gamma_p) + sin(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))) - cos(q5_l)*(cos(q1_l)*((cos(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2)) + sin(q1_l)*((sin(omega)*(sin(gamma_p)*sin(q3_l) - cos(q3_l)*(cos(alpha_p)*cos(gamma_p)*cos(omega) - cos(gamma_p)*sin(alpha_p)*sin(omega))))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(alpha_p)*cos(gamma_p)*sin(omega) + cos(gamma_p)*cos(omega)*sin(alpha_p)))/(cos(omega)^2 + sin(omega)^2)));
0, 0, 0, -cos(gamma_p)*sin(beta_p), 0, cos(beta_p), sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p), cos(q1_l)*((cos(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)) + sin(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)), cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)), cos(q1_l)*((cos(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)) + sin(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)), - cos(q4_l)*(sin(q2_l)*(cos(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)) - sin(q1_l)*((cos(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p))) - sin(q4_l)*(cos(q2_l)*(cos(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)) - sin(q1_l)*((cos(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p))), sin(q5_l)*(cos(q4_l)*(cos(q2_l)*(cos(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)) - sin(q1_l)*((cos(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2))) + sin(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p))) - sin(q4_l)*(sin(q2_l)*(cos(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)) - sin(q1_l)*((cos(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2))) - cos(q2_l)*(sin(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) - cos(gamma_p)*cos(q3_l)*sin(beta_p)))) + cos(q5_l)*(cos(q1_l)*((cos(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) - (sin(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)) + sin(q1_l)*((sin(omega)*(cos(q3_l)*(cos(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p)) + sin(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p))) + cos(gamma_p)*sin(beta_p)*sin(q3_l)))/(cos(omega)^2 + sin(omega)^2) + (cos(omega)*(cos(omega)*(cos(alpha_p)*cos(beta_p) - sin(alpha_p)*sin(beta_p)*sin(gamma_p)) - sin(omega)*(cos(beta_p)*sin(alpha_p) + cos(alpha_p)*sin(beta_p)*sin(gamma_p))))/(cos(omega)^2 + sin(omega)^2)))];
J_r=[J_v_r;J_w_r];
J_l=[J_v_l;J_w_l];
u=[x_p y_p z_p alpha_p beta_p gamma_p]';
du=[dx_p dy_p dz_p dalpha_p dbeta_p dgamma_p]';
J_u_r=J_r(1:6,1:6);
J_q_r=J_r(1:6,7:12);
J_u_l=J_l(1:6,1:6);
J_q_l=J_l(1:6,7:12);
%% velocity
v_ar=dr_ar;
v_al=dr_al;
R_fr_y=rot(2,beta_fr,3);
R_fr_z=rot(3,gamma_fr,3);
w_foot_r=[0;dbeta_fr;0]+R_fr_y*[0;0;dgamma_fr]+R_fr_y*R_fr_z*[dalpha_fr;0;0];
R_fl_y=rot(2,beta_fl,3);
R_fl_z=rot(3,gamma_fl,3);
w_foot_l=[0;dbeta_fl;0]+R_fl_y*[0;0;dgamma_fl]+R_fl_y*R_fl_z*[dalpha_fl;0;0];
vw_r=[v_ar;w_foot_r];
vw_l=[v_al;w_foot_l];
dqr=pinv(J_q_r)*(vw_r-J_u_r*du);
dql=pinv(J_q_l)*(vw_l-J_u_l*du);
%% Output
if and(SCR==1,t<=2*Step_Time)
dq=0.01*ones(12,1);
else
dq=[dqr;dql];
end
if max(abs(dq))>6
dq=0.01*ones(12,1);
end
% dq=[dqr;dql;0;0];
end