-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathfcn_stance_dynamics5.m
144 lines (137 loc) · 10.2 KB
/
fcn_stance_dynamics5.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
function [fx,gx,gforce] = fcn_stance_dynamics5(q)
dq=[q(6); q(7); q(8); q(9); q(10)];
G(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;
G(2,1)=(73696*sin(q(1) + q(2) + q(4)))/625 + (131957*sin(q(1) + q(2)))/1250;
G(3,1)=- (4704*sin(q(1) + q(3) + q(5)))/625 - (24843*sin(q(1) + q(3)))/1250;
G(4,1)=(73696*sin(q(1) + q(2) + q(4)))/625;
G(5,1)=-(4704*sin(q(1) + q(3) + q(5)))/625;
D(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;
D(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;
D(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;
D(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;
D(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;
D(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;
D(2,2)=(5386*cos(q(4)))/625 + 47259/5000;
D(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;
D(2,4)=(2693*cos(q(4)))/625 + 15281/3125;
D(2,5)=- (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (192*cos(q(2) - q(3) - q(5)))/625;
D(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;
D(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;
D(3,3)=(384*cos(q(5)))/625 + 7243/5000;
D(3,4)=- (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (507*cos(q(2) - q(3) + q(4)))/625;
D(3,5)=(192*cos(q(5)))/625 + 1201/3125;
D(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;
D(4,2)=(2693*cos(q(4)))/625 + 15281/3125;
D(4,3)=- (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (507*cos(q(2) - q(3) + q(4)))/625;
D(4,4)=15281/3125;
D(4,5)=-(192*cos(q(2) - q(3) + q(4) - q(5)))/625;
D(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;
D(5,2)=- (192*cos(q(2) - q(3) + q(4) - q(5)))/625 - (192*cos(q(2) - q(3) - q(5)))/625;
D(5,3)=(192*cos(q(5)))/625 + 1201/3125;
D(5,4)=-(192*cos(q(2) - q(3) + q(4) - q(5)))/625;
D(5,5)=1201/3125;
C(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);
C(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);
C(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);
C(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;
C(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;
C(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;
C(2,2)=-(2693*dq(4)*sin(q(4)))/625;
C(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);
C(2,4)=-(2693*sin(q(4))*(dq(1) + dq(2) + dq(4)))/625;
C(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;
C(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);
C(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);
C(3,3)=-(192*dq(5)*sin(q(5)))/625;
C(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;
C(3,5)=-(192*sin(q(5))*(dq(1) + dq(3) + dq(5)))/625;
C(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);
C(4,2)=(2693*sin(q(4))*(dq(1) + dq(2)))/625;
C(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);
C(4,4)=0;
C(4,5)=-(192*sin(q(2) - q(3) + q(4) - q(5))*(dq(1) + dq(3) + dq(5)))/625;
C(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;
C(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);
C(5,3)=(192*sin(q(5))*(dq(1) + dq(3)))/625;
C(5,4)=(192*sin(q(2) - q(3) + q(4) - q(5))*(dq(1) + dq(2) + dq(4)))/625;
C(5,5)=0;
B=[zeros(1,4);eye(4)];
% F applied at the hip
% J(1,1)=- (2*cos(q(1) + q(2) + q(4)))/5 - (2*cos(q(1) + q(2)))/5;
% J(1,2)=- (2*cos(q(1) + q(2) + q(4)))/5 - (2*cos(q(1) + q(2)))/5;
% J(1,3)=0;
% J(1,4)=-(2*cos(q(1) + q(2) + q(4)))/5;
% J(1,5)=0;
% J(2,1)=(2*sin(q(1) + q(2) + q(4)))/5 + (2*sin(q(1) + q(2)))/5;
% J(2,2)=(2*sin(q(1) + q(2) + q(4)))/5 + (2*sin(q(1) + q(2)))/5;
% J(2,3)=0;
% J(2,4)=(2*sin(q(1) + q(2) + q(4)))/5;
% J(2,5)=0;
%F applied at the head
J(1,1)=(63*cos(q(1)))/100 - (2*cos(q(1) + q(2)))/5 - (2*cos(q(1) + q(2) + q(4)))/5;
J(1,2)=- (2*cos(q(1) + q(2) + q(4)))/5 - (2*cos(q(1) + q(2)))/5;
J(1,3)=0;
J(1,4)=-(2*cos(q(1) + q(2) + q(4)))/5;
J(1,5)=0;
J(2,1)=(2*sin(q(1) + q(2) + q(4)))/5 + (2*sin(q(1) + q(2)))/5 - (63*sin(q(1)))/100;
J(2,2)=(2*sin(q(1) + q(2) + q(4)))/5 + (2*sin(q(1) + q(2)))/5;
J(2,3)=0;
J(2,4)=(2*sin(q(1) + q(2) + q(4)))/5;
J(2,5)=0;
%x=[q(1);q(2);q(3);q(4);q(5)];
% ratio=(sin(q(1)+q(2)+q(4))+sin(q(1)+q(2)))/(cos(q(1)+q(2)+q(4))+cos(q(1)+q(2)));
% force=[ratio ; 1];
% J'*force;
fx=[dq;D\(-C*dq-G)]; % f is 10*1 function of q and qdot
gx=[zeros(5,4);D\B]; %g is 10*4 only function of q
gforce=[zeros(5,2);D\J'];% 10*2 only function of q
end