test-zmp.l

(defun calc-zmp-diff-from-two-poses
    (key-pose-av-list key-pose-rc-list key-pose-tm-list
                      &key (dt 0.005) (debug-view nil) (robot *robot*))
  (pprint `(calc-zmp-diff-from-two-poses ,key-pose-av-list
                                         ,(mapcar #'(lambda (c)
                                                      `(make-coords :pos ,(send c :worldpos)
                                                                    :rot ,(send c :worldrot)))
                                                  key-pose-rc-list)
                                         ,key-pose-tm-list))
  (let* ((data (pos-list-interpolation
                key-pose-av-list key-pose-tm-list dt :interpolator-class linear-interpolator))
         (av-list (cadr (memq :data data)))
         (tm-list (cadr (memq :time data))))
    (send robot :angle-vector (car key-pose-av-list))
    (send robot :move-coords (car key-pose-rc-list) (car (send *robot* :links)))
;;    (dotimes (i 2)
      (send robot :calc-zmp
            (car key-pose-av-list)
            (car key-pose-rc-list)
            :pzmpz (elt (send (send robot :foot-midcoords) :worldpos) 2)
            :dt dt)
      ;;)
    (let ((ret
           (mapcar #'(lambda (av ttm)
                       (send robot :angle-vector av)
                       (send robot :fix-leg-to-coords (make-coords)) ;; TODO fix leg to coords is assumed
                       (send robot :move-centroid-on-foot :both '(:rleg :lleg) :min-loop 2) ;; Mimic AutoBalancer behaviour
                       (if debug-view (send *irtviewer* :draw-objects :flush nil))
                       (let* ((fm (send (send robot :foot-midcoords) :worldpos))
                              (czmp (send robot :calc-zmp
                                          av (send (car (send robot :links)) :copy-worldcoords)
                                          :pzmpz (elt fm 2)
                                          :dt dt)))
                         (when debug-view
                           (send fm :draw-on :flush nil :color #f(0 1 0) :size 200)
                           (send-all (send robot :support-polygons) :draw-on :flush nil)
                           (send czmp :draw-on :flush t :color #f(1 0 0) :size 250)
                           )
                         ;; Return value
                         (list czmp fm (send robot :centroid) ttm av)))
                   av-list tm-list)))
      ;;(subseq ret 2) ;; Why?
      ret)))
      
      (calc-zmp-diff-from-two-poses
   (list #f(0.0
      0.0
      -26.0
      50.0
      -24.0
      0.0
      0.0
      0.0
      0.0
      -26.0
      50.0
      -24.0
      0.0
      0.0
      0.0
      0.0
      0.0
      0.0
      10.0
      -10.0
      0.0
      -25.0
      0.0
      0.0
      -10.0
      15.0
      10.0
      10.0
      0.0
      -25.0
      0.0
      0.0
      -10.0
      -15.0
      )
    #f(0.0
       0.0
       -26.0
       50.0
       -24.0
       0.0
       0.0
       0.0
       0.0
       -26.0
       50.0
       -24.0
       0.0
       0.0
       0.0
       0.0
       0.0
       40.0
       50.0
       -30.0
       -10.0
       -120.0
       -25.0
       -5.0
       -20.0
       60.0
       50.0
       30.0
       10.0
       -120.0
       25.0
       5.0
       -20.0
       -60.0
       )
    #f(0.0
       0.0
       -26.0
       50.0
       -24.0
       0.0
       0.0
       0.0
       0.0
       -26.0
       50.0
       -24.0
       0.0
       0.0
       0.0
       0.0
       0.0
       40.0
       50.0
       -30.0
       -10.0
       -120.0
       -25.0
       -5.0
       -20.0
       60.0
       -29.8788
       22.4374
       -18.2877
       -64.9042
       20.2272
       12.3016
       8.10165
       -60.0
       )
    #f(0.0
       0.0
       -26.0
       50.0
       -24.0
       0.0
       0.0
       0.0
       0.0
       -26.0
       50.0
       -24.0
       0.0
       0.0
       0.0
       0.0
       0.0
       0.0
       10.0
       -10.0
       0.0
       -25.0
       0.0
       0.0
       -10.0
       15.0
       10.0
       10.0
       0.0
       -25.0
       0.0
       0.0
       -10.0
       -15.0
       ))
   (list (make-coords :pos #f(-7.80531 -1.59029 661.702) :rot #2f((1.0 0.000368 2.775558e-16) (-0.000368 1.0 0.0) (-2.775557e-16 -1.022521e-19 1.0)))
    (make-coords :pos #f(-10.4309 -1.58934 661.702) :rot #2f((1.0 0.000367 1.665335e-16) (-0.000367 1.0 4.865168e-22) (-1.665334e-16 -6.154345e-20 1.0)))
    (make-coords :pos #f(-33.3687 -0.016529 661.702) :rot #2f((1.0 1.148171e-05 1.665335e-16) (-1.148171e-05 1.0 7.333339e-21) (-1.665335e-16 -9.245428e-21 1.0)))
    (make-coords :pos #f(-7.80531 -1.59029 661.702) :rot #2f((1.0 0.000368 2.220446e-16) (-0.000368 1.0 2.467471e-22) (-2.220446e-16 -8.204842e-20 1.0))))
   (list 1.10538 0.555183 0.852216))