ROS_Robotics_Lab3/workspace.asv at main · caverar/ROS_Robotics_Lab3 · GitHub

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
%% Lab 2 - Matlab and ROS with PhantomX
clear
clc

l = 0.01.*[14.5, 10.63, 10.65, 8.97]; % Length of links
L(1) = Link('revolute','alpha',pi/2,'a',0,   'd',l(1),'offset',0,   'qlim',[-3*pi/4 3*pi/4]);
L(2) = Link('revolute','alpha',0,   'a',l(2),'d',0,   'offset',pi/2,'qlim',[-3*pi/4 3*pi/4]);
L(3) = Link('revolute','alpha',0,   'a',l(3),'d',0,   'offset',0,   'qlim',[-3*pi/4 3*pi/4]);
L(4) = Link('revolute','alpha',0,   'a',0,   'd',0,   'offset',0,   'qlim',[-3*pi/4 3*pi/4]);
PhantomX = SerialLink(L,'name','Px');

%Tool orientation following NAO Convention
PhantomX.tool = [0 0 1 l(4); -1 0 0 0; 0 -1 0 0; 0 0 0 1];

%% 2d workspace

rng('default')
samples = 100000;

q2 = ((3*pi/2)*rand(samples,1))-((3*pi/4)*(ones(samples,1)));
q3 = ((3*pi/2)*rand(samples,1))-((3*pi/4)*(ones(samples,1)));
q4 = ((3*pi/2)*rand(samples,1))-((3*pi/4)*(ones(samples,1)));

x_pos = zeros(samples,1);
y_pos = zeros(samples,1);

disp(x_pos(1))

for i = 1:1:samples
    MTH = PhantomX.fkine([0 q2(i) q3(i) q4(i)]);
    x_pos(i) = MTH(1,4);
    y_pos(i) = MTH(2,4);
end

plot(x_pos,y_pos);

%% 2d boundary
bound=boundary(x_pos,y_pos);
plot(x_pos(bound),y_pos(bound))

%% 3d workspace
rng('default')
samples = 1000000;
q1 = ((3*pi/2)*rand(samples,1))-((3*pi/4)*(ones(samples,1)));
q2 = ((3*pi/2)*rand(samples,1))-((3*pi/4)*(ones(samples,1)));
q3 = ((3*pi/2)*rand(samples,1))-((3*pi/4)*(ones(samples,1)));
q4 = ((3*pi/2)*rand(samples,1))-((3*pi/4)*(ones(samples,1)));

x_pos = zeros(samples,1);
y_pos = zeros(samples,1);
z_pos = zeros(samples,1);
disp(x_pos(1))

for i = 1:1:samples
    MTH = PhantomX.fkine([q1(i) q2(i) q3(i) q4(i)]);
    x_pos(i) = MTH(1,4);
    y_pos(i) = MTH(2,4);
    z_pos(i) = MTH(3,4);

end


plot3(x_pos,y_pos,z_pos);

%% 3d boundary
bound=boundary(x_pos,y_pos,z_pos);
trisurf(bound,x_pos,y_pos,z_pos,'Facecolor','red','FaceAlpha',0.1)