-
Notifications
You must be signed in to change notification settings - Fork 9
/
ForwardAccSYM.m
59 lines (45 loc) · 10.4 KB
/
ForwardAccSYM.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
% Forward Acceleration Jdot and J
syms t;
syms q1(t);
syms q2(t);
syms q3(t);
syms q4(t);
syms q5(t);
syms q6(t);
L1= sym('L1');
L2= sym('L2');
L3= sym('L3');
L4= sym('L4');
A0T1= [ cos(0.0175*q1(t) - 1.5708), 0, -sin(0.0175*q1(t) - 1.5708), 0;...
sin(0.0175*q1(t) - 1.5708), 0, cos(0.0175*q1(t) - 1.5708), 0;...
0, -1, 0, L1];
A0T2= [ cos(0.0175*q1(t) - 1.5708)*cos(0.0175*q2(t) - 1.5708), -sin(0.0175*q1(t) - 1.5708), cos(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708), 0;...
cos(0.0175*q2(t) - 1.5708)*sin(0.0175*q1(t) - 1.5708), cos(0.0175*q1(t) - 1.5708), sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708), 0;...
-sin(0.0175*q2(t) - 1.5708), 0, cos(0.0175*q2(t) - 1.5708), L1];
A0T3= [ cos(0.0175*q1(t) - 1.5708)*cos(0.0175*q2(t) - 1.5708), -sin(0.0175*q1(t) - 1.5708), cos(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708), cos(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708)*(L2 + q3(t));...
cos(0.0175*q2(t) - 1.5708)*sin(0.0175*q1(t) - 1.5708), cos(0.0175*q1(t) - 1.5708), sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708), sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708)*(L2 + q3(t));...
-sin(0.0175*q2(t) - 1.5708), 0, cos(0.0175*q2(t) - 1.5708), L1 + cos(0.0175*q2(t) - 1.5708)*(L2 + q3(t))];
A0T4= [ cos(0.0175*q1(t) - 1.5708)*cos(0.0175*q2(t) - 1.5708)*cos(0.0175*q4(t) + 1.5708) - sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q4(t) + 1.5708), cos(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708), cos(0.0175*q4(t) + 1.5708)*sin(0.0175*q1(t) - 1.5708) + cos(0.0175*q1(t) - 1.5708)*cos(0.0175*q2(t) - 1.5708)*sin(0.0175*q4(t) + 1.5708), cos(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708)*(L2 + q3(t)) + L3*cos(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708);...
cos(0.0175*q1(t) - 1.5708)*sin(0.0175*q4(t) + 1.5708) + cos(0.0175*q2(t) - 1.5708)*cos(0.0175*q4(t) + 1.5708)*sin(0.0175*q1(t) - 1.5708), sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708), cos(0.0175*q2(t) - 1.5708)*sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q4(t) + 1.5708) - cos(0.0175*q1(t) - 1.5708)*cos(0.0175*q4(t) + 1.5708), sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708)*(L2 + q3(t)) + L3*sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708);...
-cos(0.0175*q4(t) + 1.5708)*sin(0.0175*q2(t) - 1.5708), cos(0.0175*q2(t) - 1.5708), -sin(0.0175*q2(t) - 1.5708)*sin(0.0175*q4(t) + 1.5708), L1 + cos(0.0175*q2(t) - 1.5708)*(L2 + q3(t)) + L3*cos(0.0175*q2(t) - 1.5708)];
A0T5= [ cos(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708)*sin(0.0175*q5(t) + 1.5708) - cos(0.0175*q5(t) + 1.5708)*(sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q4(t) + 1.5708) - cos(0.0175*q1(t) - 1.5708)*cos(0.0175*q2(t) - 1.5708)*cos(0.0175*q4(t) + 1.5708)), cos(0.0175*q4(t) + 1.5708)*sin(0.0175*q1(t) - 1.5708) + cos(0.0175*q1(t) - 1.5708)*cos(0.0175*q2(t) - 1.5708)*sin(0.0175*q4(t) + 1.5708), - sin(0.0175*q5(t) + 1.5708)*(sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q4(t) + 1.5708) - cos(0.0175*q1(t) - 1.5708)*cos(0.0175*q2(t) - 1.5708)*cos(0.0175*q4(t) + 1.5708)) - cos(0.0175*q1(t) - 1.5708)*cos(0.0175*q5(t) + 1.5708)*sin(0.0175*q2(t) - 1.5708), cos(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708)*(L2 + q3(t)) + L3*cos(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708);...
cos(0.0175*q5(t) + 1.5708)*(cos(0.0175*q1(t) - 1.5708)*sin(0.0175*q4(t) + 1.5708) + cos(0.0175*q2(t) - 1.5708)*cos(0.0175*q4(t) + 1.5708)*sin(0.0175*q1(t) - 1.5708)) + sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708)*sin(0.0175*q5(t) + 1.5708), cos(0.0175*q2(t) - 1.5708)*sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q4(t) + 1.5708) - cos(0.0175*q1(t) - 1.5708)*cos(0.0175*q4(t) + 1.5708), sin(0.0175*q5(t) + 1.5708)*(cos(0.0175*q1(t) - 1.5708)*sin(0.0175*q4(t) + 1.5708) + cos(0.0175*q2(t) - 1.5708)*cos(0.0175*q4(t) + 1.5708)*sin(0.0175*q1(t) - 1.5708)) - cos(0.0175*q5(t) + 1.5708)*sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708), sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708)*(L2 + q3(t)) + L3*sin(0.0175*q1(t) - 1.5708)*sin(0.0175*q2(t) - 1.5708);...
cos(0.0175*q2(t) - 1.5708)*sin(0.0175*q5(t) + 1.5708) - cos(0.0175*q4(t) + 1.5708)*cos(0.0175*q5(t) + 1.5708)*sin(0.0175*q2(t) - 1.5708), -sin(0.0175*q2(t) - 1.5708)*sin(0.0175*q4(t) + 1.5708), - cos(0.0175*q2(t) - 1.5708)*cos(0.0175*q5(t) + 1.5708) - cos(0.0175*q4(t) + 1.5708)*sin(0.0175*q2(t) - 1.5708)*sin(0.0175*q5(t) + 1.5708), L1 + cos(0.0175*q2(t) - 1.5708)*(L2 + q3(t)) + L3*cos(0.0175*q2(t) - 1.5708)];
A0T6= [ sin((pi*q6(t))/180)*(cos((pi*(q4(t) + 90))/180)*sin((pi*(q1(t) - 90))/180) + cos((pi*(q1(t) - 90))/180)*cos((pi*(q2(t) - 90))/180)*sin((pi*(q4(t) + 90))/180)) - cos((pi*q6(t))/180)*(cos((pi*(q5(t) + 90))/180)*(sin((pi*(q1(t) - 90))/180)*sin((pi*(q4(t) + 90))/180) - cos((pi*(q1(t) - 90))/180)*cos((pi*(q2(t) - 90))/180)*cos((pi*(q4(t) + 90))/180)) - cos((pi*(q1(t) - 90))/180)*sin((pi*(q2(t) - 90))/180)*sin((pi*(q5(t) + 90))/180)), sin((pi*q6(t))/180)*(cos((pi*(q5(t) + 90))/180)*(sin((pi*(q1(t) - 90))/180)*sin((pi*(q4(t) + 90))/180) - cos((pi*(q1(t) - 90))/180)*cos((pi*(q2(t) - 90))/180)*cos((pi*(q4(t) + 90))/180)) - cos((pi*(q1(t) - 90))/180)*sin((pi*(q2(t) - 90))/180)*sin((pi*(q5(t) + 90))/180)) + cos((pi*q6(t))/180)*(cos((pi*(q4(t) + 90))/180)*sin((pi*(q1(t) - 90))/180) + cos((pi*(q1(t) - 90))/180)*cos((pi*(q2(t) - 90))/180)*sin((pi*(q4(t) + 90))/180)), - sin((pi*(q5(t) + 90))/180)*(sin((pi*(q1(t) - 90))/180)*sin((pi*(q4(t) + 90))/180) - cos((pi*(q1(t) - 90))/180)*cos((pi*(q2(t) - 90))/180)*cos((pi*(q4(t) + 90))/180)) - cos((pi*(q1(t) - 90))/180)*cos((pi*(q5(t) + 90))/180)*sin((pi*(q2(t) - 90))/180), cos((pi*(q1(t) - 90))/180)*sin((pi*(q2(t) - 90))/180)*(L2 + q3(t)) - L4*(sin((pi*(q5(t) + 90))/180)*(sin((pi*(q1(t) - 90))/180)*sin((pi*(q4(t) + 90))/180) - cos((pi*(q1(t) - 90))/180)*cos((pi*(q2(t) - 90))/180)*cos((pi*(q4(t) + 90))/180)) + cos((pi*(q1(t) - 90))/180)*cos((pi*(q5(t) + 90))/180)*sin((pi*(q2(t) - 90))/180)) + L3*cos((pi*(q1(t) - 90))/180)*sin((pi*(q2(t) - 90))/180);
cos((pi*q6(t))/180)*(cos((pi*(q5(t) + 90))/180)*(cos((pi*(q1(t) - 90))/180)*sin((pi*(q4(t) + 90))/180) + cos((pi*(q2(t) - 90))/180)*cos((pi*(q4(t) + 90))/180)*sin((pi*(q1(t) - 90))/180)) + sin((pi*(q1(t) - 90))/180)*sin((pi*(q2(t) - 90))/180)*sin((pi*(q5(t) + 90))/180)) - sin((pi*q6(t))/180)*(cos((pi*(q1(t) - 90))/180)*cos((pi*(q4(t) + 90))/180) - cos((pi*(q2(t) - 90))/180)*sin((pi*(q1(t) - 90))/180)*sin((pi*(q4(t) + 90))/180)), - sin((pi*q6(t))/180)*(cos((pi*(q5(t) + 90))/180)*(cos((pi*(q1(t) - 90))/180)*sin((pi*(q4(t) + 90))/180) + cos((pi*(q2(t) - 90))/180)*cos((pi*(q4(t) + 90))/180)*sin((pi*(q1(t) - 90))/180)) + sin((pi*(q1(t) - 90))/180)*sin((pi*(q2(t) - 90))/180)*sin((pi*(q5(t) + 90))/180)) - cos((pi*q6(t))/180)*(cos((pi*(q1(t) - 90))/180)*cos((pi*(q4(t) + 90))/180) - cos((pi*(q2(t) - 90))/180)*sin((pi*(q1(t) - 90))/180)*sin((pi*(q4(t) + 90))/180)), sin((pi*(q5(t) + 90))/180)*(cos((pi*(q1(t) - 90))/180)*sin((pi*(q4(t) + 90))/180) + cos((pi*(q2(t) - 90))/180)*cos((pi*(q4(t) + 90))/180)*sin((pi*(q1(t) - 90))/180)) - cos((pi*(q5(t) + 90))/180)*sin((pi*(q1(t) - 90))/180)*sin((pi*(q2(t) - 90))/180), L4*(sin((pi*(q5(t) + 90))/180)*(cos((pi*(q1(t) - 90))/180)*sin((pi*(q4(t) + 90))/180) + cos((pi*(q2(t) - 90))/180)*cos((pi*(q4(t) + 90))/180)*sin((pi*(q1(t) - 90))/180)) - cos((pi*(q5(t) + 90))/180)*sin((pi*(q1(t) - 90))/180)*sin((pi*(q2(t) - 90))/180)) + sin((pi*(q1(t) - 90))/180)*sin((pi*(q2(t) - 90))/180)*(L2 + q3(t)) + L3*sin((pi*(q1(t) - 90))/180)*sin((pi*(q2(t) - 90))/180);
cos((pi*q6(t))/180)*(cos((pi*(q2(t) - 90))/180)*sin((pi*(q5(t) + 90))/180) - cos((pi*(q4(t) + 90))/180)*cos((pi*(q5(t) + 90))/180)*sin((pi*(q2(t) - 90))/180)) - sin((pi*q6(t))/180)*sin((pi*(q2(t) - 90))/180)*sin((pi*(q4(t) + 90))/180), - sin((pi*q6(t))/180)*(cos((pi*(q2(t) - 90))/180)*sin((pi*(q5(t) + 90))/180) - cos((pi*(q4(t) + 90))/180)*cos((pi*(q5(t) + 90))/180)*sin((pi*(q2(t) - 90))/180)) - cos((pi*q6(t))/180)*sin((pi*(q2(t) - 90))/180)*sin((pi*(q4(t) + 90))/180), - cos((pi*(q2(t) - 90))/180)*cos((pi*(q5(t) + 90))/180) - cos((pi*(q4(t) + 90))/180)*sin((pi*(q2(t) - 90))/180)*sin((pi*(q5(t) + 90))/180), L1 - L4*(cos((pi*(q2(t) - 90))/180)*cos((pi*(q5(t) + 90))/180) + cos((pi*(q4(t) + 90))/180)*sin((pi*(q2(t) - 90))/180)*sin((pi*(q5(t) + 90))/180)) + cos((pi*(q2(t) - 90))/180)*(L2 + q3(t)) + L3*cos((pi*(q2(t) - 90))/180)];
Jv1= cross([0;0;1], A0T6(:,4)-[0;0;0]);
Jv2= cross(A0T1(:,3),(A0T6(:,4)-A0T1(:,4)));
Jv3= A0T3(:,3);
Jv4= cross(A0T3(:,3),(A0T6(:,4)-A0T3(:,4)));
Jv5= cross(A0T4(:,3),(A0T6(:,4)-A0T4(:,4)));
Jv6= cross(A0T5(:,3),(A0T6(:,4)-A0T5(:,4)));
Jw1=[0;0;1];
Jw2= A0T1(:,3);
Jw3=[0;0;0];
Jw4= A0T3(:,3);
Jw5= A0T4(:,3);
Jw6= A0T5(:,3);
VelocityMat= [Jv1 Jv2 Jv3 Jv4 Jv5 Jv6; Jw1 Jw2 Jw3 Jw4 Jw5 Jw6];
J=VelocityMat;
Jdev= diff(J,t);