forked from quan-dao/snake-robot-velocity-ctrl
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathode_snakeRob.m
executable file
·66 lines (47 loc) · 18.6 KB
/
ode_snakeRob.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
function f=ode_snakeRob(t,inp)
theta1=inp(1); dtheta1=inp(2);
theta2=inp(3); dtheta2=inp(4);
theta3=inp(5); dtheta3=inp(6);
theta4=inp(7); dtheta4=inp(8);
theta5=inp(9); dtheta5=inp(10);
theta6=inp(11); dtheta6=inp(12);
theta7=inp(13); dtheta7=inp(14);
thetaN=inp(15); dthetaN=inp(16);
px=inp(17); dpx=inp(18);
py=inp(19); dpy=inp(20);
theta8=thetaN; dtheta8=dthetaN;
theta=[theta1;theta2;theta3;theta4;theta5;theta6;theta7;thetaN];
dtheta=[dtheta1;dtheta2;dtheta3;dtheta4;dtheta5;dtheta6;dtheta7;dthetaN];
global N m l J ct cn
Mr1=[ m*((7*cos(theta1)^2)/8 + (7*sin(theta1)^2)/8)*l^2 + J, l^2*m*((13*cos(theta1)*cos(theta2))/8 + (13*sin(theta1)*sin(theta2))/8), l^2*m*((11*cos(theta1)*cos(theta3))/8 + (11*sin(theta1)*sin(theta3))/8), l^2*m*((9*cos(theta1)*cos(theta4))/8 + (9*sin(theta1)*sin(theta4))/8), l^2*m*((7*cos(theta1)*cos(theta5))/8 + (7*sin(theta1)*sin(theta5))/8), l^2*m*((5*cos(theta1)*cos(theta6))/8 + (5*sin(theta1)*sin(theta6))/8), l^2*m*((3*cos(theta1)*cos(theta7))/8 + (3*sin(theta1)*sin(theta7))/8), l^2*m*((cos(theta1)*cos(theta8))/8 + (sin(theta1)*sin(theta8))/8)];
Mr2=[ l^2*m*((13*cos(theta1)*cos(theta2))/8 + (13*sin(theta1)*sin(theta2))/8), m*((31*cos(theta2)^2)/8 + (31*sin(theta2)^2)/8)*l^2 + J, l^2*m*((33*cos(theta2)*cos(theta3))/8 + (33*sin(theta2)*sin(theta3))/8), l^2*m*((27*cos(theta2)*cos(theta4))/8 + (27*sin(theta2)*sin(theta4))/8), l^2*m*((21*cos(theta2)*cos(theta5))/8 + (21*sin(theta2)*sin(theta5))/8), l^2*m*((15*cos(theta2)*cos(theta6))/8 + (15*sin(theta2)*sin(theta6))/8), l^2*m*((9*cos(theta2)*cos(theta7))/8 + (9*sin(theta2)*sin(theta7))/8), l^2*m*((3*cos(theta2)*cos(theta8))/8 + (3*sin(theta2)*sin(theta8))/8)];
Mr3=[ l^2*m*((11*cos(theta1)*cos(theta3))/8 + (11*sin(theta1)*sin(theta3))/8), l^2*m*((33*cos(theta2)*cos(theta3))/8 + (33*sin(theta2)*sin(theta3))/8), m*((47*cos(theta3)^2)/8 + (47*sin(theta3)^2)/8)*l^2 + J, l^2*m*((45*cos(theta3)*cos(theta4))/8 + (45*sin(theta3)*sin(theta4))/8), l^2*m*((35*cos(theta3)*cos(theta5))/8 + (35*sin(theta3)*sin(theta5))/8), l^2*m*((25*cos(theta3)*cos(theta6))/8 + (25*sin(theta3)*sin(theta6))/8), l^2*m*((15*cos(theta3)*cos(theta7))/8 + (15*sin(theta3)*sin(theta7))/8), l^2*m*((5*cos(theta3)*cos(theta8))/8 + (5*sin(theta3)*sin(theta8))/8)];
Mr4=[ l^2*m*((9*cos(theta1)*cos(theta4))/8 + (9*sin(theta1)*sin(theta4))/8), l^2*m*((27*cos(theta2)*cos(theta4))/8 + (27*sin(theta2)*sin(theta4))/8), l^2*m*((45*cos(theta3)*cos(theta4))/8 + (45*sin(theta3)*sin(theta4))/8), m*((55*cos(theta4)^2)/8 + (55*sin(theta4)^2)/8)*l^2 + J, l^2*m*((49*cos(theta4)*cos(theta5))/8 + (49*sin(theta4)*sin(theta5))/8), l^2*m*((35*cos(theta4)*cos(theta6))/8 + (35*sin(theta4)*sin(theta6))/8), l^2*m*((21*cos(theta4)*cos(theta7))/8 + (21*sin(theta4)*sin(theta7))/8), l^2*m*((7*cos(theta4)*cos(theta8))/8 + (7*sin(theta4)*sin(theta8))/8)];
Mr5=[ l^2*m*((7*cos(theta1)*cos(theta5))/8 + (7*sin(theta1)*sin(theta5))/8), l^2*m*((21*cos(theta2)*cos(theta5))/8 + (21*sin(theta2)*sin(theta5))/8), l^2*m*((35*cos(theta3)*cos(theta5))/8 + (35*sin(theta3)*sin(theta5))/8), l^2*m*((49*cos(theta4)*cos(theta5))/8 + (49*sin(theta4)*sin(theta5))/8), m*((55*cos(theta5)^2)/8 + (55*sin(theta5)^2)/8)*l^2 + J, l^2*m*((45*cos(theta5)*cos(theta6))/8 + (45*sin(theta5)*sin(theta6))/8), l^2*m*((27*cos(theta5)*cos(theta7))/8 + (27*sin(theta5)*sin(theta7))/8), l^2*m*((9*cos(theta5)*cos(theta8))/8 + (9*sin(theta5)*sin(theta8))/8)];
Mr6=[ l^2*m*((5*cos(theta1)*cos(theta6))/8 + (5*sin(theta1)*sin(theta6))/8), l^2*m*((15*cos(theta2)*cos(theta6))/8 + (15*sin(theta2)*sin(theta6))/8), l^2*m*((25*cos(theta3)*cos(theta6))/8 + (25*sin(theta3)*sin(theta6))/8), l^2*m*((35*cos(theta4)*cos(theta6))/8 + (35*sin(theta4)*sin(theta6))/8), l^2*m*((45*cos(theta5)*cos(theta6))/8 + (45*sin(theta5)*sin(theta6))/8), m*((47*cos(theta6)^2)/8 + (47*sin(theta6)^2)/8)*l^2 + J, l^2*m*((33*cos(theta6)*cos(theta7))/8 + (33*sin(theta6)*sin(theta7))/8), l^2*m*((11*cos(theta6)*cos(theta8))/8 + (11*sin(theta6)*sin(theta8))/8)];
Mr7=[ l^2*m*((3*cos(theta1)*cos(theta7))/8 + (3*sin(theta1)*sin(theta7))/8), l^2*m*((9*cos(theta2)*cos(theta7))/8 + (9*sin(theta2)*sin(theta7))/8), l^2*m*((15*cos(theta3)*cos(theta7))/8 + (15*sin(theta3)*sin(theta7))/8), l^2*m*((21*cos(theta4)*cos(theta7))/8 + (21*sin(theta4)*sin(theta7))/8), l^2*m*((27*cos(theta5)*cos(theta7))/8 + (27*sin(theta5)*sin(theta7))/8), l^2*m*((33*cos(theta6)*cos(theta7))/8 + (33*sin(theta6)*sin(theta7))/8), m*((31*cos(theta7)^2)/8 + (31*sin(theta7)^2)/8)*l^2 + J, l^2*m*((13*cos(theta7)*cos(theta8))/8 + (13*sin(theta7)*sin(theta8))/8)];
Mr8=[ l^2*m*((cos(theta1)*cos(theta8))/8 + (sin(theta1)*sin(theta8))/8), l^2*m*((3*cos(theta2)*cos(theta8))/8 + (3*sin(theta2)*sin(theta8))/8), l^2*m*((5*cos(theta3)*cos(theta8))/8 + (5*sin(theta3)*sin(theta8))/8), l^2*m*((7*cos(theta4)*cos(theta8))/8 + (7*sin(theta4)*sin(theta8))/8), l^2*m*((9*cos(theta5)*cos(theta8))/8 + (9*sin(theta5)*sin(theta8))/8), l^2*m*((11*cos(theta6)*cos(theta8))/8 + (11*sin(theta6)*sin(theta8))/8), l^2*m*((13*cos(theta7)*cos(theta8))/8 + (13*sin(theta7)*sin(theta8))/8), m*((7*cos(theta8)^2)/8 + (7*sin(theta8)^2)/8)*l^2 + J];
matM=[Mr1;Mr2;Mr3;Mr4;Mr5;Mr6;Mr7;Mr8];
Wr1=[ 0, -l^2*m*((13*cos(theta1)*sin(theta2))/8 - (13*cos(theta2)*sin(theta1))/8), -l^2*m*((11*cos(theta1)*sin(theta3))/8 - (11*cos(theta3)*sin(theta1))/8), -l^2*m*((9*cos(theta1)*sin(theta4))/8 - (9*cos(theta4)*sin(theta1))/8), -l^2*m*((7*cos(theta1)*sin(theta5))/8 - (7*cos(theta5)*sin(theta1))/8), -l^2*m*((5*cos(theta1)*sin(theta6))/8 - (5*cos(theta6)*sin(theta1))/8), -l^2*m*((3*cos(theta1)*sin(theta7))/8 - (3*cos(theta7)*sin(theta1))/8), -l^2*m*((cos(theta1)*sin(theta8))/8 - (cos(theta8)*sin(theta1))/8)];
Wr2=[ l^2*m*((13*cos(theta1)*sin(theta2))/8 - (13*cos(theta2)*sin(theta1))/8), 0, -l^2*m*((33*cos(theta2)*sin(theta3))/8 - (33*cos(theta3)*sin(theta2))/8), -l^2*m*((27*cos(theta2)*sin(theta4))/8 - (27*cos(theta4)*sin(theta2))/8), -l^2*m*((21*cos(theta2)*sin(theta5))/8 - (21*cos(theta5)*sin(theta2))/8), -l^2*m*((15*cos(theta2)*sin(theta6))/8 - (15*cos(theta6)*sin(theta2))/8), -l^2*m*((9*cos(theta2)*sin(theta7))/8 - (9*cos(theta7)*sin(theta2))/8), -l^2*m*((3*cos(theta2)*sin(theta8))/8 - (3*cos(theta8)*sin(theta2))/8)];
Wr3=[ l^2*m*((11*cos(theta1)*sin(theta3))/8 - (11*cos(theta3)*sin(theta1))/8), l^2*m*((33*cos(theta2)*sin(theta3))/8 - (33*cos(theta3)*sin(theta2))/8), 0, -l^2*m*((45*cos(theta3)*sin(theta4))/8 - (45*cos(theta4)*sin(theta3))/8), -l^2*m*((35*cos(theta3)*sin(theta5))/8 - (35*cos(theta5)*sin(theta3))/8), -l^2*m*((25*cos(theta3)*sin(theta6))/8 - (25*cos(theta6)*sin(theta3))/8), -l^2*m*((15*cos(theta3)*sin(theta7))/8 - (15*cos(theta7)*sin(theta3))/8), -l^2*m*((5*cos(theta3)*sin(theta8))/8 - (5*cos(theta8)*sin(theta3))/8)];
Wr4=[ l^2*m*((9*cos(theta1)*sin(theta4))/8 - (9*cos(theta4)*sin(theta1))/8), l^2*m*((27*cos(theta2)*sin(theta4))/8 - (27*cos(theta4)*sin(theta2))/8), l^2*m*((45*cos(theta3)*sin(theta4))/8 - (45*cos(theta4)*sin(theta3))/8), 0, -l^2*m*((49*cos(theta4)*sin(theta5))/8 - (49*cos(theta5)*sin(theta4))/8), -l^2*m*((35*cos(theta4)*sin(theta6))/8 - (35*cos(theta6)*sin(theta4))/8), -l^2*m*((21*cos(theta4)*sin(theta7))/8 - (21*cos(theta7)*sin(theta4))/8), -l^2*m*((7*cos(theta4)*sin(theta8))/8 - (7*cos(theta8)*sin(theta4))/8)];
Wr5=[ l^2*m*((7*cos(theta1)*sin(theta5))/8 - (7*cos(theta5)*sin(theta1))/8), l^2*m*((21*cos(theta2)*sin(theta5))/8 - (21*cos(theta5)*sin(theta2))/8), l^2*m*((35*cos(theta3)*sin(theta5))/8 - (35*cos(theta5)*sin(theta3))/8), l^2*m*((49*cos(theta4)*sin(theta5))/8 - (49*cos(theta5)*sin(theta4))/8), 0, -l^2*m*((45*cos(theta5)*sin(theta6))/8 - (45*cos(theta6)*sin(theta5))/8), -l^2*m*((27*cos(theta5)*sin(theta7))/8 - (27*cos(theta7)*sin(theta5))/8), -l^2*m*((9*cos(theta5)*sin(theta8))/8 - (9*cos(theta8)*sin(theta5))/8)];
Wr6=[ l^2*m*((5*cos(theta1)*sin(theta6))/8 - (5*cos(theta6)*sin(theta1))/8), l^2*m*((15*cos(theta2)*sin(theta6))/8 - (15*cos(theta6)*sin(theta2))/8), l^2*m*((25*cos(theta3)*sin(theta6))/8 - (25*cos(theta6)*sin(theta3))/8), l^2*m*((35*cos(theta4)*sin(theta6))/8 - (35*cos(theta6)*sin(theta4))/8), l^2*m*((45*cos(theta5)*sin(theta6))/8 - (45*cos(theta6)*sin(theta5))/8), 0, -l^2*m*((33*cos(theta6)*sin(theta7))/8 - (33*cos(theta7)*sin(theta6))/8), -l^2*m*((11*cos(theta6)*sin(theta8))/8 - (11*cos(theta8)*sin(theta6))/8)];
Wr7=[ l^2*m*((3*cos(theta1)*sin(theta7))/8 - (3*cos(theta7)*sin(theta1))/8), l^2*m*((9*cos(theta2)*sin(theta7))/8 - (9*cos(theta7)*sin(theta2))/8), l^2*m*((15*cos(theta3)*sin(theta7))/8 - (15*cos(theta7)*sin(theta3))/8), l^2*m*((21*cos(theta4)*sin(theta7))/8 - (21*cos(theta7)*sin(theta4))/8), l^2*m*((27*cos(theta5)*sin(theta7))/8 - (27*cos(theta7)*sin(theta5))/8), l^2*m*((33*cos(theta6)*sin(theta7))/8 - (33*cos(theta7)*sin(theta6))/8), 0, -l^2*m*((13*cos(theta7)*sin(theta8))/8 - (13*cos(theta8)*sin(theta7))/8)];
Wr8=[ l^2*m*((cos(theta1)*sin(theta8))/8 - (cos(theta8)*sin(theta1))/8), l^2*m*((3*cos(theta2)*sin(theta8))/8 - (3*cos(theta8)*sin(theta2))/8), l^2*m*((5*cos(theta3)*sin(theta8))/8 - (5*cos(theta8)*sin(theta3))/8), l^2*m*((7*cos(theta4)*sin(theta8))/8 - (7*cos(theta8)*sin(theta4))/8), l^2*m*((9*cos(theta5)*sin(theta8))/8 - (9*cos(theta8)*sin(theta5))/8), l^2*m*((11*cos(theta6)*sin(theta8))/8 - (11*cos(theta8)*sin(theta6))/8), l^2*m*((13*cos(theta7)*sin(theta8))/8 - (13*cos(theta8)*sin(theta7))/8), 0];
matW=[Wr1;Wr2;Wr3;Wr4;Wr5;Wr6;Wr7;Wr8];
Stheta=createNumerical_matrixStheta(N,theta);
fR_x=transpose([ - (ct*cos(theta1)^2 + cn*sin(theta1)^2)*(dpx + (7*dtheta1*l*sin(theta1))/8 + (13*dtheta2*l*sin(theta2))/8 + (11*dtheta3*l*sin(theta3))/8 + (9*dtheta4*l*sin(theta4))/8 + (7*dtheta5*l*sin(theta5))/8 + (5*dtheta6*l*sin(theta6))/8 + (3*dtheta7*l*sin(theta7))/8 + (dtheta8*l*sin(theta8))/8) - cos(theta1)*sin(theta1)*(cn - ct)*((7*dtheta1*l*cos(theta1))/8 - dpy + (13*dtheta2*l*cos(theta2))/8 + (11*dtheta3*l*cos(theta3))/8 + (9*dtheta4*l*cos(theta4))/8 + (7*dtheta5*l*cos(theta5))/8 + (5*dtheta6*l*cos(theta6))/8 + (3*dtheta7*l*cos(theta7))/8 + (dtheta8*l*cos(theta8))/8), - (ct*cos(theta2)^2 + cn*sin(theta2)^2)*(dpx - (dtheta1*l*sin(theta1))/8 + (5*dtheta2*l*sin(theta2))/8 + (11*dtheta3*l*sin(theta3))/8 + (9*dtheta4*l*sin(theta4))/8 + (7*dtheta5*l*sin(theta5))/8 + (5*dtheta6*l*sin(theta6))/8 + (3*dtheta7*l*sin(theta7))/8 + (dtheta8*l*sin(theta8))/8) - cos(theta2)*sin(theta2)*(cn - ct)*((5*dtheta2*l*cos(theta2))/8 - (dtheta1*l*cos(theta1))/8 - dpy + (11*dtheta3*l*cos(theta3))/8 + (9*dtheta4*l*cos(theta4))/8 + (7*dtheta5*l*cos(theta5))/8 + (5*dtheta6*l*cos(theta6))/8 + (3*dtheta7*l*cos(theta7))/8 + (dtheta8*l*cos(theta8))/8), - (ct*cos(theta3)^2 + cn*sin(theta3)^2)*(dpx - (dtheta1*l*sin(theta1))/8 - (3*dtheta2*l*sin(theta2))/8 + (3*dtheta3*l*sin(theta3))/8 + (9*dtheta4*l*sin(theta4))/8 + (7*dtheta5*l*sin(theta5))/8 + (5*dtheta6*l*sin(theta6))/8 + (3*dtheta7*l*sin(theta7))/8 + (dtheta8*l*sin(theta8))/8) - cos(theta3)*sin(theta3)*(cn - ct)*((3*dtheta3*l*cos(theta3))/8 - (dtheta1*l*cos(theta1))/8 - (3*dtheta2*l*cos(theta2))/8 - dpy + (9*dtheta4*l*cos(theta4))/8 + (7*dtheta5*l*cos(theta5))/8 + (5*dtheta6*l*cos(theta6))/8 + (3*dtheta7*l*cos(theta7))/8 + (dtheta8*l*cos(theta8))/8), - (ct*cos(theta4)^2 + cn*sin(theta4)^2)*(dpx - (dtheta1*l*sin(theta1))/8 - (3*dtheta2*l*sin(theta2))/8 - (5*dtheta3*l*sin(theta3))/8 + (dtheta4*l*sin(theta4))/8 + (7*dtheta5*l*sin(theta5))/8 + (5*dtheta6*l*sin(theta6))/8 + (3*dtheta7*l*sin(theta7))/8 + (dtheta8*l*sin(theta8))/8) - cos(theta4)*sin(theta4)*(cn - ct)*((dtheta4*l*cos(theta4))/8 - (dtheta1*l*cos(theta1))/8 - (3*dtheta2*l*cos(theta2))/8 - (5*dtheta3*l*cos(theta3))/8 - dpy + (7*dtheta5*l*cos(theta5))/8 + (5*dtheta6*l*cos(theta6))/8 + (3*dtheta7*l*cos(theta7))/8 + (dtheta8*l*cos(theta8))/8), (ct*cos(theta5)^2 + cn*sin(theta5)^2)*((dtheta1*l*sin(theta1))/8 - dpx + (3*dtheta2*l*sin(theta2))/8 + (5*dtheta3*l*sin(theta3))/8 + (7*dtheta4*l*sin(theta4))/8 + (dtheta5*l*sin(theta5))/8 - (5*dtheta6*l*sin(theta6))/8 - (3*dtheta7*l*sin(theta7))/8 - (dtheta8*l*sin(theta8))/8) + cos(theta5)*sin(theta5)*(cn - ct)*(dpy + (dtheta1*l*cos(theta1))/8 + (3*dtheta2*l*cos(theta2))/8 + (5*dtheta3*l*cos(theta3))/8 + (7*dtheta4*l*cos(theta4))/8 + (dtheta5*l*cos(theta5))/8 - (5*dtheta6*l*cos(theta6))/8 - (3*dtheta7*l*cos(theta7))/8 - (dtheta8*l*cos(theta8))/8), (ct*cos(theta6)^2 + cn*sin(theta6)^2)*((dtheta1*l*sin(theta1))/8 - dpx + (3*dtheta2*l*sin(theta2))/8 + (5*dtheta3*l*sin(theta3))/8 + (7*dtheta4*l*sin(theta4))/8 + (9*dtheta5*l*sin(theta5))/8 + (3*dtheta6*l*sin(theta6))/8 - (3*dtheta7*l*sin(theta7))/8 - (dtheta8*l*sin(theta8))/8) + cos(theta6)*sin(theta6)*(cn - ct)*(dpy + (dtheta1*l*cos(theta1))/8 + (3*dtheta2*l*cos(theta2))/8 + (5*dtheta3*l*cos(theta3))/8 + (7*dtheta4*l*cos(theta4))/8 + (9*dtheta5*l*cos(theta5))/8 + (3*dtheta6*l*cos(theta6))/8 - (3*dtheta7*l*cos(theta7))/8 - (dtheta8*l*cos(theta8))/8), (ct*cos(theta7)^2 + cn*sin(theta7)^2)*((dtheta1*l*sin(theta1))/8 - dpx + (3*dtheta2*l*sin(theta2))/8 + (5*dtheta3*l*sin(theta3))/8 + (7*dtheta4*l*sin(theta4))/8 + (9*dtheta5*l*sin(theta5))/8 + (11*dtheta6*l*sin(theta6))/8 + (5*dtheta7*l*sin(theta7))/8 - (dtheta8*l*sin(theta8))/8) + cos(theta7)*sin(theta7)*(cn - ct)*(dpy + (dtheta1*l*cos(theta1))/8 + (3*dtheta2*l*cos(theta2))/8 + (5*dtheta3*l*cos(theta3))/8 + (7*dtheta4*l*cos(theta4))/8 + (9*dtheta5*l*cos(theta5))/8 + (11*dtheta6*l*cos(theta6))/8 + (5*dtheta7*l*cos(theta7))/8 - (dtheta8*l*cos(theta8))/8), (ct*cos(theta8)^2 + cn*sin(theta8)^2)*((dtheta1*l*sin(theta1))/8 - dpx + (3*dtheta2*l*sin(theta2))/8 + (5*dtheta3*l*sin(theta3))/8 + (7*dtheta4*l*sin(theta4))/8 + (9*dtheta5*l*sin(theta5))/8 + (11*dtheta6*l*sin(theta6))/8 + (13*dtheta7*l*sin(theta7))/8 + (7*dtheta8*l*sin(theta8))/8) + cos(theta8)*sin(theta8)*(cn - ct)*(dpy + (dtheta1*l*cos(theta1))/8 + (3*dtheta2*l*cos(theta2))/8 + (5*dtheta3*l*cos(theta3))/8 + (7*dtheta4*l*cos(theta4))/8 + (9*dtheta5*l*cos(theta5))/8 + (11*dtheta6*l*cos(theta6))/8 + (13*dtheta7*l*cos(theta7))/8 + (7*dtheta8*l*cos(theta8))/8)]);
Ctheta=createNumerical_matrixCtheta(N,theta);
fR_y=transpose([ (cn*cos(theta1)^2 + ct*sin(theta1)^2)*((7*dtheta1*l*cos(theta1))/8 - dpy + (13*dtheta2*l*cos(theta2))/8 + (11*dtheta3*l*cos(theta3))/8 + (9*dtheta4*l*cos(theta4))/8 + (7*dtheta5*l*cos(theta5))/8 + (5*dtheta6*l*cos(theta6))/8 + (3*dtheta7*l*cos(theta7))/8 + (dtheta8*l*cos(theta8))/8) + cos(theta1)*sin(theta1)*(cn - ct)*(dpx + (7*dtheta1*l*sin(theta1))/8 + (13*dtheta2*l*sin(theta2))/8 + (11*dtheta3*l*sin(theta3))/8 + (9*dtheta4*l*sin(theta4))/8 + (7*dtheta5*l*sin(theta5))/8 + (5*dtheta6*l*sin(theta6))/8 + (3*dtheta7*l*sin(theta7))/8 + (dtheta8*l*sin(theta8))/8), (cn*cos(theta2)^2 + ct*sin(theta2)^2)*((5*dtheta2*l*cos(theta2))/8 - (dtheta1*l*cos(theta1))/8 - dpy + (11*dtheta3*l*cos(theta3))/8 + (9*dtheta4*l*cos(theta4))/8 + (7*dtheta5*l*cos(theta5))/8 + (5*dtheta6*l*cos(theta6))/8 + (3*dtheta7*l*cos(theta7))/8 + (dtheta8*l*cos(theta8))/8) + cos(theta2)*sin(theta2)*(cn - ct)*(dpx - (dtheta1*l*sin(theta1))/8 + (5*dtheta2*l*sin(theta2))/8 + (11*dtheta3*l*sin(theta3))/8 + (9*dtheta4*l*sin(theta4))/8 + (7*dtheta5*l*sin(theta5))/8 + (5*dtheta6*l*sin(theta6))/8 + (3*dtheta7*l*sin(theta7))/8 + (dtheta8*l*sin(theta8))/8), (cn*cos(theta3)^2 + ct*sin(theta3)^2)*((3*dtheta3*l*cos(theta3))/8 - (dtheta1*l*cos(theta1))/8 - (3*dtheta2*l*cos(theta2))/8 - dpy + (9*dtheta4*l*cos(theta4))/8 + (7*dtheta5*l*cos(theta5))/8 + (5*dtheta6*l*cos(theta6))/8 + (3*dtheta7*l*cos(theta7))/8 + (dtheta8*l*cos(theta8))/8) + cos(theta3)*sin(theta3)*(cn - ct)*(dpx - (dtheta1*l*sin(theta1))/8 - (3*dtheta2*l*sin(theta2))/8 + (3*dtheta3*l*sin(theta3))/8 + (9*dtheta4*l*sin(theta4))/8 + (7*dtheta5*l*sin(theta5))/8 + (5*dtheta6*l*sin(theta6))/8 + (3*dtheta7*l*sin(theta7))/8 + (dtheta8*l*sin(theta8))/8), (cn*cos(theta4)^2 + ct*sin(theta4)^2)*((dtheta4*l*cos(theta4))/8 - (dtheta1*l*cos(theta1))/8 - (3*dtheta2*l*cos(theta2))/8 - (5*dtheta3*l*cos(theta3))/8 - dpy + (7*dtheta5*l*cos(theta5))/8 + (5*dtheta6*l*cos(theta6))/8 + (3*dtheta7*l*cos(theta7))/8 + (dtheta8*l*cos(theta8))/8) + cos(theta4)*sin(theta4)*(cn - ct)*(dpx - (dtheta1*l*sin(theta1))/8 - (3*dtheta2*l*sin(theta2))/8 - (5*dtheta3*l*sin(theta3))/8 + (dtheta4*l*sin(theta4))/8 + (7*dtheta5*l*sin(theta5))/8 + (5*dtheta6*l*sin(theta6))/8 + (3*dtheta7*l*sin(theta7))/8 + (dtheta8*l*sin(theta8))/8), - (cn*cos(theta5)^2 + ct*sin(theta5)^2)*(dpy + (dtheta1*l*cos(theta1))/8 + (3*dtheta2*l*cos(theta2))/8 + (5*dtheta3*l*cos(theta3))/8 + (7*dtheta4*l*cos(theta4))/8 + (dtheta5*l*cos(theta5))/8 - (5*dtheta6*l*cos(theta6))/8 - (3*dtheta7*l*cos(theta7))/8 - (dtheta8*l*cos(theta8))/8) - cos(theta5)*sin(theta5)*(cn - ct)*((dtheta1*l*sin(theta1))/8 - dpx + (3*dtheta2*l*sin(theta2))/8 + (5*dtheta3*l*sin(theta3))/8 + (7*dtheta4*l*sin(theta4))/8 + (dtheta5*l*sin(theta5))/8 - (5*dtheta6*l*sin(theta6))/8 - (3*dtheta7*l*sin(theta7))/8 - (dtheta8*l*sin(theta8))/8), - (cn*cos(theta6)^2 + ct*sin(theta6)^2)*(dpy + (dtheta1*l*cos(theta1))/8 + (3*dtheta2*l*cos(theta2))/8 + (5*dtheta3*l*cos(theta3))/8 + (7*dtheta4*l*cos(theta4))/8 + (9*dtheta5*l*cos(theta5))/8 + (3*dtheta6*l*cos(theta6))/8 - (3*dtheta7*l*cos(theta7))/8 - (dtheta8*l*cos(theta8))/8) - cos(theta6)*sin(theta6)*(cn - ct)*((dtheta1*l*sin(theta1))/8 - dpx + (3*dtheta2*l*sin(theta2))/8 + (5*dtheta3*l*sin(theta3))/8 + (7*dtheta4*l*sin(theta4))/8 + (9*dtheta5*l*sin(theta5))/8 + (3*dtheta6*l*sin(theta6))/8 - (3*dtheta7*l*sin(theta7))/8 - (dtheta8*l*sin(theta8))/8), - (cn*cos(theta7)^2 + ct*sin(theta7)^2)*(dpy + (dtheta1*l*cos(theta1))/8 + (3*dtheta2*l*cos(theta2))/8 + (5*dtheta3*l*cos(theta3))/8 + (7*dtheta4*l*cos(theta4))/8 + (9*dtheta5*l*cos(theta5))/8 + (11*dtheta6*l*cos(theta6))/8 + (5*dtheta7*l*cos(theta7))/8 - (dtheta8*l*cos(theta8))/8) - cos(theta7)*sin(theta7)*(cn - ct)*((dtheta1*l*sin(theta1))/8 - dpx + (3*dtheta2*l*sin(theta2))/8 + (5*dtheta3*l*sin(theta3))/8 + (7*dtheta4*l*sin(theta4))/8 + (9*dtheta5*l*sin(theta5))/8 + (11*dtheta6*l*sin(theta6))/8 + (5*dtheta7*l*sin(theta7))/8 - (dtheta8*l*sin(theta8))/8), - (cn*cos(theta8)^2 + ct*sin(theta8)^2)*(dpy + (dtheta1*l*cos(theta1))/8 + (3*dtheta2*l*cos(theta2))/8 + (5*dtheta3*l*cos(theta3))/8 + (7*dtheta4*l*cos(theta4))/8 + (9*dtheta5*l*cos(theta5))/8 + (11*dtheta6*l*cos(theta6))/8 + (13*dtheta7*l*cos(theta7))/8 + (7*dtheta8*l*cos(theta8))/8) - cos(theta8)*sin(theta8)*(cn - ct)*((dtheta1*l*sin(theta1))/8 - dpx + (3*dtheta2*l*sin(theta2))/8 + (5*dtheta3*l*sin(theta3))/8 + (7*dtheta4*l*sin(theta4))/8 + (9*dtheta5*l*sin(theta5))/8 + (11*dtheta6*l*sin(theta6))/8 + (13*dtheta7*l*sin(theta7))/8 + (7*dtheta8*l*sin(theta8))/8)]);
D=createNotation_matrixD(N);
global u
ddtheta=(matM^-1)*(-matW*(dtheta.^2)+l*Stheta*N*fR_x-l*Ctheta*N*fR_y+transpose(D)*u);
e=1+zeros(N,1);
ddp=(1/(N*m))*[e'*fR_x;e'*fR_y];
ddtheta1=ddtheta(1); ddtheta2=ddtheta(2); ddtheta3=ddtheta(3); ddtheta4=ddtheta(4); ddtheta5=ddtheta(5);
ddtheta6=ddtheta(6); ddtheta7=ddtheta(7); ddthetaN=ddtheta(8);
ddpx=ddp(1); ddpy=ddp(2);
f=[dtheta1;ddtheta1;dtheta2;ddtheta2;dtheta3;ddtheta3;dtheta4;ddtheta4;dtheta5;ddtheta5;dtheta6;ddtheta6;dtheta7;ddtheta7;dthetaN;ddthetaN;dpx;ddpx;dpy;ddpy];
end