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
|
function move=create_3d_line(v_range,dp,theta,prev_move,limits)
%% Create a random trajectory segment based on the input
if ~isstruct(prev_move)
%wlog assume +X as initial direction and origin as start
prev_move.P1=[-1,0,0];
prev_move.P2=[0,0,0];
end
move.P1=prev_move.P2;
%Hard-coded settings for G-code dump, make this more general later if need be
if length(v_range)==2
move.v_req=ezrand(v_range(1),v_range(2));
else
move.v_req=v_range;
end
%dummy value
move.a_max=1;
move.tolerance=10;
dp_old=prev_move.P2-prev_move.P1;
u_old=dp_old/norm(dp_old);
if length(theta)==1
theta_vec=[-theta,theta,-theta,theta,-theta,theta];
elseif length(theta)==2
theta_vec=[theta,theta,theta];
theta_vec=theta_vec(:);
else
theta_vec=theta;
end
inlimits = false;
while ~inlimits
%Rotate about Z
theta_z=rand(1)*(theta_vec(2)-theta_vec(1)) + theta_vec(1);
u_raw=u_old*[cos(theta_z),-sin(theta_z),0;sin(theta_z),cos(theta_z),0;0,0,1];
%rotate about Y
theta_y=rand(1)*(theta_vec(4)-theta_vec(3)) + theta_vec(3);
u_raw=u_raw*[cos(theta_y),0,-sin(theta_y);0,1,0;sin(theta_y),0,cos(theta_y)];
theta_x=rand(1)*(theta_vec(6)-theta_vec(5)) + theta_vec(5);
u_raw=u_raw*[1, 0, 0;0, cos(theta_x),sin(theta_x);0,-sin(theta_x),cos(theta_x)];
%Hack to make a minimum length move
if length(dp) == 1
move.P2=move.P1+u_raw*dp;
else
move.P2=move.P1+u_raw*(rand(1)*(dp(2)-dp(1))+dp(1));
end
if move.P2(1)<limits(2) && move.P2(1)>limits(1) && move.P2(2)<limits(4) && move.P2(2)>limits(3) && move.P2(3)<limits(6) && move.P2(3)>limits(5)
inlimits = true;
end
end
end
|
