-
Notifications
You must be signed in to change notification settings - Fork 0
/
generateZ3File.m
117 lines (91 loc) · 4.87 KB
/
generateZ3File.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
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
function [] = generateZ3File (initpos, goalpos, number_of_hops)
default_min = -1000;
default_max = 1000;
% Workspace dimensions
width = 30;
height = 30;
wksp = createWorkspace(width,height);
%wksp = [];
fid = fopen ('constraints.smt2', 'w');
for k = 1 : size(wksp,1)
fprintf(fid, ';;Obstacle %d\n', k);
fprintf(fid, '(declare-fun obs%dxbl () Real)\n', k);
fprintf(fid, '(declare-fun obs%dxbr () Real)\n', k);
fprintf(fid, '(declare-fun obs%dxtr () Real)\n', k);
fprintf(fid, '(declare-fun obs%dxtl () Real)\n', k);
fprintf(fid, '(declare-fun obs%dybl () Real)\n', k);
fprintf(fid, '(declare-fun obs%dybr () Real)\n', k);
fprintf(fid, '(declare-fun obs%dytr () Real)\n', k);
fprintf(fid, '(declare-fun obs%dytl () Real)\n\n', k);
end
fprintf(fid, '(declare-fun epsilon () Real)\n\n');
for k = 0 : number_of_hops
fprintf(fid, '(declare-fun x%d () Real)\n', k);
fprintf(fid, '(declare-fun y%d () Real)\n', k);
end
fprintf(fid, '\n');
for k = 1 : size(wksp,1)
for m = 1 : number_of_hops
fprintf(fid, '(declare-fun a%d_%d () Real)\n', k, m);
fprintf(fid, '(declare-fun b%d_%d () Real)\n', k, m);
fprintf(fid, '(declare-fun c%d_%d () Real)\n', k, m);
end
end
fprintf(fid, '\n\n');
for k = 1 : size(wksp,1)
fprintf(fid, ';;Obstacle %d\n', k);
fprintf(fid, '(assert (= obs%dxbl %f))\n', k, wksp{k,1}(1,1));
fprintf(fid, '(assert (= obs%dxbr %f))\n', k, wksp{k,1}(1,2));
fprintf(fid, '(assert (= obs%dxtr %f))\n', k, wksp{k,1}(1,2));
fprintf(fid, '(assert (= obs%dxtl %f))\n', k, wksp{k,1}(1,1));
fprintf(fid, '(assert (= obs%dybl %f))\n', k, wksp{k,1}(2,1));
fprintf(fid, '(assert (= obs%dybr %f))\n', k, wksp{k,1}(2,1));
fprintf(fid, '(assert (= obs%dytr %f))\n', k, wksp{k,1}(2,2));
fprintf(fid, '(assert (= obs%dytl %f))\n\n', k, wksp{k,1}(2,2));
end
fprintf(fid, '(assert (= epsilon 1))\n\n');
for k = 0 : number_of_hops
if (k == 0)
fprintf(fid, '(assert (= x0 %d))\n', initpos(1));
fprintf(fid, '(assert (= y0 %d))\n', initpos(2));
end
if (k > 0 && k < number_of_hops)
fprintf(fid, '(assert (>= x%d 0))\n', k);
fprintf(fid, '(assert (<= x%d 30))\n', k);
fprintf(fid, '(assert (>= y%d 0))\n', k);
fprintf(fid, '(assert (<= y%d 30))\n', k);
end
if (k == number_of_hops)
fprintf(fid, '(assert (>= x%d %d))\n', k, goalpos(1) - 1);
fprintf(fid, '(assert (<= x%d %d))\n', k, goalpos(1) + 1);
fprintf(fid, '(assert (>= y%d %d))\n', k, goalpos(2) - 1);
fprintf(fid, '(assert (<= y%d %d))\n', k, goalpos(2) + 1);
end
fprintf(fid, '\n');
end
fprintf(fid, '\n\n');
% Check if the obstacles are ouside the rectangle covering the envelop
for k = 1 : size(wksp,1)
for m = 1 : number_of_hops
fprintf(fid, '(assert (or (and (< (+ (* a%d_%d x%d) (* b%d_%d y%d) c%d_%d (- epsilon)) 0)\n', k, m, m -1 , k, m, m - 1, k, m);
fprintf(fid, ' (< (+ (* a%d_%d x%d) (* b%d_%d y%d) c%d_%d (- epsilon)) 0)\n', k, m, m, k, m, m, k, m);
fprintf(fid, ' (> (+ (* a%d_%d obs%dxbl) (* b%d_%d obs%dybl) c%d_%d (- epsilon)) 0)\n', k, m, k, k, m, k, k, m);
fprintf(fid, ' (> (+ (* a%d_%d obs%dxbr) (* b%d_%d obs%dybr) c%d_%d (- epsilon)) 0)\n', k, m, k, k, m, k, k, m);
fprintf(fid, ' (> (+ (* a%d_%d obs%dxtl) (* b%d_%d obs%dytl) c%d_%d (- epsilon)) 0)\n', k, m, k, k, m, k, k, m);
fprintf(fid, ' (> (+ (* a%d_%d obs%dxtr) (* b%d_%d obs%dytr) c%d_%d (- epsilon)) 0))\n', k, m, k, k, m, k, k, m);
fprintf(fid, ' (and (> (+ (* a%d_%d x%d) (* b%d_%d y%d) c%d_%d (- epsilon)) 0)\n', k, m, m -1, k, m, m - 1, k, m);
fprintf(fid, ' (> (+ (* a%d_%d x%d) (* b%d_%d y%d) c%d_%d (- epsilon)) 0)\n', k, m, m, k, m, m, k, m);
fprintf(fid, ' (< (+ (* a%d_%d obs%dxbl) (* b%d_%d obs%dybl) c%d_%d (- epsilon)) 0)\n', k, m, k, k, m, k, k, m);
fprintf(fid, ' (< (+ (* a%d_%d obs%dxbr) (* b%d_%d obs%dybr) c%d_%d (- epsilon)) 0)\n', k, m, k, k, m, k, k, m);
fprintf(fid, ' (< (+ (* a%d_%d obs%dxtl) (* b%d_%d obs%dytl) c%d_%d (- epsilon)) 0)\n', k, m, k, k, m, k, k, m);
fprintf(fid, ' (< (+ (* a%d_%d obs%dxtr) (* b%d_%d obs%dytr) c%d_%d (- epsilon)) 0))))\n\n', k, m, k, k, m, k, k, m);
end
end
fprintf(fid, '(check-sat)\n');
fprintf(fid, '\n\n');
for k = 0 : number_of_hops
fprintf(fid, '(get-value (x%d))\n', k);
fprintf(fid, '(get-value (y%d))\n', k);
end
fclose(fid);
end