test program for min-max-table of hrp2 chest

test-hrp2-chest-min-max-table.l

(load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l")
(load "package://hrpsys_ros_bridge/euslisp/datalogger-log-parser.l")

(defun init
  ()
  (hrp2jsknt-init)
  (setq *robot* *hrp2jsknt*)
  (objects (list *robot*))
  )

(defun test
  ()
  ;; pose0                                                                                                                                                                                                                                                                                                             
  (send *robot* :init-pose)
  (send *robot* :larm :shoulder-r :joint-angle 90)
  (send *robot* :rarm :shoulder-r :joint-angle -90)
  (send *ri* :angle-vector (send *robot* :angle-vector) 5000)
  (send *ri* :wait-interpolation)

  (send *ri* :set-log-maxlength 1500)
  (send *ri* :start-log)

  ;; pose1                                                                                                                                                                                                                                                                                                             
  (send *robot* :torso :waist-p :joint-angle 59.4121)
  (send *robot* :torso :waist-y :joint-angle 37.5852)
  (send *ri* :angle-vector (send *robot* :angle-vector) 5000)
  (send *ri* :wait-interpolation)

  (send *ri* :save-log "/tmp/pose1")

  ;; pose2                                                                                                                                                                                                                                                                                                             
  ;; (send *robot* :torso :waist-p :joint-angle 30.5979)                                                                                                                                                                                                                                                               
  ;; (send *robot* :torso :waist-y :joint-angle 42.2031)                                                                                                                                                                                                                                                               
  ;; (send *ri* :angle-vector (send *robot* :angle-vector) 5000)                                                                                                                                                                                                                                                       
  ;; (send *ri* :wait-interpolation)                                                                                                                                                                                                                                                                                   

  )

(defun load-log
  ()
  (setq *log* (instance datalogger-log-parser-controller :init "/home/leus/tmp/pose1/pose1_HRP2JSKNT_20151026191803" :robot *robot*))
  (while (not (send *log* :eof-p))
      (send *log* :state)
    (send *robot* :angle-vector (send *log* :potentio-vector))
    (format t "~a, ~a # waist-p, waist-y~%" (send (send *robot* :torso :waist-p) :joint-angle) (send (send *robot* :torso :waist-y) :joint-angle))
    )
  )