-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathfcn_cm_kinematics.m
116 lines (110 loc) · 12.8 KB
/
fcn_cm_kinematics.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
function [cm,v_cm,a_cm] = fcn_cm_kinematics(q)
% ,dq,ddq
xo=0;yo=0;dxo=0;dyo=0;ddxo=0;ddyo=0;
g=9.8;lT=0.63;lf=0.4;lt=0.4;MT=12;Mf=6.8;Mt=3.2;IT=1.33;If=0.47;It=0.2;pMT=0.24;pMf=0.11;pMt=0.24;
% % a_cm(1,1)=ddxo - dq(5)*((Mt*dq(1)*pMt*sin(q(1) + q(3) + q(5)))/(MT + 2*Mf + 2*Mt) + (Mt*dq(3)*pMt*...
% % sin(q(1) + q(3) + q(5)))/(MT + 2*Mf + 2*Mt) + (Mt*dq(5)*pMt*sin(q(1) + q(3) + q(5)))/(MT + 2*Mf + 2*...
% % Mt)) - dq(1)*((dq(4)*(MT*lt*sin(- q(1) - q(2) - q(4)) + 2*Mf*lt*sin(- q(1) - q(2) - q(4)) + Mt*lt*...
% % sin(- q(1) - q(2) - q(4)) + Mt*sin(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) + (dq(1)*(MT*(lt*...
% % sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2)) + pMT*sin(q(1))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) + sin(- q(1) - q(2))*...
% % (lf - pMf)) + Mt*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2)) + lf*sin(q(1) + q(3)) + pMt*sin(q(1) +...
% % q(3) + q(5))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2)) + pMf*sin(q(1) + q(3))) + Mt*...
% % sin(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) + (dq(2)*(MT*(lt*sin(- q(1) - q(2) - q(4)) + lf*...
% % sin(- q(1) - q(2))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2))) + Mt*(lt*sin(- q(1) - q(2) - q(4)) + lf*...
% % sin(- q(1) - q(2))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) + sin(- q(1) - q(2))*(lf - pMf)) + Mt*sin(- q(1) - q(2) - q(4))*...
% % (lt - pMt)))/(MT + 2*Mf + 2*Mt) + (dq(3)*(Mt*(lf*sin(q(1) + q(3)) + pMt*sin(q(1) + q(3) + q(5))) + Mf*pMf*sin(q(1) +...
% % q(3))))/(MT + 2*Mf + 2*Mt) + (Mt*dq(5)*pMt*sin(q(1) + q(3) + q(5)))/(MT + 2*Mf + 2*Mt)) - dq(4)*((dq(1)*(MT*lt*...
% % sin(- q(1) - q(2) - q(4)) + 2*Mf*lt*sin(- q(1) - q(2) - q(4)) + Mt*lt*sin(- q(1) - q(2) - q(4)) + Mt*sin(- q(1) - q(2) - q(4))*...
% % (lt - pMt)))/(MT + 2*Mf + 2*Mt) + (dq(2)*(MT*lt*sin(- q(1) - q(2) - q(4)) + 2*Mf*lt*sin(- q(1) - q(2) - q(4)) + Mt*lt*...
% % sin(- q(1) - q(2) - q(4)) + Mt*sin(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) + (dq(4)*(MT*lt*...
% % sin(- q(1) - q(2) - q(4)) + 2*Mf*lt*sin(- q(1) - q(2) - q(4)) + Mt*lt*sin(- q(1) - q(2) - q(4)) + Mt*sin(- q(1) - q(2) - q(4))*...
% % (lt - pMt)))/(MT + 2*Mf + 2*Mt)) - dq(2)*((dq(4)*(MT*lt*sin(- q(1) - q(2) - q(4)) + 2*Mf*lt*...
% % sin(- q(1) - q(2) - q(4)) + Mt*lt*sin(- q(1) - q(2) - q(4)) + Mt*sin(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) +...
% % (dq(1)*(MT*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) + lf*...
% % sin(- q(1) - q(2))) + Mt*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) +...
% % sin(- q(1) - q(2))*(lf - pMf)) + Mt*sin(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) + (dq(2)*(MT*(lt*...
% % sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2))) + Mt*(lt*...
% % sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) + sin(- q(1) - q(2))*(lf - pMf)) + Mt*...
% % sin(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt)) - dq(3)*((dq(1)*(Mt*(lf*sin(q(1) + q(3)) + pMt*sin(q(1) + q(3) +...
% % q(5))) + Mf*pMf*sin(q(1) + q(3))))/(MT + 2*Mf + 2*Mt) + (dq(3)*(Mt*(lf*sin(q(1) + q(3)) + pMt*sin(q(1) +...
% % q(3) + q(5))) + Mf*pMf*sin(q(1) + q(3))))/(MT + 2*Mf + 2*Mt) + (Mt*dq(5)*pMt*sin(q(1) + q(3) +...
% % q(5)))/(MT + 2*Mf + 2*Mt)) - (ddq(2)*(MT*(lt*cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mf*(lt*...
% % cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mt*(lt*cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mf*(lt*...
% % cos(- q(1) - q(2) - q(4)) + cos(- q(1) - q(2))*(lf - pMf)) + Mt*cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*...
% % Mt) - (ddq(1)*(Mf*(lt*cos(- q(1) - q(2) - q(4)) + cos(- q(1) - q(2))*(lf - pMf)) + Mt*(lt*...
% % cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2)) - lf*cos(q(1) + q(3)) - pMt*cos(q(1) + q(3) + q(5))) + Mf*(lt*...
% % cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2)) - pMf*cos(q(1) + q(3))) + MT*(lt*cos(- q(1) - q(2) - q(4)) + lf*...
% % cos(- q(1) - q(2)) - pMT*cos(q(1))) + Mt*cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) + (ddq(3)*(Mt*(lf*...
% % cos(q(1) + q(3)) + pMt*cos(q(1) + q(3) + q(5))) + Mf*pMf*cos(q(1) + q(3))))/(MT + 2*Mf + 2*Mt) - (ddq(4)*(MT*...
% % lt*cos(- q(1) - q(2) - q(4)) + 2*Mf*lt*cos(- q(1) - q(2) - q(4)) + Mt*lt*cos(- q(1) - q(2) - q(4)) +...
% % Mt*cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) + (Mt*ddq(5)*pMt*cos(q(1) + q(3) +...
% % q(5)))/(MT + 2*Mf + 2*Mt);
% % a_cm(2,1)=ddyo - dq(5)*((Mt*dq(1)*pMt*cos(q(1) + q(3) + q(5)))/(MT + 2*Mf + 2*Mt) + (Mt*dq(3)*pMt*...
% % cos(q(1) + q(3) + q(5)))/(MT + 2*Mf + 2*Mt) + (Mt*dq(5)*pMt*cos(q(1) + q(3) + q(5)))/(MT + 2*Mf + 2*Mt)) +...
% % dq(2)*((dq(1)*(MT*(lt*cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mf*(lt*...
% % cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mt*(lt*cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mf*(lt*...
% % cos(- q(1) - q(2) - q(4)) + cos(- q(1) - q(2))*(lf - pMf)) + Mt*cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) +...
% % (dq(2)*(MT*(lt*cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mf*(lt*cos(- q(1) - q(2) - q(4)) + lf*...
% % cos(- q(1) - q(2))) + Mt*(lt*cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mf*(lt*cos(- q(1) - q(2) - q(4)) +...
% % cos(- q(1) - q(2))*(lf - pMf)) + Mt*cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) + (dq(4)*(MT*lt*...
% % cos(- q(1) - q(2) - q(4)) + 2*Mf*lt*cos(- q(1) - q(2) - q(4)) + Mt*lt*cos(- q(1) - q(2) - q(4)) + Mt*cos(- q(1) - q(2) - q(4))*...
% % (lt - pMt)))/(MT + 2*Mf + 2*Mt)) - dq(3)*((dq(1)*(Mt*(lf*cos(q(1) + q(3)) + pMt*cos(q(1) + q(3) + q(5))) + Mf*pMf*...
% % cos(q(1) + q(3))))/(MT + 2*Mf + 2*Mt) + (dq(3)*(Mt*(lf*cos(q(1) + q(3)) + pMt*cos(q(1) + q(3) + q(5))) + Mf*...
% % pMf*cos(q(1) + q(3))))/(MT + 2*Mf + 2*Mt) + (Mt*dq(5)*pMt*cos(q(1) + q(3) + q(5)))/(MT + 2*Mf + 2*Mt)) +...
% % dq(1)*((dq(2)*(MT*(lt*cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mf*(lt*...
% % cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mt*(lt*cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mf*(lt*...
% % cos(- q(1) - q(2) - q(4)) + cos(- q(1) - q(2))*(lf - pMf)) + Mt*cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) +...
% % (dq(1)*(Mf*(lt*cos(- q(1) - q(2) - q(4)) + cos(- q(1) - q(2))*(lf - pMf)) + Mt*(lt*...
% % cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2)) - lf*cos(q(1) + q(3)) - pMt*cos(q(1) + q(3) + q(5))) + Mf*(lt*...
% % cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2)) - pMf*cos(q(1) + q(3))) + MT*(lt*cos(- q(1) - q(2) - q(4)) + lf*...
% % cos(- q(1) - q(2)) - pMT*cos(q(1))) + Mt*cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) - (dq(3)*(Mt*(lf*...
% % cos(q(1) + q(3)) + pMt*cos(q(1) + q(3) + q(5))) + Mf*pMf*cos(q(1) + q(3))))/(MT + 2*Mf + 2*Mt) + (dq(4)*(MT*...
% % lt*cos(- q(1) - q(2) - q(4)) + 2*Mf*lt*cos(- q(1) - q(2) - q(4)) + Mt*lt*cos(- q(1) - q(2) - q(4)) +...
% % Mt*cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) - (Mt*dq(5)*pMt*cos(q(1) + q(3) +...
% % q(5)))/(MT + 2*Mf + 2*Mt)) + dq(4)*((dq(1)*(MT*lt*cos(- q(1) - q(2) - q(4)) + 2*Mf*lt*...
% % cos(- q(1) - q(2) - q(4)) + Mt*lt*cos(- q(1) - q(2) - q(4)) + Mt*cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) +...
% % (dq(2)*(MT*lt*cos(- q(1) - q(2) - q(4)) + 2*Mf*lt*cos(- q(1) - q(2) - q(4)) + Mt*lt*...
% % cos(- q(1) - q(2) - q(4)) + Mt*cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) + (dq(4)*(MT*lt*...
% % cos(- q(1) - q(2) - q(4)) + 2*Mf*lt*cos(- q(1) - q(2) - q(4)) + Mt*lt*cos(- q(1) - q(2) - q(4)) + Mt*cos(- q(1) - q(2) - q(4))*...
% % (lt - pMt)))/(MT + 2*Mf + 2*Mt)) - (ddq(4)*(MT*lt*sin(- q(1) - q(2) - q(4)) + 2*Mf*lt*sin(- q(1) - q(2) - q(4)) + Mt*...
% % lt*sin(- q(1) - q(2) - q(4)) + Mt*sin(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) - (ddq(1)*...
% % (MT*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2)) + pMT*sin(q(1))) + Mf*(lt*...
% % sin(- q(1) - q(2) - q(4)) + sin(- q(1) - q(2))*(lf - pMf)) + Mt*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2)) + lf*...
% % sin(q(1) + q(3)) + pMt*sin(q(1) + q(3) + q(5))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2)) +...
% % pMf*sin(q(1) + q(3))) + Mt*sin(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) - (ddq(2)*(MT*(lt*...
% % sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2))) + Mt*(lt*...
% % sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) + sin(- q(1) - q(2))*(lf - pMf)) + Mt*...
% % sin(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) - (ddq(3)*(Mt*(lf*sin(q(1) + q(3)) + pMt*sin(q(1) + q(3) + q(5))) +...
% % Mf*pMf*sin(q(1) + q(3))))/(MT + 2*Mf + 2*Mt) - (Mt*ddq(5)*pMt*sin(q(1) + q(3) + q(5)))/(MT + 2*Mf + 2*Mt);
% %
% % v_cm(1,1)=dxo - (dq(2)*(MT*(lt*cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mf*(lt*...
% % cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mt*(lt*cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2))) + Mf*(lt*...
% % cos(- q(1) - q(2) - q(4)) + cos(- q(1) - q(2))*(lf - pMf)) + Mt*cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*...
% % Mt) - (dq(1)*(Mf*(lt*cos(- q(1) - q(2) - q(4)) + cos(- q(1) - q(2))*(lf - pMf)) + Mt*(lt*...
% % cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2)) - lf*cos(q(1) + q(3)) - pMt*cos(q(1) + q(3) + q(5))) + Mf*(lt*...
% % cos(- q(1) - q(2) - q(4)) + lf*cos(- q(1) - q(2)) - pMf*cos(q(1) + q(3))) + MT*(lt*cos(- q(1) - q(2) - q(4)) + lf*...
% % cos(- q(1) - q(2)) - pMT*cos(q(1))) + Mt*cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) + (dq(3)*(Mt*(lf*...
% % cos(q(1) + q(3)) + pMt*cos(q(1) + q(3) + q(5))) + Mf*pMf*cos(q(1) + q(3))))/(MT + 2*Mf + 2*Mt) - (dq(4)*(MT*...
% % lt*cos(- q(1) - q(2) - q(4)) + 2*Mf*lt*cos(- q(1) - q(2) - q(4)) + Mt*lt*cos(- q(1) - q(2) - q(4)) +...
% % Mt*cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) + (Mt*dq(5)*pMt*cos(q(1) + q(3) +...
% % q(5)))/(MT + 2*Mf + 2*Mt);
% % v_cm(2,1)=dyo - (dq(4)*(MT*lt*sin(- q(1) - q(2) - q(4)) + 2*Mf*lt*sin(- q(1) - q(2) - q(4)) + Mt*...
% % lt*sin(- q(1) - q(2) - q(4)) + Mt*sin(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) - (dq(1)*...
% % (MT*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2)) + pMT*sin(q(1))) + Mf*(lt*...
% % sin(- q(1) - q(2) - q(4)) + sin(- q(1) - q(2))*(lf - pMf)) + Mt*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2)) + lf*...
% % sin(q(1) + q(3)) + pMt*sin(q(1) + q(3) + q(5))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2)) +...
% % pMf*sin(q(1) + q(3))) + Mt*sin(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) - (dq(2)*(MT*(lt*...
% % sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2))) + Mt*(lt*...
% % sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2))) + Mf*(lt*sin(- q(1) - q(2) - q(4)) + sin(- q(1) - q(2))*(lf - pMf)) + Mt*...
% % sin(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt) - (dq(3)*(Mt*(lf*sin(q(1) + q(3)) + pMt*sin(q(1) + q(3) + q(5))) +...
% % Mf*pMf*sin(q(1) + q(3))))/(MT + 2*Mf + 2*Mt) - (Mt*dq(5)*pMt*sin(q(1) + q(3) + q(5)))/(MT + 2*Mf + 2*Mt);
cm(1,1)=(Mf*(xo + lt*sin(- q(1) - q(2) - q(4)) + sin(- q(1) - q(2))*(lf - pMf)) + Mt*(xo + lt*...
sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2)) + lf*sin(q(1) + q(3)) + pMt*sin(q(1) + q(3) + q(5))) + Mf*(xo + lt*...
sin(- q(1) - q(2) - q(4)) + lf*sin(- q(1) - q(2)) + pMf*sin(q(1) + q(3))) + MT*(xo + lt*sin(- q(1) - q(2) - q(4)) + lf*...
sin(- q(1) - q(2)) + pMT*sin(q(1))) + Mt*(xo + sin(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf + 2*Mt);
cm(2,1)=(Mt*(yo - lt*cos(- q(1) - q(2) - q(4)) - lf*cos(- q(1) - q(2)) + lf*cos(q(1) + q(3)) +...
pMt*cos(q(1) + q(3) + q(5))) + Mf*(yo - lt*cos(- q(1) - q(2) - q(4)) - lf*cos(- q(1) - q(2)) + pMf*...
cos(q(1) + q(3))) - Mf*(lt*cos(- q(1) - q(2) - q(4)) - yo + cos(- q(1) - q(2))*(lf - pMf)) + MT*(yo - lt*...
cos(- q(1) - q(2) - q(4)) - lf*cos(- q(1) - q(2)) + pMT*cos(q(1))) + Mt*(yo - cos(- q(1) - q(2) - q(4))*(lt - pMt)))/(MT + 2*Mf +...
2*Mt);
end