hrp2jsk-fridge-sample.l

(require "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsk-interface.l")
(require "models/room73b2-hitachi-fiesta-refrigerator-object.l")

(hrp2jsk-init)
(send *ri* :start-auto-balancer)
(send *ri* :start-st)

(setq *fridge* (room73b2-hitachi-fiesta-refrigerator))
(send *fridge* :translate #f(1000 0 50))
(send *fridge* :rotate 3.14 :z)

(objects (list *hrp2jsk* *fridge*))
(send *hrp2jsk* :fullbody-inverse-kinematics
      (list
        (send (send *hrp2jsk* :rleg :end-coords) :copy-worldcoords)
        (send (send *hrp2jsk* :lleg :end-coords) :copy-worldcoords)
        (send (send *fridge* :handle-door1) :copy-worldcoords))
      :link-list
      (list
        (send *hrp2jsk* :link-list (send (send *hrp2jsk* :rleg :end-coords) :parent))
        (send *hrp2jsk* :link-list (send (send *hrp2jsk* :lleg :end-coords) :parent))
        (send *hrp2jsk* :link-list (send (send *hrp2jsk* :larm :end-coords) :parent)))
      :move-target
      (list
        (send *hrp2jsk* :rleg :end-coords)
        (send *hrp2jsk* :lleg :end-coords)
        (send *hrp2jsk* :larm :end-coords))
      :translation-axis (list t t t)
      :rotation-axis (list t t :z))
(send *ri* :angle-vector (send *hrp2jsk* :angle-vector))
(send *ri* :go-pos 1 0 0)