-
Notifications
You must be signed in to change notification settings - Fork 0
/
Project-example.lisp
117 lines (103 loc) · 3.24 KB
/
Project-example.lisp
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
(asdf:operate 'asdf:load-op 'ae2sbvzot)
(use-package :trio-utils)
;;examples of defining variables
(defvar l_indexes `(bin aisle ws belt))
(defvar r_indexes `(Link1 Link2 EndEff Cart))
(defvar o_indexes `(head_area chest_area arm_area leg_area))
(defvar velocity_indexes `(low mid high))
(defvar alpha 3)
;;examples of defining functions
(defun l-constraints ()
(eval (append `(&&)
(loop for l in l_indexes collect
`(->
(-P- ,(read-from-string (format nil "Link1_In_~A" l)))
(!! (||
(-P- ,(read-from-string (format nil "Link1_In_~A" l)))
(-P- ,(read-from-string (format nil "Link1_In_~A" l)))
(-P- ,(read-from-string (format nil "Link1_In_~A" l)))
))))
(loop for l in l_indexes collect
`(->
(-P- ,(read-from-string (format nil "Link2_In_~A" l)))
(!! (||
(-P- ,(read-from-string (format nil "Link2_In_~A" l)))
(-P- ,(read-from-string (format nil "Link2_In_~A" l)))
(-P- ,(read-from-string (format nil "Link2_In_~A" l)))
))))
(loop for l in l_indexes collect
`(->
(-P- ,(read-from-string (format nil "Endeff_In_~A" l)))
(!! (||
(-P- ,(read-from-string (format nil "Endeff_In_~A" l)))
(-P- ,(read-from-string (format nil "Endeff_In_~A" l)))
(-P- ,(read-from-string (format nil "Endeff_In_~A" l)))
))))
(loop for l in l_indexes collect
`(->
(-P- ,(read-from-string (format nil "Cart_In_~A" l)))
(!! (||
(-P- ,(read-from-string (format nil "Cart_In_~A" l)))
(-P- ,(read-from-string (format nil "Cart_In_~A" l)))
(-P- ,(read-from-string (format nil "Cart_In_~A" l)))
))))
)))
(defun velocity-constraints ()
(eval (append `(&&)
(loop for v in velocity_indexes collect
`(->
(-P- ,(read-from-string (format nil "Link1_vel_~A" v)))
(!! (||
(-P- ,(read-from-string (format nil "Link1_vel_~A" v)))
(-P- ,(read-from-string (format nil "Link1_vel_~A" v)))
(-P- ,(read-from-string (format nil "Link1_vel_~A" v)))
))))
(loop for v in velocity_indexes collect
`(->
(-P- ,(read-from-string (format nil "Link2_vel_~A" v)))
(!! (||
(-P- ,(read-from-string (format nil "Link2_vel_~A" v)))
(-P- ,(read-from-string (format nil "Link2_vel_~A" v)))
(-P- ,(read-from-string (format nil "Link2_vel_~A" v)))
))))
(loop for v in velocity_indexes collect
`(->
(-P- ,(read-from-string (format nil "Endeff_vel_~A" v)))
(!! (||
(-P- ,(read-from-string (format nil "Endeff_vel_~A" v)))
(-P- ,(read-from-string (format nil "Endeff_vel_~A" v)))
(-P- ,(read-from-string (format nil "Endeff_vel_~A" v)))
))))
(loop for v in velocity_indexes collect
`(->
(-P- ,(read-from-string (format nil "Cart_vel_~A" v)))
(!! (||
(-P- ,(read-from-string (format nil "Cart_vel_~A" v)))
(-P- ,(read-from-string (format nil "Cart_vel_~A" v)))
(-P- ,(read-from-string (format nil "Cart_vel_~A" v)))
))))
)))
;;examples of defining predicates
(defconstant contact
(Alw(&&
(->
(&&(-P- endeff_in_belt)(-P- arm_area_in_belt))
(-P- contact)
)
(->
(&&(-P- Endeff_vel_high) (-P- contact))
(withinF (-P- Endeff_vel_low) alpha)
)
)))
(defvar *spec*
(alw
(&&
(l-constraints)
(velocity-constraints)
contact
)))
(format t "~S" *spec*)
(ae2sbvzot:zot 10
(yesterday(&&
*spec*
)))