-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathfcn_extended_DCG.m
206 lines (198 loc) · 13.3 KB
/
fcn_extended_DCG.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
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
function [De,Ce,Ge,jacF,djacF] = fcn_extended_DCG(q)
dq=[q(6); q(7); q(8); q(9); q(10)];
De(1,1)=(5386*cos(q(4)))/625 - (384*cos(q(2) - q(3) + q(4) - q(5)))/625 - (1014*cos(q(2) - q(3) +...
q(4)))/625 - (288*cos(q(2) + q(4)))/125 - (288*cos(q(2)))/125 - (1014*cos(q(2) - q(3)))/625 + (384*...
cos(q(5)))/625 - (384*cos(q(2) - q(3) - q(5)))/625 + 8076/625;
De(1,2)=(5386*cos(q(4)))/625 - (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (507*cos(q(2) - q(3) +...
q(4)))/625 - (144*cos(q(2) + q(4)))/125 - (144*cos(q(2)))/125 - (507*cos(q(2) - q(3)))/625 - (192*...
cos(q(2) - q(3) - q(5)))/625 + 47259/5000;
De(1,3)=(384*cos(q(5)))/625 - (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (507*cos(q(2) - q(3) +...
q(4)))/625 - (507*cos(q(2) - q(3)))/625 - (192*cos(q(2) - q(3) - q(5)))/625 + 7243/5000;
De(1,4)=(2693*cos(q(4)))/625 - (507*cos(q(2) - q(3) + q(4)))/625 - (144*cos(q(2) +...
q(4)))/125 - (192*cos(q(2) - q(3) + q(4) - q(5)))/625 + 15281/3125;
De(1,5)=(192*cos(q(5)))/625 - (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (192*...
cos(q(2) - q(3) - q(5)))/625 + 1201/3125;
De(1,6)=(96*cos(q(1) + q(3) + q(5)))/125 - (1504*cos(q(1) + q(2) + q(4)))/125 - (2693*cos(q(1) +...
q(2)))/250 + (507*cos(q(1) + q(3)))/250 + (72*cos(q(1)))/25;
De(1,7)=(1504*sin(q(1) + q(2) + q(4)))/125 - (96*sin(q(1) + q(3) + q(5)))/125 + (2693*sin(q(1) +...
q(2)))/250 - (507*sin(q(1) + q(3)))/250 - (72*sin(q(1)))/25;
De(2,1)=(5386*cos(q(4)))/625 - (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (507*cos(q(2) - q(3) +...
q(4)))/625 - (144*cos(q(2) + q(4)))/125 - (144*cos(q(2)))/125 - (507*cos(q(2) - q(3)))/625 - (192*...
cos(q(2) - q(3) - q(5)))/625 + 47259/5000;
De(2,2)=(5386*cos(q(4)))/625 + 47259/5000;
De(2,3)=- (507*cos(q(2) - q(3)))/625 - (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (507*...
cos(q(2) - q(3) + q(4)))/625 - (192*cos(q(2) - q(3) - q(5)))/625;
De(2,4)=(2693*cos(q(4)))/625 + 15281/3125;
De(2,5)=- (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (192*cos(q(2) - q(3) - q(5)))/625;
De(2,6)=- (1504*cos(q(1) + q(2) + q(4)))/125 - (2693*cos(q(1) + q(2)))/250;
De(2,7)=(1504*sin(q(1) + q(2) + q(4)))/125 + (2693*sin(q(1) + q(2)))/250;
De(3,1)=(384*cos(q(5)))/625 - (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (507*cos(q(2) - q(3) +...
q(4)))/625 - (507*cos(q(2) - q(3)))/625 - (192*cos(q(2) - q(3) - q(5)))/625 + 7243/5000;
De(3,2)=- (507*cos(q(2) - q(3)))/625 - (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (507*...
cos(q(2) - q(3) + q(4)))/625 - (192*cos(q(2) - q(3) - q(5)))/625;
De(3,3)=(384*cos(q(5)))/625 + 7243/5000;
De(3,4)=- (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (507*cos(q(2) - q(3) + q(4)))/625;
De(3,5)=(192*cos(q(5)))/625 + 1201/3125;
De(3,6)=(96*cos(q(1) + q(3) + q(5)))/125 + (507*cos(q(1) + q(3)))/250;
De(3,7)=- (96*sin(q(1) + q(3) + q(5)))/125 - (507*sin(q(1) + q(3)))/250;
De(4,1)=(2693*cos(q(4)))/625 - (507*cos(q(2) - q(3) + q(4)))/625 - (144*cos(q(2) +...
q(4)))/125 - (192*cos(q(2) - q(3) + q(4) - q(5)))/625 + 15281/3125;
De(4,2)=(2693*cos(q(4)))/625 + 15281/3125;
De(4,3)=- (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (507*cos(q(2) - q(3) + q(4)))/625;
De(4,4)=15281/3125;
De(4,5)=-(192*cos(q(2) - q(3) + q(4) - q(5)))/625;
De(4,6)=-(1504*cos(q(1) + q(2) + q(4)))/125;
De(4,7)=(1504*sin(q(1) + q(2) + q(4)))/125;
De(5,1)=(192*cos(q(5)))/625 - (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (192*...
cos(q(2) - q(3) - q(5)))/625 + 1201/3125;
De(5,2)=- (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (192*cos(q(2) - q(3) - q(5)))/625;
De(5,3)=(192*cos(q(5)))/625 + 1201/3125;
De(5,4)=-(192*cos(q(2) - q(3) + q(4) - q(5)))/625;
De(5,5)=1201/3125;
De(5,6)=(96*cos(q(1) + q(3) + q(5)))/125;
De(5,7)=-(96*sin(q(1) + q(3) + q(5)))/125;
De(6,1)=(96*cos(q(1) + q(3) + q(5)))/125 - (1504*cos(q(1) + q(2) + q(4)))/125 - (2693*cos(q(1) +...
q(2)))/250 + (507*cos(q(1) + q(3)))/250 + (72*cos(q(1)))/25;
De(6,2)=- (1504*cos(q(1) + q(2) + q(4)))/125 - (2693*cos(q(1) + q(2)))/250;
De(6,3)=(96*cos(q(1) + q(3) + q(5)))/125 + (507*cos(q(1) + q(3)))/250;
De(6,4)=-(1504*cos(q(1) + q(2) + q(4)))/125;
De(6,5)=(96*cos(q(1) + q(3) + q(5)))/125;
De(6,6)=32;
De(6,7)=0;
De(7,1)=(1504*sin(q(1) + q(2) + q(4)))/125 - (96*sin(q(1) + q(3) + q(5)))/125 + (2693*sin(q(1) +...
q(2)))/250 - (507*sin(q(1) + q(3)))/250 - (72*sin(q(1)))/25;
De(7,2)=(1504*sin(q(1) + q(2) + q(4)))/125 + (2693*sin(q(1) + q(2)))/250;
De(7,3)=- (96*sin(q(1) + q(3) + q(5)))/125 - (507*sin(q(1) + q(3)))/250;
De(7,4)=(1504*sin(q(1) + q(2) + q(4)))/125;
De(7,5)=-(96*sin(q(1) + q(3) + q(5)))/125;
De(7,6)=0;
De(7,7)=32;
Ce(1,1)=dq(4)*((192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625 +...
(144*sin(q(2) + q(4)))/125 - (2693*sin(q(4)))/625) - dq(5)*((192*sin(q(2) - q(3) + q(4) - q(5)))/625 +...
(192*sin(q(5)))/625 + (192*sin(q(2) - q(3) - q(5)))/625) - dq(3)*((507*sin(q(2) - q(3)))/625 + (192*...
sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625 + (192*sin(q(2) - q(3) - q(5)))/625) + dq(2)*...
((507*sin(q(2) - q(3)))/625 + (192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*sin(q(2) - q(3) +...
q(4)))/625 + (144*sin(q(2) + q(4)))/125 + (144*sin(q(2)))/125 + (192*sin(q(2) - q(3) - q(5)))/625);
Ce(1,2)=dq(4)*((192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625 +...
(144*sin(q(2) + q(4)))/125 - (2693*sin(q(4)))/625) + dq(1)*((507*sin(q(2) - q(3)))/625 + (192*...
sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625 + (144*sin(q(2) + q(4)))/125 + (144*...
sin(q(2)))/125 + (192*sin(q(2) - q(3) - q(5)))/625) + dq(2)*((507*sin(q(2) - q(3)))/625 + (192*sin(q(2) - q(3) +...
q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625 + (144*sin(q(2) + q(4)))/125 + (144*sin(q(2)))/125 + (192*sin(q(2) - q(3) - q(5)))/625);
Ce(1,3)=- dq(5)*((192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (192*sin(q(5)))/625 + (192*...
sin(q(2) - q(3) - q(5)))/625) - dq(1)*((507*sin(q(2) - q(3)))/625 + (192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*sin(q(2) - q(3) +...
q(4)))/625 + (192*sin(q(2) - q(3) - q(5)))/625) - dq(3)*((507*sin(q(2) - q(3)))/625 + (192*sin(q(2) - q(3) +...
q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625 + (192*sin(q(2) - q(3) - q(5)))/625);
Ce(1,4)=((dq(1) + dq(2) + dq(4))*(192*sin(q(2) - q(3) + q(4) - q(5)) + 507*sin(q(2) - q(3) +...
q(4)) + 720*sin(q(2) + q(4)) - 2693*sin(q(4))))/625;
Ce(1,5)=-(192*(sin(q(2) - q(3) + q(4) - q(5)) + sin(q(5)) + sin(q(2) - q(3) - q(5)))*(dq(1) +...
dq(3) + dq(5)))/625;
Ce(1,6)=0;
Ce(1,7)=0;
Ce(2,1)=- (192*dq(1)*sin(q(2) - q(3) + q(4) - q(5)))/625 - (192*dq(3)*sin(q(2) - q(3) +...
q(4) - q(5)))/625 - (192*dq(5)*sin(q(2) - q(3) + q(4) - q(5)))/625 - (507*dq(1)*sin(q(2) - q(3) + q(4)))/625 - (507*dq(3)*...
sin(q(2) - q(3) + q(4)))/625 - (144*dq(1)*sin(q(2) + q(4)))/125 - (144*dq(1)*sin(q(2)))/125 - (2693*dq(4)*...
sin(q(4)))/625 - (192*dq(1)*sin(q(2) - q(3) - q(5)))/625 - (192*dq(3)*sin(q(2) - q(3) - q(5)))/625 - (192*dq(5)*...
sin(q(2) - q(3) - q(5)))/625 - (507*dq(1)*sin(q(2) - q(3)))/625 - (507*dq(3)*sin(q(2) - q(3)))/625;
Ce(2,2)=-(2693*dq(4)*sin(q(4)))/625;
Ce(2,3)=- dq(5)*((192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (192*...
sin(q(2) - q(3) - q(5)))/625) - dq(1)*((507*sin(q(2) - q(3)))/625 + (192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*sin(q(2) - q(3) +...
q(4)))/625 + (192*sin(q(2) - q(3) - q(5)))/625) - dq(3)*((507*sin(q(2) - q(3)))/625 + (192*sin(q(2) - q(3) +...
q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625 + (192*sin(q(2) - q(3) - q(5)))/625);
Ce(2,4)=-(2693*sin(q(4))*(dq(1) + dq(2) + dq(4)))/625;
Ce(2,5)=-(192*(sin(q(2) - q(3) + q(4) - q(5)) + sin(q(2) - q(3) - q(5)))*(dq(1) + dq(3) + dq(5)))/625;
Ce(2,6)=0;
Ce(2,7)=0;
Ce(3,1)=dq(1)*((507*sin(q(2) - q(3)))/625 + (192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*...
sin(q(2) - q(3) + q(4)))/625 + (192*sin(q(2) - q(3) - q(5)))/625) + dq(2)*((507*sin(q(2) - q(3)))/625 + (192*...
sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625 + (192*sin(q(2) - q(3) - q(5)))/625) - (192*...
dq(5)*sin(q(5)))/625 + dq(4)*((192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625);
Ce(3,2)=dq(1)*((507*sin(q(2) - q(3)))/625 + (192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*...
sin(q(2) - q(3) + q(4)))/625 + (192*sin(q(2) - q(3) - q(5)))/625) + dq(2)*((507*sin(q(2) - q(3)))/625 + (192*...
sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625 + (192*sin(q(2) - q(3) - q(5)))/625) + dq(4)*...
((192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625);
Ce(3,3)=-(192*dq(5)*sin(q(5)))/625;
Ce(3,4)=(3*(64*sin(q(2) - q(3) + q(4) - q(5)) + 169*sin(q(2) - q(3) + q(4)))*(dq(1) + dq(2) + dq(4)))/625;
Ce(3,5)=-(192*sin(q(5))*(dq(1) + dq(3) + dq(5)))/625;
Ce(3,6)=0;
Ce(3,7)=0;
Ce(4,1)=(2693*dq(2)*sin(q(4)))/625 - (192*dq(5)*sin(q(2) - q(3) + q(4) - q(5)))/625 - dq(1)*((192*...
sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625 + (144*sin(q(2) + q(4)))/125 - (2693*...
sin(q(4)))/625) - dq(3)*((192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625);
Ce(4,2)=(2693*sin(q(4))*(dq(1) + dq(2)))/625;
Ce(4,3)=- (192*dq(5)*sin(q(2) - q(3) + q(4) - q(5)))/625 - dq(1)*((192*sin(q(2) - q(3) +...
q(4) - q(5)))/625 + (507*sin(q(2) - q(3) + q(4)))/625) - dq(3)*((192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (507*...
sin(q(2) - q(3) + q(4)))/625);
Ce(4,4)=0;
Ce(4,5)=-(192*sin(q(2) - q(3) + q(4) - q(5))*(dq(1) + dq(3) + dq(5)))/625;
Ce(4,6)=0;
Ce(4,7)=0;
Ce(5,1)=(192*dq(4)*sin(q(2) - q(3) + q(4) - q(5)))/625 + dq(2)*((192*sin(q(2) - q(3) +...
q(4) - q(5)))/625 + (192*sin(q(2) - q(3) - q(5)))/625) + dq(1)*((192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (192*...
sin(q(5)))/625 + (192*sin(q(2) - q(3) - q(5)))/625) + (192*dq(3)*sin(q(5)))/625;
Ce(5,2)=(192*dq(4)*sin(q(2) - q(3) + q(4) - q(5)))/625 + dq(1)*((192*sin(q(2) - q(3) +...
q(4) - q(5)))/625 + (192*sin(q(2) - q(3) - q(5)))/625) + dq(2)*((192*sin(q(2) - q(3) + q(4) - q(5)))/625 + (192*sin(q(2) - q(3) - q(5)))/625);
Ce(5,3)=(192*sin(q(5))*(dq(1) + dq(3)))/625;
Ce(5,4)=(192*sin(q(2) - q(3) + q(4) - q(5))*(dq(1) + dq(2) + dq(4)))/625;
Ce(5,5)=0;
Ce(5,6)=0;
Ce(5,7)=0;
Ce(6,1)=dq(2)*((1504*sin(q(1) + q(2) + q(4)))/125 + (2693*sin(q(1) + q(2)))/250) - dq(3)*((96*...
sin(q(1) + q(3) + q(5)))/125 + (507*sin(q(1) + q(3)))/250) - dq(1)*((96*sin(q(1) + q(3) + q(5)))/125 - (1504*...
sin(q(1) + q(2) + q(4)))/125 - (2693*sin(q(1) + q(2)))/250 + (507*sin(q(1) + q(3)))/250 + (72*sin(q(1)))/25) +...
(1504*dq(4)*sin(q(1) + q(2) + q(4)))/125 - (96*dq(5)*sin(q(1) + q(3) + q(5)))/125;
Ce(6,2)=dq(1)*((1504*sin(q(1) + q(2) + q(4)))/125 + (2693*sin(q(1) + q(2)))/250) + dq(2)*((1504*...
sin(q(1) + q(2) + q(4)))/125 + (2693*sin(q(1) + q(2)))/250) + (1504*dq(4)*sin(q(1) + q(2) + q(4)))/125;
Ce(6,3)=- dq(1)*((96*sin(q(1) + q(3) + q(5)))/125 + (507*sin(q(1) + q(3)))/250) - dq(3)*((96*...
sin(q(1) + q(3) + q(5)))/125 + (507*sin(q(1) + q(3)))/250) - (96*dq(5)*sin(q(1) + q(3) + q(5)))/125;
Ce(6,4)=(1504*sin(q(1) + q(2) + q(4))*(dq(1) + dq(2) + dq(4)))/125;
Ce(6,5)=-(96*sin(q(1) + q(3) + q(5))*(dq(1) + dq(3) + dq(5)))/125;
Ce(6,6)=0;
Ce(6,7)=0;
Ce(7,1)=dq(2)*((1504*cos(q(1) + q(2) + q(4)))/125 + (2693*cos(q(1) + q(2)))/250) - dq(3)*((96*...
cos(q(1) + q(3) + q(5)))/125 + (507*cos(q(1) + q(3)))/250) - dq(1)*((96*cos(q(1) + q(3) + q(5)))/125 - (1504*...
cos(q(1) + q(2) + q(4)))/125 - (2693*cos(q(1) + q(2)))/250 + (507*cos(q(1) + q(3)))/250 + (72*cos(q(1)))/25) +...
(1504*dq(4)*cos(q(1) + q(2) + q(4)))/125 - (96*dq(5)*cos(q(1) + q(3) + q(5)))/125;
Ce(7,2)=dq(1)*((1504*cos(q(1) + q(2) + q(4)))/125 + (2693*cos(q(1) + q(2)))/250) + dq(2)*((1504*...
cos(q(1) + q(2) + q(4)))/125 + (2693*cos(q(1) + q(2)))/250) + (1504*dq(4)*cos(q(1) + q(2) + q(4)))/125;
Ce(7,3)=- dq(1)*((96*cos(q(1) + q(3) + q(5)))/125 + (507*cos(q(1) + q(3)))/250) - dq(3)*((96*...
cos(q(1) + q(3) + q(5)))/125 + (507*cos(q(1) + q(3)))/250) - (96*dq(5)*cos(q(1) + q(3) + q(5)))/125;
Ce(7,4)=(1504*cos(q(1) + q(2) + q(4))*(dq(1) + dq(2) + dq(4)))/125;
Ce(7,5)=-(96*cos(q(1) + q(3) + q(5))*(dq(1) + dq(3) + dq(5)))/125;
Ce(7,6)=0;
Ce(7,7)=0;
Ge(1,1)=(73696*sin(q(1) + q(2) + q(4)))/625 - (4704*sin(q(1) + q(3) + q(5)))/625 + (131957*...
sin(q(1) + q(2)))/1250 - (24843*sin(q(1) + q(3)))/1250 - (3528*sin(q(1)))/125;
Ge(2,1)=(73696*sin(q(1) + q(2) + q(4)))/625 + (131957*sin(q(1) + q(2)))/1250;
Ge(3,1)=- (4704*sin(q(1) + q(3) + q(5)))/625 - (24843*sin(q(1) + q(3)))/1250;
Ge(4,1)=(73696*sin(q(1) + q(2) + q(4)))/625;
Ge(5,1)=-(4704*sin(q(1) + q(3) + q(5)))/625;
Ge(6,1)=0;
Ge(7,1)=1568/5;
jacF(1,1)=0;
jacF(1,2)=0;
jacF(1,3)=0;
jacF(1,4)=0;
jacF(1,5)=0;
jacF(1,6)=1;
jacF(1,7)=0;
jacF(2,1)=0;
jacF(2,2)=0;
jacF(2,3)=0;
jacF(2,4)=0;
jacF(2,5)=0;
jacF(2,6)=0;
jacF(2,7)=1;
djacF(1,1)=0;
djacF(1,2)=0;
djacF(1,3)=0;
djacF(1,4)=0;
djacF(1,5)=0;
djacF(1,6)=0;
djacF(1,7)=0;
djacF(2,1)=0;
djacF(2,2)=0;
djacF(2,3)=0;
djacF(2,4)=0;
djacF(2,5)=0;
djacF(2,6)=0;
djacF(2,7)=0;