forked from quan-dao/snake-robot-velocity-ctrl
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathcalculate_fR.m
executable file
·25 lines (21 loc) · 9.22 KB
/
calculate_fR.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
function f=calculate_fR(theta,dtheta,p,dp)
theta1=theta(1); px=p(1); py=p(2);
theta2=theta(2);
theta3=theta(3);
theta4=theta(4);
theta5=theta(5);
theta6=theta(6);
theta7=theta(7);
theta8=theta(8);
dtheta1=dtheta(1); dpx=dp(1); dpy=dp(2);
dtheta2=dtheta(2);
dtheta3=dtheta(3);
dtheta4=dtheta(4);
dtheta5=dtheta(5);
dtheta6=dtheta(6);
dtheta7=dtheta(7);
dtheta8=dtheta(8);
global ct cn l
temp=[ - (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), (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)];
f=transpose(temp);
end