-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtest.m
47 lines (26 loc) · 1.16 KB
/
test.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
% Winch/pulley locations (ai)
FrameAnchors = [ ...
[-4.70; 4.30] ...
, [ 3.70; 3.20] ...
, [ 3.50; -5.20] ...
, [-4.50; -5.00] ...
];
% Platform anchors (bi)
%Platform = zeros(4,2);
Platform = (20*[ -0.02 , 0.02 , 0.02 , -0.02 ;
-0.01 , -0.01 , 0.01 , 0.01 ]);
CableOffset = zeros(1,4);
p = [0; 0; 1; 0; 0; 1];
r = [p(1); p(2)];
R = [p(3), p(4); p(5), p(6)];
L = FrameAnchors - ( repmat(r, 1, 4) + R * Platform );
l = (sqrt( sum( ( L ).^2, 1)) + CableOffset);
u = L ./ l;
AT = vertcat( ...
u ... % Force part
, cross([R*Platform; zeros(1,4)], [u; zeros(1,4)]) ... % Torque part
);
dot(R*Platform, [u(2,:); -u(1,:)]);
MassMatrix = 50;
wrench = 10;
ddy = mldivide(MassMatrix, wrench)