gistfile1.txt

(load "package://pr2eus/robot-interface.l")
(load "irteus/demo/sample-robot-model.l")
(if (not (boundp '*robot*))
    (setq *robot* (instance sample-robot :init)))
(setq av0 (send *robot* :reset-pose))
(send *robot* :fix-leg-to-coords (make-coords))
(setq c0 (make-coords :pos #f(300  500 800)))
(setq c1 (make-coords :pos #f(300 -100 800)))
(objects (list *robot* c0 c1))
(if (not (boundp '*ri*))
    (setq *ri* (instance robot-interface :init :robot sample-robot)))

;; interpolate joint angle space                                                
(setq av1 (send *robot* :larm :inverse-kinematics c0))
(setq av2 (send *robot* :larm :inverse-kinematics c1))
(send *ri* :angle-vector-sequence (list av0 av1 av2) (list 5000 5000 5000))
(send *ri* :wait-interpolation)

;; interpolate cartesian space                                                  
(setq avs nil)
(dotimes (i 20)
  (let ((tmp-c))
    (setq tmp-c (midcoords (/ i 20.0) c0 c1))
    (send *robot* :larm :inverse-kinematics tmp-c)
    (push (send *robot* :angle-vector) avs)
    ))
(setq avs (nreverse avs))
;; (send *ri* :set-interpolation-mode :linear) for real robot                   
(send *ri* :angle-vector (car avs) 2000)
(send *ri* :angle-vector-sequence avs 100)