(defun node-operation (s1 s2) (maze-operation s1 s2)) (defun maze-operation (s1 s2) (cond ((> (distance (robot-position s1) (robot-position s2)) 1.2) (print 'error)) (t (list 'move (v- (robot-position s2) (robot-position s1))))) ) (defun maze-operations (state-path) (let ((state (pop state-path)) (result nil)) (dolist (s state-path) (setq result (cons (maze-operation state s) result)) (setq state s)) (reverse result))) (defun operations-from-path (path) (maze-operations path)) (defun motion-planning (start goal) (operations-from-path (goal-search start goal)))
> (motion-planning '(robot at (0 0)) '(robot at (3 3))) ((move (1 0)) (move (1 0)) (move (1 0)) (move (0 1)) (move (0 1)) (move (1 0)) (move (0 1)) (move (-1 0))) > (motion-planning '(robot at (0 0)) '(robot at (0 3))) ((move (1 0)) (move (1 0)) (move (1 0)) (move (0 1)) (move (-1 0)) (move (-1 0)) (move (0 1)) (move (-1 0)) (move (0 1))) > (motion-planning '(robot at (0 0)) '(robot at (2 4))) ((move (1 0)) (move (1 0)) (move (1 0)) (move (0 1)) (move (0 1)) (move (1 0)) (move (0 1)) (move (-1 0)) (move (0 1)) (move (-1 0)))