zmp.l

(require "package://hrpsys_ros_bridge_tutorials/euslisp/jaxon-interface.l")
(require "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l")
(setq *robot* (instance jaxon-robot :init))
(setq *robot2* (instance jaxon-robot :init))
(setq *robot* (instance hrp2jsknt-robot :init))
(setq *robot2* (instance hrp2jsknt-robot :init))
(setq *av0* (send *robot* :reset-pose))
(setq *av1* (send *robot* :reset-manip-pose))
(setq *duration* 1.0)                   ;sec                                                               
(load "~/prog/euslib/demo/nozawa/motion/eval-motion-samples.l")
(load "~/prog/euslib/irteus_proposals/motion-lib-proposal.l")
(objects (list *robot*))

(defun get-robot-state-data-arg (av d)
  (send *robot* :worldcoords)
  (send *robot* :angle-vector av)
  (send *robot* :fix-leg-to-coords (make-cascoords) '(:rleg :lleg))
  (list :angle-vector (send *robot* :angle-vector)
        :root-coords (send (send *robot* :body_lk) :copy-worldcoords)
        :time d
        :contact-state '(:support :support)
        :end-coords-list (list (send *robot* :rleg :end-coords :copy-worldcoords)
                               (send *robot* :lleg :end-coords :copy-worldcoords))
        ))

(defun calc-zmp-from-avs (av0 av1 duration)
  (setq *rsl* (convert-to-robot-state-data
               (list (get-robot-state-data-arg av0 0)
                     (get-robot-state-data-arg (scale 0.5 (v+ av0 av1)) (/ duration 2.0))
                     (get-robot-state-data-arg av1 duration))))
  (setq *rs-all0*
        (calc-robot-state-data-list-with-wrench-torque
         *rsl*
         :robot *robot*
         :all-limbs '(:rleg :lleg)))
  (print (send *robot* :calc-zmp-from-forces-moments
        (car (send (car *rs-all0*) :wrench-list))
        (cadr (send (car *rs-all0*) :wrench-list)) :limbs '(:rleg :lleg) :fz-thre -10000000000000
        :force-sensors (list (send *robot* :force-sensor "rfsensor")
                             (send *robot* :force-sensor "lfsensor"))))
  (send *robot* :angle-vector av0)
  (send *robot2* :angle-vector av1)
  (send *robot* :fix-leg-to-coords (make-cascoords) '(:rleg :lleg))
  (send *robot2* :fix-leg-to-coords (make-cascoords) '(:rleg :lleg))
  (send *irtviewer* :objects (list *robot2* *robot*))
  ;;(send *irtviewer* :draw-objects)                                                                       
  )