From bb891e897c1be4be367516a1e7a885d3b486753a Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Tue, 30 Nov 2021 18:07:07 +0900
Subject: [PATCH] add more example on graph-view/online plotting

---
 irteus/demo/walk-motion.l | 14 ++++++++++++++
 irteus/irtmath.l          |  6 ++++++
 irteus/kalmanlib.l        | 10 ++++++++++
 3 files changed, 30 insertions(+)

diff --git a/irteus/demo/walk-motion.l b/irteus/demo/walk-motion.l
index f49d82ff..af389067 100644
--- a/irteus/demo/walk-motion.l
+++ b/irteus/demo/walk-motion.l
@@ -295,6 +295,20 @@
              tm-list (reverse zmp-list) (reverse cog-list) (reverse ref-zmp-list2))
      )
     (unless (or (null x::*display*) (= x::*display* 0))
+      ;; graph-view
+      (graph-view (list (reverse zmp-list) (reverse cog-list) (reverse ref-zmp-list2)) tm-list
+                  :xlabel "Time [s]" :ylabel "ZMP X [mm]"
+                  :keylist '("card zmp" "cog" "refmp"))
+      ;; online-version
+      (setq *gnuplot* (gnuplot))
+      (send *gnuplot* :proc-length (length tm-list))
+      (mapcar #'(lambda (tm zmp cog refzmp)
+                  (send *gnuplot* :proc-one (list zmp cog refzmp)
+                        :direction :left
+                        :title '("cart zmp" "cog" "refzmp")
+                        ))
+              tm-list (reverse zmp-list) (reverse cog-list) (reverse ref-zmp-list2))
+      ;; raw-command version
       (let ((gp-command-list (list
                               "set xlabel 'Time [s]';"
                               "set ylabel 'ZMP X [mm]';"
diff --git a/irteus/irtmath.l b/irteus/irtmath.l
index 3528ecc5..55400172 100644
--- a/irteus/irtmath.l
+++ b/irteus/irtmath.l
@@ -666,6 +666,12 @@
 #|
 (defun lms-draw (r point-list)
   (let ()
+    ;; graph-view
+    (graph-view (list (mapcar #'vector-y point-list)) (mapcar #'vector-x point-list)
+                :mode "point" :xrange '(0 80) :yrange '(0 50)
+                :keylist (list "data")
+                :graph-instance *gnuplot*)
+    ;; raw-command version
     (send *gnuplot* :command (format nil "plot [0:80][0:50] ~A * x + ~A title \"regression\", '-' with point title \"data\"~%~Ae"
                                      (/ (elt (car r) 0) (- (elt (car r) 1)))
                                      (/ (cadr r) (- (elt (car r) 1)))
diff --git a/irteus/kalmanlib.l b/irteus/kalmanlib.l
index 6bf7ad0d..23fe65c1 100644
--- a/irteus/kalmanlib.l
+++ b/irteus/kalmanlib.l
@@ -187,6 +187,9 @@
         (f (format nil "data_~a.dat" suffix) :direction :output)
       (dotimes (i (length x))
         (format f "~A ~A~%" (elt x i) (elt y i))))
+    ;; online gnuplotting
+    (if (not (boundp '*g*))
+        (send *g* :proc-length (length x)))
 
     ;; write estimated data
     (with-open-file
@@ -197,6 +200,13 @@
            (setq z (send kf :proc (float-vector (elt x i) (elt y i)) :adaptive t)))
           (t
            (setq z (send kf :proc (float-vector (elt x i) (elt y i))))))
+        ;; online gnuplotting
+        (send *g* :proc-one (list (elt y i) (elt z 1) (send kf :error) (aref (send kf :P) 1 1) -0.37727)
+              :direction :left
+              :xrange '(0 100)
+              :yrange '(-0.6 0)
+              :y2tics (list nil nil t t nil)
+              :title '("ovserved" "estimated" "error" "P covariance" "baseline"))
         (format f "~A ~A ~A ~A~%" (elt x i) (elt z 1) (send kf :error) (aref (send kf :P) 1 1))))
 
     ;; write graph data