foot steps test

test-set-foot-steps.l

(load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l")

(defun setup
  ()
  (hrp2jsknts-init)
  (send *ri* :start-auto-balancer)
  (setq *robot* *hrp2jsknts*)
  (objects (list *robot*))
  )

(defun test-set-foot-steps
  ()
  (let* (lleg-coords-orig
         rleg-coords-orig
         lleg-coords-new)
    ;; set initial pose                                                                                                                                   
    (send *robot* :reset-pose)
    (send *robot* :fix-leg-to-coords (make-coords))
    (setq lleg-coords-orig (send (send *robot* :lleg :end-coords) :copy-worldcoords))
    (setq rleg-coords-orig (send (send *robot* :rleg :end-coords) :copy-worldcoords))
    ;; set target pose                                                                                                                                    
    (send *robot* :lleg :move-end-pos #f(0 70 0) :world)
    (send *robot* :lleg :move-end-rot 20 :z :world)
    (setq lleg-coords-new (send (send *robot* :lleg :end-coords) :copy-worldcoords))
    ;; set name                                                                                                                                           
    (send lleg-coords-orig :name "lleg")
    (send rleg-coords-orig :name "rleg")
    (send lleg-coords-new :name "lleg")
    ;; send footstep                                                                                                                                      
    (setq footstep-list (list rleg-coords-orig lleg-coords-new))
    (send *ri* :set-foot-steps-no-wait footstep-list)
    (send *ri* :wait-foot-steps)
    ))

(warn "(setup)~%")
(warn "(test-set-foot-steps)~%")