jaxon-holding.l

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

(defun setup-real
  ()
  (jaxon-init)
  (setq *robot* *jaxon*)
  (objects (list *robot*))
  (send *ri* :start-grasp)
  )


;; 実験1
;; この関節角で
;; abcあり，stなしで地面に立たせて，
;; (send *ri* :set-ref-force (float-vector 0 0 -20) 2000 :arms)
;; して箱を渡すと抱えながら立てる．
;; stを入れるとガタガタした．

;; 実験2
;; この関節角で
;; abcあり，stありで地面に立たせて，
;; (send *ri* :set-ref-force (float-vector 0 0 -10) 2000 :rarm)
;; (send *ri* :set-ref-force (float-vector 0 0 -30) 2000 :larm)
;; して箱を渡すと抱えながら立てる．
;; (send *ri* :go-pos 0.2 0 0) (send *ri* :go-pos 0 0 30) などで歩ける．

(defun test1
  ()
  (send *robot* :reset-manip-pose)
  (send *robot* :head :neck-p :joint-angle 0)
  (send *robot* :arms :move-end-pos #f(300 0 0) :world)
  (send *robot* :larm :move-end-pos #f(150 50 300) :world)
  (send *robot* :larm :move-end-rot 50 :x :world)
  (send *robot* :rarm :move-end-pos #f(125 -50 0) :world)
  (send *robot* :rarm :move-end-rot 20 :z :world)
  (send *robot* :rarm :move-end-rot -50 :x :world)
  (send *robot* :rarm :elbow-p :joint-angle -60)
  (send *robot* :rarm :collar-y :joint-angle 60)
  )

;;(send *ri* :angle-vector (send *robot* :angle-vector) 5000)