-
Notifications
You must be signed in to change notification settings - Fork 1
/
C_q.m
69 lines (66 loc) · 5.85 KB
/
C_q.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
function [C] = C_q(q,dq)
C(1,1)=dq(4)*((512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) - q(3) + q(4)))/6250 +...
(8*sin(q(2) + q(4)))/5 - (34029*sin(q(4)))/6250) - dq(5)*((512*sin(q(2) - q(3) + q(4) - q(5)))/3125 -...
(512*sin(q(3) - q(2) + q(5)))/3125 + (512*sin(q(5)))/3125) - dq(3)*((5971*sin(q(2) - q(3)))/6250 + (512*...
sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) - q(3) + q(4)))/6250 - (512*sin(q(3) - q(2) +...
q(5)))/3125) + dq(2)*((5971*sin(q(2) - q(3)))/6250 + (512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) -...
q(3) + q(4)))/6250 - (512*sin(q(3) - q(2) + q(5)))/3125 + (8*sin(q(2) + q(4)))/5 + (8*sin(q(2)))/5);
C(1,2)=dq(4)*((512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) - q(3) + q(4)))/6250 +...
(8*sin(q(2) + q(4)))/5 - (34029*sin(q(4)))/6250) + dq(1)*((5971*sin(q(2) - q(3)))/6250 + (512*...
sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) - q(3) + q(4)))/6250 - (512*sin(q(3) - q(2) +...
q(5)))/3125 + (8*sin(q(2) + q(4)))/5 + (8*sin(q(2)))/5) + dq(2)*((5971*sin(q(2) - q(3)))/6250 + (512*sin(q(2) -...
q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) - q(3) + q(4)))/6250 - (512*sin(q(3) - q(2) + q(5)))/3125 + (8*...
sin(q(2) + q(4)))/5 + (8*sin(q(2)))/5);
C(1,3)=- dq(5)*((512*sin(q(2) - q(3) + q(4) - q(5)))/3125 - (512*sin(q(3) - q(2) + q(5)))/3125 +...
(512*sin(q(5)))/3125) - dq(1)*((5971*sin(q(2) - q(3)))/6250 + (512*sin(q(2) - q(3) + q(4) - q(5)))/3125 +...
(5971*sin(q(2) - q(3) + q(4)))/6250 - (512*sin(q(3) - q(2) + q(5)))/3125) - dq(3)*((5971*sin(q(2) -...
q(3)))/6250 + (512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) - q(3) + q(4)))/6250 - (512*sin(q(3) -...
q(2) + q(5)))/3125);
C(1,4)=((dq(1) + dq(2) + dq(4))*(1024*sin(q(2) - q(3) + q(4) - q(5)) + 5971*sin(q(2) - q(3) +...
q(4)) + 10000*sin(q(2) + q(4)) - 34029*sin(q(4))))/6250;
C(1,5)=-(512*(sin(q(2) - q(3) + q(4) - q(5)) - sin(q(3) - q(2) + q(5)) + sin(q(5)))*(dq(1) +...
dq(3) + dq(5)))/3125;
C(2,1)=(512*dq(1)*sin(q(3) - q(2) + q(5)))/3125 - (512*dq(3)*sin(q(2) - q(3) + q(4) -...
q(5)))/3125 - (512*dq(5)*sin(q(2) - q(3) + q(4) - q(5)))/3125 - (5971*dq(1)*sin(q(2) - q(3) + q(4)))/6250 - (512*...
dq(1)*sin(q(2) - q(3) + q(4) - q(5)))/3125 - (5971*dq(3)*sin(q(2) - q(3) + q(4)))/6250 + (512*dq(3)*...
sin(q(3) - q(2) + q(5)))/3125 + (512*dq(5)*sin(q(3) - q(2) + q(5)))/3125 - (8*dq(1)*sin(q(2) + q(4)))/5 - (8*...
dq(1)*sin(q(2)))/5 - (34029*dq(4)*sin(q(4)))/6250 - (5971*dq(1)*sin(q(2) - q(3)))/6250 - (5971*dq(3)*...
sin(q(2) - q(3)))/6250;
C(2,2)=-(34029*dq(4)*sin(q(4)))/6250;
C(2,3)=- dq(1)*((5971*sin(q(2) - q(3)))/6250 + (512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*...
sin(q(2) - q(3) + q(4)))/6250 - (512*sin(q(3) - q(2) + q(5)))/3125) - dq(3)*((5971*sin(q(2) - q(3)))/6250 +...
(512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) - q(3) + q(4)))/6250 - (512*sin(q(3) - q(2) +...
q(5)))/3125) - dq(5)*((512*sin(q(2) - q(3) + q(4) - q(5)))/3125 - (512*sin(q(3) - q(2) + q(5)))/3125);
C(2,4)=-(34029*sin(q(4))*(dq(1) + dq(2) + dq(4)))/6250;
C(2,5)=-(512*(sin(q(2) - q(3) + q(4) - q(5)) - sin(q(3) - q(2) + q(5)))*(dq(1) + dq(3) + dq(5)))/3125;
C(3,1)=dq(1)*((5971*sin(q(2) - q(3)))/6250 + (512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*...
sin(q(2) - q(3) + q(4)))/6250 - (512*sin(q(3) - q(2) + q(5)))/3125) + dq(2)*((5971*sin(q(2) - q(3)))/6250 +...
(512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) - q(3) + q(4)))/6250 - (512*sin(q(3) - q(2) +...
q(5)))/3125) - (512*dq(5)*sin(q(5)))/3125 + dq(4)*((512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) -...
q(3) + q(4)))/6250);
C(3,2)=dq(1)*((5971*sin(q(2) - q(3)))/6250 + (512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*...
sin(q(2) - q(3) + q(4)))/6250 - (512*sin(q(3) - q(2) + q(5)))/3125) + dq(2)*((5971*sin(q(2) - q(3)))/6250 +...
(512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) - q(3) + q(4)))/6250 - (512*sin(q(3) - q(2) +...
q(5)))/3125) + dq(4)*((512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) - q(3) + q(4)))/6250);
C(3,3)=-(512*dq(5)*sin(q(5)))/3125;
C(3,4)=((1024*sin(q(2) - q(3) + q(4) - q(5)) + 5971*sin(q(2) - q(3) + q(4)))*(dq(1) + dq(2) + dq(4)))/6250;
C(3,5)=-(512*sin(q(5))*(dq(1) + dq(3) + dq(5)))/3125;
C(4,1)=(34029*dq(2)*sin(q(4)))/6250 - (512*dq(5)*sin(q(2) - q(3) + q(4) - q(5)))/3125 - dq(1)*...
((512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) - q(3) + q(4)))/6250 + (8*sin(q(2) +...
q(4)))/5 - (34029*sin(q(4)))/6250) - dq(3)*((512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*sin(q(2) -...
q(3) + q(4)))/6250);
C(4,2)=(34029*sin(q(4))*(dq(1) + dq(2)))/6250;
C(4,3)=- (512*dq(5)*sin(q(2) - q(3) + q(4) - q(5)))/3125 - dq(1)*((512*sin(q(2) - q(3) + q(4) -...
q(5)))/3125 + (5971*sin(q(2) - q(3) + q(4)))/6250) - dq(3)*((512*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (5971*...
sin(q(2) - q(3) + q(4)))/6250);
C(4,4)=0;
C(4,5)=-(512*sin(q(2) - q(3) + q(4) - q(5))*(dq(1) + dq(3) + dq(5)))/3125;
C(5,1)=dq(1)*((512*sin(q(2) - q(3) + q(4) - q(5)))/3125 - (512*sin(q(3) - q(2) + q(5)))/3125 +...
(512*sin(q(5)))/3125) + (512*dq(4)*sin(q(2) - q(3) + q(4) - q(5)))/3125 + (512*dq(3)*sin(q(5)))/3125 +...
dq(2)*((512*sin(q(2) - q(3) + q(4) - q(5)))/3125 - (512*sin(q(3) - q(2) + q(5)))/3125);
C(5,2)=(512*dq(4)*sin(q(2) - q(3) + q(4) - q(5)))/3125 + dq(1)*((512*sin(q(2) - q(3) + q(4) -...
q(5)))/3125 - (512*sin(q(3) - q(2) + q(5)))/3125) + dq(2)*((512*sin(q(2) - q(3) + q(4) - q(5)))/3125 - (512*...
sin(q(3) - q(2) + q(5)))/3125);
C(5,3)=(512*sin(q(5))*(dq(1) + dq(3)))/3125;
C(5,4)=(512*sin(q(2) - q(3) + q(4) - q(5))*(dq(1) + dq(2) + dq(4)))/3125;
C(5,5)=0;