ymaguchi-457.l

;; from https://github.com/euslisp/jskeus/pull/595
(defmethod interpolator
  (:pass-time
   (dt)
   "process interpolation for dt[sec]"
   (when interpolatingp
     (setq position (send self :interpolation))
     (incf time dt)
     (setq segment-time (- time (if (= segment 0) 0 (nth (1- segment) time-list))))
     (when (eps> time (nth segment time-list) (* 0.1 dt))
       ;; if time-segment is not aligned, need to fix the data (see https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/457)
       (while (and (< segment segment-num) (eps> time (nth segment time-list) (* 0.1 dt)))
         (setq segment-time (- time (nth segment time-list)))
         (incf segment)))
     (when (>= segment segment-num)
       ;; adjust time and segment-time to exact osition)
       (setq segment (1- segment-num))
       (setq time (car (last time-list)))
       (setq segment-time (- time (if (= segment 0) 0 (nth (1- segment) time-list))))
       ;; re-calculate :interpolation
       (setq position (send self :interpolation))
       (send self :reset))
     ;; if time exceeds the last time-list, truncate it
     (when (> time (car (last time-list)))
       (setq segment-time (- segment-time (- time (car (last time-list)))))
       (setq time (car (last time-list))))
     position))
  ;
  )

;; from https://github.com/euslisp/jskeus/pull/596
(defmethod minjerk-interpolator
  (:interpolation
   ()
   "Minjerk interpolator, a.k.a Hoff & Arbib
    Example code is:
(setq l (instance minjerk-interpolator :init))
(send l :reset :position-list (list #f(1 2 3) #f(3 4 5) #f(1 2 3)) :time-list (list 0.1 0.18))
(send l :start-interpolation)
(while (send l :interpolatingp) (send l :pass-time 0.02) (print (send l :position)))
"
   (let* ((xi (nth segment position-list))
          (xf (nth (1+ segment) position-list))
          (vi (nth segment velocity-list))
          (vf (nth (1+ segment) velocity-list))
          (ai (nth segment acceleration-list))
          (af (nth (1+ segment) acceleration-list))
          ;;
          (t1+t2 (- (nth segment time-list) (if (> segment 0) (nth (1- segment) time-list) 0))) ;; total time of segment
          ;; A=(gx-(x+v*t+(a/2.0)*t*t))/(t*t*t)
          ;; B=(gv-(v+a*t))/(t*t)
          ;; C=(ga-a)/t
          (A  (scale (/ 1.0 (* t1+t2 t1+t2 t1+t2)) (v- xf (reduce #'v+ (list xi (scale t1+t2 vi) (scale (* t1+t2 t1+t2) (scale 0.5 ai)))))))
          (B  (scale (/ 1.0 (* t1+t2 t1+t2))      (v- vf (v+ vi (scale t1+t2 ai)))))
          (C  (scale (/ 1.0 (* t1+t2))           (v- af ai)))
          ;; a0=x
          ;; a1=v
          ;; a2=a/2.0
          ;; a3=10*A-4*B+0.5*C
          ;;; a4=(-15*A+7*B-C)/t
          ;; a5=(6*A-3*B+0.5*C)/(t*t)
          (a0 xi)
          (a1 vi)
          (a2 (scale 0.5 ai))
          (a3 (v+ (v- (scale 10 A) (scale 4 B)) (scale 0.5 C)))
          (a4 (scale (/ 1.0 t1+t2) (v- (v+ (scale -15 A) (scale 7 B)) C)))
          (a5 (scale (/ 1.0 t1+t2 t1+t2) (v+ (v+ (scale 6 A) (scale -3 B)) (scale 0.5 C))))
          )
     ;; x=a0+a1*t+a2*t*t+a3*t*t*t+a4*t*t*t*t+a5*t*t*t*t*t
     ;; v=a1+2*a2*t+3*a3*t*t+4*a4*t*t*t+5*a5*t*t*t*t
     ;; a=2*a2+6*a3*t+12*a4*t*t+20*a5*t*t*t
     (setq position
           (reduce #'v+ (list a0
                              (scale (expt segment-time 1) a1) (scale (expt segment-time 2) a2)
                              (scale (expt segment-time 3) a3) (scale (expt segment-time 4) a4)
                              (scale (expt segment-time 5) a5)))
           velocity
           (reduce #'v+ (list a1
                              (scale (* 2 (expt segment-time 1)) a2) (scale (* 3 (expt segment-time 2)) a3)
                              (scale (* 4 (expt segment-time 3)) a4) (scale (* 5 (expt segment-time 4)) a5)))
           acceleration
           (reduce #'v+ (list (scale 2 a2)
                              (scale (* 6 (expt segment-time 1)) a3) (scale (* 12 (expt segment-time 2)) a4)
                              (scale (* 20 (expt segment-time 3)) a5))))
     position))
  ;
  )


;; https://github.com/k-okada/jskeus/blob/7309fa24ff16cec872c9a3a044b599f1fcf1a18f/irteus/test/interpolator.l#L52
(defun pos-list-interpolation
  (pos-list ;; (list pos_1 pos_2 ... pos_N), pos_i is float-vector
   time-list ;; (list dtime_1 dtime_2 ... dtime_{N-1}), dtime_i is time[s] between time at pos_{i+1} - pos_i
   dt ;; dt [s]
   &key (interpolator-class minjerk-interpolator)
        ((:interpolator ip) (instance interpolator-class :init))
        (initial-time 0.0) (neglect-first) (vel-vector-list) (acc-vector-list))
  (let* ((data-list) (tm-list) (vel-data-list) (acc-data-list))
    (assert (= (length pos-list) (1+ (length time-list)))
            (format nil "check length of pos-list(~A) and tm-list(~A)"
                    (length pos-list) (length time-list)))
    (setq vel-vector-list
          (reverse
           (do ((i 0 (1+ i)) (vel-list))
               ((> i (length time-list)) vel-list)
               (if (or (= i 0) (= i (length time-list)))
                   (push (instantiate float-vector (length (car pos-list))) vel-list)
                 (let* ((v0 (scale (/ 1.0  (elt time-list (1- i)))
                                   (v- (elt pos-list i) (elt pos-list (1- i)))))
                        (v1 (scale (/ 1.0 (elt time-list i))
                                   (v- (elt pos-list (1+ i)) (elt pos-list i))))
                        (v (scale 0.5 (v+ v0 v1))))
                   (dotimes (i (length v)) (if (< (* (elt v0 i) (elt v1 i)) 0) (setf (elt v i) 0)))
                   (push v vel-list))))))
    (setq acc-vector-list
          (reverse
           (do ((i 0 (1+ i)) (acc-list))
               ((> i (length time-list)) acc-list)
               (if (or (= i 0) (= i (length time-list)))
                   (push (instantiate float-vector (length (car vel-vector-list))) acc-list)
                 (let* ((v0 (scale (/ 1.0  (elt time-list (1- i)))
                                   (v- (elt vel-vector-list i) (elt vel-vector-list (1- i)))))
                        (v1 (scale (/ 1.0 (elt time-list i))
                                   (v- (elt vel-vector-list (1+ i)) (elt vel-vector-list i))))
                        (v (scale 0.5 (v+ v0 v1))))
                   (dotimes (i (length v)) (if (< (* (elt v0 i) (elt v1 i)) 0) (setf (elt v i) 0)))
                   (push v acc-list))))))
    (format t "=INPUT~%")
    (format t "time ~A~%" time-list)
    (format t " pos ~A~%" pos-list)
    (format t " vel ~A~%" vel-vector-list)
    (format t " acc ~A~%" acc-vector-list)
    (send* ip :reset
           :position-list pos-list
           :time-list (let (r) (dolist (n time-list) (push (+ n (if r (car r) 0)) r)) (nreverse r)) ;; list of time[sec] from start for each control point
           (append
            (if vel-vector-list (list :velocity-list vel-vector-list))
            (if acc-vector-list (list :acceleration-list acc-vector-list))))
    (send ip :start-interpolation)
    (while (send ip :interpolatingp)
      (push (if (send ip :interpolatingp)
                (+ initial-time (send ip :time))
              (+ dt (car tm-list))) tm-list)
      (send ip :pass-time dt)
      (push (send ip :position) data-list)
      (if (find-method ip :velocity) (push (send ip :velocity) vel-data-list))
      (if (find-method ip :acceleration) (push (send ip :acceleration) acc-data-list))
      )
    (format t "=OUTPUT~%")
    (if (and vel-data-list acc-data-list)
        (mapcar #'(lambda (tm pos vel acc)
                    (format t "~7,5f ~7,3f ~13,1f ~13,1f~%"
                            tm (elt pos 0) (elt vel 0) (elt acc 0)))
                (reverse tm-list) (reverse data-list) (reverse vel-data-list) (reverse acc-data-list)))
    (append
     (list :data (if neglect-first (cdr (reverse data-list)) (reverse data-list))
           :time (if neglect-first (cdr (reverse tm-list)) (reverse tm-list)))
     (if (find-method ip :velocity)
         (list :velocity (if neglect-first (cdr (reverse vel-data-list)) (reverse vel-data-list))))
     (if (find-method ip :acceleration)
         (list :acceleration (if neglect-first (cdr (reverse acc-data-list)) (reverse acc-data-list))))
     )))


;; from https://github.com/euslisp/jskeus/pull/261
(defclass gnuplot
  :super propertied-object
  :slots (strm data data-length last-command debug)
  )

(defmethod gnuplot
  (:init (host &key (clear t) ((:debug _debug)))
	 (setq strm
	       (cond
		((string= (unix:gethostname) host)
		 (piped-fork "gnuplot"))
		(t
		 (piped-fork
		  "rsh" host
		  (format nil "(setenv DISPLAY ~A:0 ; cd ~A ; gnuplot)"
			  (unix:gethostname) (pwd)))
		 )
		))
	 (setq data-length 10)
	 (if clear (send self :clear))
         (setq debug _debug)
	 self)
  (:clear
   ()
   #-:cygwin
   (if (fboundp 'x::query-window-title-list)
       (let ((bef (x::query-window-title-list)) aft dif)
         (format strm "clear~%")
         (while (not dif)
           (setq aft (x::query-window-title-list))
           (setq dif (set-difference aft bef :test #'(lambda (a b) (= (cdr a) (cdr b))))))
         (setf (get self :win-id) (cdr (car dif)))
         )
     (format strm "clear~%"))
   #-:linux
   (format strm "clear~%")
   )
  ;; (send *G* :draw #f(0 1 2 3 4 5) #f(5 4 3 2 1 0) :xrange '(0 10) :yrange '(0 10) :title '("data1" "data2"))
  (:draw (&rest vs)
	 (setq last-command vs)
         (if debug (warn ";; :draw ~S~%" vs))
	 (let (str range xrange yrange title (clear nil) (line-width 1) (direction :right) (xscale 1.0) (xoffset 0.0) (type :lines))
	   (dotimes (i (length vs))
	     (if (eq (elt vs i) :range) (setq range (elt vs (1+ i))))
	     (if (eq (elt vs i) :xrange) (setq xrange (elt vs (1+ i))))
	     (if (eq (elt vs i) :yrange) (setq yrange (elt vs (1+ i))))
	     (if (eq (elt vs i) :title) (setq title (elt vs (1+ i))))
	     (if (eq (elt vs i) :clear) (setq clear (elt vs (1+ i))))
	     (if (eq (elt vs i) :line-width) (setq line-width (elt vs (1+ i))))
	     (if (eq (elt vs i) :direction) (setq direction (elt vs (1+ i))))
	     (if (eq (elt vs i) :xscale) (setq xscale (elt vs (1+ i))))
	     (if (eq (elt vs i) :xoffset) (setq xoffset (elt vs (1+ i))))
             (if (eq (elt vs i) :type) (setq type (elt vs (1+ i))))
	     )
	   (setq vs (remove :range vs))
	   (setq vs (remove range vs :test #'equal))
	   (setq vs (remove :xrange vs))
	   (setq vs (remove xrange vs :test #'equal))
	   (setq vs (remove :yrange vs))
	   (setq vs (remove yrange vs :test #'equal))
	   (setq vs (remove :title vs))
	   (setq vs (remove title vs :test #'equal))
	   (setq vs (remove :clear vs))
	   (setq vs (remove clear vs :test #'equal))
	   (setq vs (remove :line-width vs))
	   (setq vs (remove line-width vs :test #'equal))
	   (setq vs (remove :direction vs))
	   (setq vs (remove direction vs :test #'equal))
           (setq vs (remove :xscale vs))
           (setq vs (remove xscale vs :test #'equal))
           (setq vs (remove :xoffset vs))
           (setq vs (remove xoffset vs :test #'equal))
           (setq vs (remove :type vs))
           (setq vs (remove type vs :test #'equal))
	   ;;
	   (if clear (send self :clear))
           (case type
                 (:lines ;; default
                  (format strm "plot ")
                  (if (setq range (or range xrange))
                      (format strm "[~A:~A]" (first range) (second range))
                    (format strm "[]"))
                  (if yrange (format strm "[~A:~A]" (first yrange) (second yrange)))
                  (format strm " '-'")
                  (if title (format strm " title \"~A\"" (pop title)))
                  (format strm " w lp lw ~A" line-width)
                  (dolist (v (cdr vs))
                    (format strm ", '-'")
                    (if title (format strm " title \"~A\"" (pop title)))
                    (format strm " w lp lw ~A" line-width)
                    )
                  (format strm "~%")
                  (dolist (v vs)
                    (dotimes (i (length v))
                      (if (eq direction :left)
                          (format strm "~A ~A~%" (+ (* i xscale) xoffset) (elt v (1- (- (length v) i))))
                        (format strm "~A ~A~%" (+ (* i xscale) xoffset) (elt v i))))
                    (format strm "e~%"))
                  )
                 (:2dmap
                  (format strm "set pm3d map~%")
                  (format strm "unset ztics~%")
                  (if yrange (format strm "set cbrange [~A:~A]~%" (elt yrange 0) (elt yrange 1))
                    (format strm "set autoscale cb~%"))
                  (format strm "splot '-' with pm3d~%")

                  (dotimes (i (length (car vs)))
                    (dotimes (ii 2)
                      (dotimes (j (length vs))
                        (dotimes (jj 2)
                          (let ((x (+ (* (+ i ii) xscale) xoffset))
                                (y (+ j jj)) ;; 0 1 1 2 2 3 ...
                                (z (if (eq direction :left) (elt (elt vs j) (1- (- (length (car vs)) i))) (elt (elt vs j) i)))
                                )
                            (format strm "~A ~A ~A~%" x y z)
                            )))
                      (format strm "~%")
                      ))

                  (format strm "e~%")
                  )
                 (t (warn "unknown type ~A~%" type))
                 )
	   ))
  (:save (f &key (type "postscript eps color \"Times-Roman\" 24"))
	 (format strm "set terminal ~A~%" type)
	 (format strm "set output ~s~%" f)
	 (if last-command (send-lexpr self :draw last-command)
	   (format strm "replot~%"))
	 (format strm "set output~%")
	 (format strm "set terminal x11~%")
	 (if last-command (send-lexpr self :draw last-command)
	   (format strm "replot~%"))
	 )
  (:replot () (format strm "replot~%"))
  (:reset () (format strm "reset~%"))
  (:command (msg) (format strm "~A~%" msg))
  (:quit () (format strm "quit~%"))
  )

(defun gnuplot (&key (host (unix:gethostname)))
  (instance gnuplot :init host))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; plot function for 2d or 3d plot
;; argument
;;   ordinate-list ;; list of data for ordinate axis
;;     2D = (list (list y00 y01 ... y0n), ... (list ym0 ym1 ... ymn))
;;     3D = (list (list z00 z01 ... z0n), ... (list zm0 zm1 ... zmn))
;;   abscissa-list ;; list of data for abscissa axes
;;     2D = (list x0 x1 ... xn)
;;     3D = (list xylist0 ... xylistn) ;; xylist = (list x y)
;;   keylist ;; list of data's key
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun user::graph-view
  (ordinate-list
   &optional (abscissa-list (let ((idx -1)) (mapcar #'(lambda (x) (incf idx)) (make-list (length (car ordinate-list)))))) ;; range function
   &key (title "Graph") (xlabel "X") (ylabel "Y") (zlabel "Z")
        (dump-graph nil) (graph-fname (format nil "~A.eps" (substitute #\_ (elt " " 0) title)))
	;;(mode "points")
        (mode "lines")
	keylist xrange yrange zrange
	x11 additional-func
	no-dump ((:graph-instance gp) (if (boundp 'user::*gp*) user::*gp* (setq user::*gp* (gnuplot))))
	(fname (format nil "data~A" (sys::address gp))))
  (labels ((gen-range-string
            (range)
            (if range (format nil "[~A:~A]" (car range) (cadr range)) "[]"))
           (2d-or-3d (r-2d r-3d) (if (atom (car abscissa-list)) (eval r-2d) (eval r-3d))))
    (unless keylist (setq keylist (let ((idx -1)) (mapcar #'(lambda (x) (incf idx)) (make-list (length ordinate-list))))))
    ;; dump dat file
    (unless no-dump
      (with-open-file
       (f (format nil "/tmp/~A.dat" fname) :direction :output)
       (format f (2d-or-3d "# x vals..~%" "# x y vals..~%"))
       (dotimes (i (length abscissa-list))
         (if (atom (car abscissa-list))
             (format f "~A " (elt abscissa-list i))
           (format f "~A ~A " (elt (elt abscissa-list i) 0) (elt (elt abscissa-list i) 1)))
         ;;(dolist (d ordinate-list) (format f "~A " (elt d i)))
	 (dolist (d ordinate-list)
	   (if (< i (length d))
	       (format f "~A " (elt d i))))
         (format f "~%")
         )
       )
      )

    ;; plot
    (mapcar #'(lambda (d1 d2)
                (send gp :command (format nil "set ~A \"~A\"" d1 d2)))
            '(user::title user::xlabel user::ylabel user::zlabel)
            (list title xlabel ylabel zlabel))
    (if additional-func (funcall additional-func))
    (dotimes (i (length ordinate-list))
      (send gp :command
            (format nil "~A \"/tmp/~A.dat\" using ~A title \"~A\" with ~A"
                    (case
                     i
                     (0 (apply #'format
                               (list nil
                                     (2d-or-3d "plot ~A ~A" "splot ~A ~A ~A")
                                     (gen-range-string xrange)
                                     (gen-range-string yrange)
                                     (2d-or-3d nil (gen-range-string zrange)))))
                     (t "replot"))
                    fname
                    (format nil "~A:~A" (2d-or-3d "1" "1:2") (+ i (2d-or-3d 2 3)))
                    (elt keylist i)
                    mode))
      )
    (if x11 (send gp :command "set terminal X11"))
    (when dump-graph
      (unix:usleep 200000)
      (send gp :save graph-fname)
      (unix:usleep 200000))
    ))

;; Main function
(defun draw-graph (&optional (interval 0.001))
  ;; 10 points trajectory (#f(0) #f(1) ... #f(9))
  (let ((traj-points 10) pos-list time-list r)
    (dotimes (i traj-points)
      (setq pos-list (append pos-list (list (float-vector i)))))
    (setq time-list (make-list (- traj-points 1) :initial-element 0.001))
    (setq r (pos-list-interpolation pos-list time-list
                                    interval))
    (setq r-pos (mapcar #'(lambda (x) (elt x 0)) (cadr (memq :data r))))
    (setq r-vel (mapcar #'(lambda (x) (/ (elt x 0) 10)) (cadr (memq :velocity r))))
    (setq r-acc (mapcar #'(lambda (x) (/ (elt x 0) 100)) (cadr (memq :acceleration r))))
    (graph-view (list r-pos r-vel r-acc) (cadr (memq :time r)) :keylist (list "position" "velocity" "acceleration"))
    r-pos))