forked from ariannagavioli/Motion-Planning-for-KUKA-LBR
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdistancesAndGrads.m
56 lines (51 loc) · 30 KB
/
distancesAndGrads.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
function [dists,grads] = distancesAndGrads(q, obs)
%Distances of the control points from the obstacles along with their derivative computed in q
points = zeros(5,3);
points(1,:) = [-(cos(q(1))*sin(q(2)))/5; -(sin(q(1))*sin(q(2)))/5; cos(q(2))/5 + 9/25];
points(2,:) = [-(21*cos(q(1))*sin(q(2)))/50; -(21*sin(q(1))*sin(q(2)))/50; (21*cos(q(2)))/50 + 9/25];
points(3,:) = [-(21*cos(q(1))*sin(q(2)))/50 - (sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 - (cos(q(1))*cos(q(4))*sin(q(2)))/5; (sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 - (21*sin(q(1))*sin(q(2)))/50 - (cos(q(4))*sin(q(1))*sin(q(2)))/5;(21*cos(q(2)))/50 + (cos(q(2))*cos(q(4)))/5 + (cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25];
points(4,:) = [-(21*cos(q(1))*sin(q(2)))/50 - (2*sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 - (2*cos(q(1))*cos(q(4))*sin(q(2)))/5;(2*sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 - (21*sin(q(1))*sin(q(2)))/50 - (2*cos(q(4))*sin(q(1))*sin(q(2)))/5;(21*cos(q(2)))/50 + (2*cos(q(2))*cos(q(4)))/5 + (2*cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25];
points(5,:) = [(21*sin(q(6))*(cos(q(5))*(cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))))/100 - (21*cos(q(6))*(sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) + cos(q(1))*cos(q(4))*sin(q(2))))/100 - (21*cos(q(1))*sin(q(2)))/50 - (2*sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 - (2*cos(q(1))*cos(q(4))*sin(q(2)))/5;
(21*cos(q(6))*(sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) - cos(q(4))*sin(q(1))*sin(q(2))))/100 - (21*sin(q(1))*sin(q(2)))/50 - (21*sin(q(6))*(cos(q(5))*(cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) + sin(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))))/100 + (2*sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 - (2*cos(q(4))*sin(q(1))*sin(q(2)))/5;
(21*cos(q(2)))/50 + (2*cos(q(2))*cos(q(4)))/5 + (21*sin(q(6))*(cos(q(5))*(cos(q(2))*sin(q(4)) - cos(q(3))*cos(q(4))*sin(q(2))) + sin(q(2))*sin(q(3))*sin(q(5))))/100 + (21*cos(q(6))*(cos(q(2))*cos(q(4)) + cos(q(3))*sin(q(2))*sin(q(4))))/100 + (2*cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25];
dists = zeros(5,1);
for i=1:5
dists(i) = norm(points(i,:) - obs);
end
grads = zeros(5,7);
grads(1,:) = [ (2*sin(q(2))*(obs(2)*cos(q(1)) - obs(1)*sin(q(1))))/5;
(2*cos(q(2))*sin(q(1))*(obs(2) + (sin(q(1))*sin(q(2)))/5))/5 - (2*sin(q(2))*(cos(q(2))/5 - obs(3) + 9/25))/5 + (2*cos(q(1))*cos(q(2))*(obs(1) + (cos(q(1))*sin(q(2)))/5))/5;
0;
0;
0;
0;
0];
grads(2,:) = [ (21*sin(q(2))*(obs(2)*cos(q(1)) - obs(1)*sin(q(1))))/25;
(21*cos(q(2))*sin(q(1))*(obs(2) + (21*sin(q(1))*sin(q(2)))/50))/25 - (21*sin(q(2))*((21*cos(q(2)))/50 - obs(3) + 9/25))/25 + (21*cos(q(1))*cos(q(2))*(obs(1) + (21*cos(q(1))*sin(q(2)))/50))/25;
0;
0;
0;
0;
0];
grads(3,:) = [ (21*obs(2)*cos(q(1))*sin(q(2)))/25 - (21*obs(1)*sin(q(1))*sin(q(2)))/25 + (2*obs(2)*cos(q(1))*cos(q(4))*sin(q(2)))/5 - (2*obs(1)*cos(q(4))*sin(q(1))*sin(q(2)))/5 + (2*obs(1)*cos(q(1))*sin(q(3))*sin(q(4)))/5 + (2*obs(2)*sin(q(1))*sin(q(3))*sin(q(4)))/5 - (2*obs(2)*cos(q(1))*cos(q(2))*cos(q(3))*sin(q(4)))/5 + (2*obs(1)*cos(q(2))*cos(q(3))*sin(q(1))*sin(q(4)))/5;
(cos(q(1))*(21*cos(q(2)) + 10*cos(q(2))*cos(q(4)) + 10*cos(q(3))*sin(q(2))*sin(q(4)))*(obs(1) + (21*cos(q(1))*sin(q(2)))/50 + (sin(q(1))*sin(q(3))*sin(q(4)))/5 + (cos(q(1))*cos(q(4))*sin(q(2)))/5 - (cos(q(1))*cos(q(2))*cos(q(3))*sin(q(4)))/5))/25 - 2*((21*sin(q(2)))/50 + (cos(q(4))*sin(q(2)))/5 - (cos(q(2))*cos(q(3))*sin(q(4)))/5)*((21*cos(q(2)))/50 - obs(3) + (cos(q(2))*cos(q(4)))/5 + (cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25) + (sin(q(1))*(21*cos(q(2)) + 10*cos(q(2))*cos(q(4)) + 10*cos(q(3))*sin(q(2))*sin(q(4)))*(obs(2) + (21*sin(q(1))*sin(q(2)))/50 + (cos(q(4))*sin(q(1))*sin(q(2)))/5 - (cos(q(1))*sin(q(3))*sin(q(4)))/5 - (cos(q(2))*cos(q(3))*sin(q(1))*sin(q(4)))/5))/25;
(2*sin(q(4))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))*(obs(1) + (21*cos(q(1))*sin(q(2)))/50 + (sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 + (cos(q(1))*cos(q(4))*sin(q(2)))/5))/5 - (2*sin(q(4))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))*(obs(2) + (21*sin(q(1))*sin(q(2)))/50 - (sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (cos(q(4))*sin(q(1))*sin(q(2)))/5))/5 - (2*sin(q(2))*sin(q(3))*sin(q(4))*((21*cos(q(2)))/50 - obs(3) + (cos(q(2))*cos(q(4)))/5 + (cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25))/5;
2*((cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 - (cos(q(1))*sin(q(2))*sin(q(4)))/5)*(obs(1) + (21*cos(q(1))*sin(q(2)))/50 + (sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 + (cos(q(1))*cos(q(4))*sin(q(2)))/5) - 2*((cos(q(2))*sin(q(4)))/5 - (cos(q(3))*cos(q(4))*sin(q(2)))/5)*((21*cos(q(2)))/50 - obs(3) + (cos(q(2))*cos(q(4)))/5 + (cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25) - 2*((cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (sin(q(1))*sin(q(2))*sin(q(4)))/5)*(obs(2) + (21*sin(q(1))*sin(q(2)))/50 - (sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (cos(q(4))*sin(q(1))*sin(q(2)))/5);
0;
0;
0];
grads(4,:) = [ (21*obs(2)*cos(q(1))*sin(q(2)))/25 - (21*obs(1)*sin(q(1))*sin(q(2)))/25 + (4*obs(2)*cos(q(1))*cos(q(4))*sin(q(2)))/5 - (4*obs(1)*cos(q(4))*sin(q(1))*sin(q(2)))/5 + (4*obs(1)*cos(q(1))*sin(q(3))*sin(q(4)))/5 + (4*obs(2)*sin(q(1))*sin(q(3))*sin(q(4)))/5 - (4*obs(2)*cos(q(1))*cos(q(2))*cos(q(3))*sin(q(4)))/5 + (4*obs(1)*cos(q(2))*cos(q(3))*sin(q(1))*sin(q(4)))/5;
(cos(q(1))*(21*cos(q(2)) + 20*cos(q(2))*cos(q(4)) + 20*cos(q(3))*sin(q(2))*sin(q(4)))*(obs(1) + (21*cos(q(1))*sin(q(2)))/50 + (2*sin(q(1))*sin(q(3))*sin(q(4)))/5 + (2*cos(q(1))*cos(q(4))*sin(q(2)))/5 - (2*cos(q(1))*cos(q(2))*cos(q(3))*sin(q(4)))/5))/25 - 2*((21*sin(q(2)))/50 + (2*cos(q(4))*sin(q(2)))/5 - (2*cos(q(2))*cos(q(3))*sin(q(4)))/5)*((21*cos(q(2)))/50 - obs(3) + (2*cos(q(2))*cos(q(4)))/5 + (2*cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25) + (sin(q(1))*(21*cos(q(2)) + 20*cos(q(2))*cos(q(4)) + 20*cos(q(3))*sin(q(2))*sin(q(4)))*(obs(2) + (21*sin(q(1))*sin(q(2)))/50 + (2*cos(q(4))*sin(q(1))*sin(q(2)))/5 - (2*cos(q(1))*sin(q(3))*sin(q(4)))/5 - (2*cos(q(2))*cos(q(3))*sin(q(1))*sin(q(4)))/5))/25;
(4*sin(q(4))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))*(obs(1) + (21*cos(q(1))*sin(q(2)))/50 + (2*sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 + (2*cos(q(1))*cos(q(4))*sin(q(2)))/5))/5 - (4*sin(q(4))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))*(obs(2) + (21*sin(q(1))*sin(q(2)))/50 - (2*sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (2*cos(q(4))*sin(q(1))*sin(q(2)))/5))/5 - (4*sin(q(2))*sin(q(3))*sin(q(4))*((21*cos(q(2)))/50 - obs(3) + (2*cos(q(2))*cos(q(4)))/5 + (2*cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25))/5;
2*((2*cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 - (2*cos(q(1))*sin(q(2))*sin(q(4)))/5)*(obs(1) + (21*cos(q(1))*sin(q(2)))/50 + (2*sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 + (2*cos(q(1))*cos(q(4))*sin(q(2)))/5) - 2*((2*cos(q(2))*sin(q(4)))/5 - (2*cos(q(3))*cos(q(4))*sin(q(2)))/5)*((21*cos(q(2)))/50 - obs(3) + (2*cos(q(2))*cos(q(4)))/5 + (2*cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25) - 2*((2*cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (2*sin(q(1))*sin(q(2))*sin(q(4)))/5)*(obs(2) + (21*sin(q(1))*sin(q(2)))/50 - (2*sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (2*cos(q(4))*sin(q(1))*sin(q(2)))/5);
0;
0;
0];
grads(5,:) = [ (21*obs(2)*cos(q(1))*sin(q(2)))/25 - (21*obs(1)*sin(q(1))*sin(q(2)))/25 + (4*obs(2)*cos(q(1))*cos(q(4))*sin(q(2)))/5 - (4*obs(1)*cos(q(4))*sin(q(1))*sin(q(2)))/5 + (4*obs(1)*cos(q(1))*sin(q(3))*sin(q(4)))/5 + (4*obs(2)*sin(q(1))*sin(q(3))*sin(q(4)))/5 - (4*obs(2)*cos(q(1))*cos(q(2))*cos(q(3))*sin(q(4)))/5 + (21*obs(2)*cos(q(1))*cos(q(4))*cos(q(6))*sin(q(2)))/50 + (4*obs(1)*cos(q(2))*cos(q(3))*sin(q(1))*sin(q(4)))/5 - (21*obs(1)*cos(q(4))*cos(q(6))*sin(q(1))*sin(q(2)))/50 + (21*obs(1)*cos(q(1))*cos(q(6))*sin(q(3))*sin(q(4)))/50 - (21*obs(1)*cos(q(1))*cos(q(3))*sin(q(5))*sin(q(6)))/50 + (21*obs(2)*cos(q(6))*sin(q(1))*sin(q(3))*sin(q(4)))/50 - (21*obs(2)*cos(q(3))*sin(q(1))*sin(q(5))*sin(q(6)))/50 - (21*obs(2)*cos(q(1))*cos(q(2))*cos(q(3))*cos(q(6))*sin(q(4)))/50 + (21*obs(1)*cos(q(2))*cos(q(3))*cos(q(6))*sin(q(1))*sin(q(4)))/50 - (21*obs(1)*cos(q(1))*cos(q(4))*cos(q(5))*sin(q(3))*sin(q(6)))/50 - (21*obs(2)*cos(q(1))*cos(q(2))*sin(q(3))*sin(q(5))*sin(q(6)))/50 + (21*obs(2)*cos(q(1))*cos(q(5))*sin(q(2))*sin(q(4))*sin(q(6)))/50 - (21*obs(2)*cos(q(4))*cos(q(5))*sin(q(1))*sin(q(3))*sin(q(6)))/50 + (21*obs(1)*cos(q(2))*sin(q(1))*sin(q(3))*sin(q(5))*sin(q(6)))/50 - (21*obs(1)*cos(q(5))*sin(q(1))*sin(q(2))*sin(q(4))*sin(q(6)))/50 + (21*obs(2)*cos(q(1))*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(6)))/50 - (21*obs(1)*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(1))*sin(q(6)))/50;
2*((21*cos(q(2))*sin(q(1)))/50 + (21*cos(q(6))*(cos(q(2))*cos(q(4))*sin(q(1)) + cos(q(3))*sin(q(1))*sin(q(2))*sin(q(4))))/100 + (21*sin(q(6))*(cos(q(5))*(cos(q(2))*sin(q(1))*sin(q(4)) - cos(q(3))*cos(q(4))*sin(q(1))*sin(q(2))) + sin(q(1))*sin(q(2))*sin(q(3))*sin(q(5))))/100 + (2*cos(q(2))*cos(q(4))*sin(q(1)))/5 + (2*cos(q(3))*sin(q(1))*sin(q(2))*sin(q(4)))/5)*(obs(2) + (21*sin(q(1))*sin(q(2)))/50 - (21*cos(q(6))*(sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) - cos(q(4))*sin(q(1))*sin(q(2))))/100 + (21*sin(q(6))*(cos(q(5))*(cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) + sin(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))))/100 - (2*sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (2*cos(q(4))*sin(q(1))*sin(q(2)))/5) - 2*((21*sin(q(2)))/50 + (2*cos(q(4))*sin(q(2)))/5 + (21*sin(q(6))*(cos(q(5))*(sin(q(2))*sin(q(4)) + cos(q(2))*cos(q(3))*cos(q(4))) - cos(q(2))*sin(q(3))*sin(q(5))))/100 + (21*cos(q(6))*(cos(q(4))*sin(q(2)) - cos(q(2))*cos(q(3))*sin(q(4))))/100 - (2*cos(q(2))*cos(q(3))*sin(q(4)))/5)*((21*cos(q(2)))/50 - obs(3) + (2*cos(q(2))*cos(q(4)))/5 + (21*sin(q(6))*(cos(q(5))*(cos(q(2))*sin(q(4)) - cos(q(3))*cos(q(4))*sin(q(2))) + sin(q(2))*sin(q(3))*sin(q(5))))/100 + (21*cos(q(6))*(cos(q(2))*cos(q(4)) + cos(q(3))*sin(q(2))*sin(q(4))))/100 + (2*cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25) + 2*((21*cos(q(1))*cos(q(2)))/50 + (21*cos(q(6))*(cos(q(1))*cos(q(2))*cos(q(4)) + cos(q(1))*cos(q(3))*sin(q(2))*sin(q(4))))/100 + (21*sin(q(6))*(cos(q(5))*(cos(q(1))*cos(q(2))*sin(q(4)) - cos(q(1))*cos(q(3))*cos(q(4))*sin(q(2))) + cos(q(1))*sin(q(2))*sin(q(3))*sin(q(5))))/100 + (2*cos(q(1))*cos(q(2))*cos(q(4)))/5 + (2*cos(q(1))*cos(q(3))*sin(q(2))*sin(q(4)))/5)*(obs(1) + (21*cos(q(1))*sin(q(2)))/50 + (21*cos(q(6))*(sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) + cos(q(1))*cos(q(4))*sin(q(2))))/100 - (21*sin(q(6))*(cos(q(5))*(cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))))/100 + (2*sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 + (2*cos(q(1))*cos(q(4))*sin(q(2)))/5);
2*((2*sin(q(4))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3))))/5 + (21*sin(q(6))*(sin(q(5))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(4))*cos(q(5))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))))/100 + (21*cos(q(6))*sin(q(4))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3))))/100)*(obs(1) + (21*cos(q(1))*sin(q(2)))/50 + (21*cos(q(6))*(sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) + cos(q(1))*cos(q(4))*sin(q(2))))/100 - (21*sin(q(6))*(cos(q(5))*(cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))))/100 + (2*sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 + (2*cos(q(1))*cos(q(4))*sin(q(2)))/5) - 2*((2*sin(q(2))*sin(q(3))*sin(q(4)))/5 - (21*sin(q(6))*(cos(q(3))*sin(q(2))*sin(q(5)) + cos(q(4))*cos(q(5))*sin(q(2))*sin(q(3))))/100 + (21*cos(q(6))*sin(q(2))*sin(q(3))*sin(q(4)))/100)*((21*cos(q(2)))/50 - obs(3) + (2*cos(q(2))*cos(q(4)))/5 + (21*sin(q(6))*(cos(q(5))*(cos(q(2))*sin(q(4)) - cos(q(3))*cos(q(4))*sin(q(2))) + sin(q(2))*sin(q(3))*sin(q(5))))/100 + (21*cos(q(6))*(cos(q(2))*cos(q(4)) + cos(q(3))*sin(q(2))*sin(q(4))))/100 + (2*cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25) - 2*((2*sin(q(4))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3))))/5 + (21*sin(q(6))*(sin(q(5))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) - cos(q(4))*cos(q(5))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))))/100 + (21*cos(q(6))*sin(q(4))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3))))/100)*(obs(2) + (21*sin(q(1))*sin(q(2)))/50 - (21*cos(q(6))*(sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) - cos(q(4))*sin(q(1))*sin(q(2))))/100 + (21*sin(q(6))*(cos(q(5))*(cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) + sin(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))))/100 - (2*sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (2*cos(q(4))*sin(q(1))*sin(q(2)))/5);
2*((21*cos(q(6))*(cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(1))*sin(q(2))*sin(q(4))))/100 + (2*cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 + (21*cos(q(5))*sin(q(6))*(sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) + cos(q(1))*cos(q(4))*sin(q(2))))/100 - (2*cos(q(1))*sin(q(2))*sin(q(4)))/5)*(obs(1) + (21*cos(q(1))*sin(q(2)))/50 + (21*cos(q(6))*(sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) + cos(q(1))*cos(q(4))*sin(q(2))))/100 - (21*sin(q(6))*(cos(q(5))*(cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))))/100 + (2*sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 + (2*cos(q(1))*cos(q(4))*sin(q(2)))/5) - 2*((2*cos(q(2))*sin(q(4)))/5 + (21*cos(q(6))*(cos(q(2))*sin(q(4)) - cos(q(3))*cos(q(4))*sin(q(2))))/100 - (21*cos(q(5))*sin(q(6))*(cos(q(2))*cos(q(4)) + cos(q(3))*sin(q(2))*sin(q(4))))/100 - (2*cos(q(3))*cos(q(4))*sin(q(2)))/5)*((21*cos(q(2)))/50 - obs(3) + (2*cos(q(2))*cos(q(4)))/5 + (21*sin(q(6))*(cos(q(5))*(cos(q(2))*sin(q(4)) - cos(q(3))*cos(q(4))*sin(q(2))) + sin(q(2))*sin(q(3))*sin(q(5))))/100 + (21*cos(q(6))*(cos(q(2))*cos(q(4)) + cos(q(3))*sin(q(2))*sin(q(4))))/100 + (2*cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25) - 2*((21*cos(q(6))*(cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) + sin(q(1))*sin(q(2))*sin(q(4))))/100 + (2*cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (2*sin(q(1))*sin(q(2))*sin(q(4)))/5 + (21*cos(q(5))*sin(q(6))*(sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) - cos(q(4))*sin(q(1))*sin(q(2))))/100)*(obs(2) + (21*sin(q(1))*sin(q(2)))/50 - (21*cos(q(6))*(sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) - cos(q(4))*sin(q(1))*sin(q(2))))/100 + (21*sin(q(6))*(cos(q(5))*(cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) + sin(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))))/100 - (2*sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (2*cos(q(4))*sin(q(1))*sin(q(2)))/5);
(21*sin(q(6))*(sin(q(5))*(cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(1))*sin(q(2))*sin(q(4))) - cos(q(5))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3))))*(obs(1) + (21*cos(q(1))*sin(q(2)))/50 + (21*cos(q(6))*(sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) + cos(q(1))*cos(q(4))*sin(q(2))))/100 - (21*sin(q(6))*(cos(q(5))*(cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))))/100 + (2*sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 + (2*cos(q(1))*cos(q(4))*sin(q(2)))/5))/50 - (21*sin(q(6))*(sin(q(5))*(cos(q(2))*sin(q(4)) - cos(q(3))*cos(q(4))*sin(q(2))) - cos(q(5))*sin(q(2))*sin(q(3)))*((21*cos(q(2)))/50 - obs(3) + (2*cos(q(2))*cos(q(4)))/5 + (21*sin(q(6))*(cos(q(5))*(cos(q(2))*sin(q(4)) - cos(q(3))*cos(q(4))*sin(q(2))) + sin(q(2))*sin(q(3))*sin(q(5))))/100 + (21*cos(q(6))*(cos(q(2))*cos(q(4)) + cos(q(3))*sin(q(2))*sin(q(4))))/100 + (2*cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25))/50 - (21*sin(q(6))*(sin(q(5))*(cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) + sin(q(1))*sin(q(2))*sin(q(4))) - cos(q(5))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3))))*(obs(2) + (21*sin(q(1))*sin(q(2)))/50 - (21*cos(q(6))*(sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) - cos(q(4))*sin(q(1))*sin(q(2))))/100 + (21*sin(q(6))*(cos(q(5))*(cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) + sin(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))))/100 - (2*sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (2*cos(q(4))*sin(q(1))*sin(q(2)))/5))/50;
2*((21*sin(q(6))*(sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) - cos(q(4))*sin(q(1))*sin(q(2))))/100 + (21*cos(q(6))*(cos(q(5))*(cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) + sin(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))))/100)*(obs(2) + (21*sin(q(1))*sin(q(2)))/50 - (21*cos(q(6))*(sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) - cos(q(4))*sin(q(1))*sin(q(2))))/100 + (21*sin(q(6))*(cos(q(5))*(cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) + sin(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))))/100 - (2*sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (2*cos(q(4))*sin(q(1))*sin(q(2)))/5) - 2*((21*sin(q(6))*(sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) + cos(q(1))*cos(q(4))*sin(q(2))))/100 + (21*cos(q(6))*(cos(q(5))*(cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))))/100)*(obs(1) + (21*cos(q(1))*sin(q(2)))/50 + (21*cos(q(6))*(sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) + cos(q(1))*cos(q(4))*sin(q(2))))/100 - (21*sin(q(6))*(cos(q(5))*(cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))))/100 + (2*sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 + (2*cos(q(1))*cos(q(4))*sin(q(2)))/5) + 2*((21*cos(q(6))*(cos(q(5))*(cos(q(2))*sin(q(4)) - cos(q(3))*cos(q(4))*sin(q(2))) + sin(q(2))*sin(q(3))*sin(q(5))))/100 - (21*sin(q(6))*(cos(q(2))*cos(q(4)) + cos(q(3))*sin(q(2))*sin(q(4))))/100)*((21*cos(q(2)))/50 - obs(3) + (2*cos(q(2))*cos(q(4)))/5 + (21*sin(q(6))*(cos(q(5))*(cos(q(2))*sin(q(4)) - cos(q(3))*cos(q(4))*sin(q(2))) + sin(q(2))*sin(q(3))*sin(q(5))))/100 + (21*cos(q(6))*(cos(q(2))*cos(q(4)) + cos(q(3))*sin(q(2))*sin(q(4))))/100 + (2*cos(q(3))*sin(q(2))*sin(q(4)))/5 + 9/25);
0];
end