mmurooka

mc_rtcを用いた人型ロボットのコントローラ作成

本ドキュメントは，Humanoid Virtual Athletics Challenge 2023 技術講習会 (2023/09/24)で利用する説明資料です．

０．HVACにおけるmc_rtcの利用例

2021年出場時のコントローラはmc_rtcを利用しています． 不整地歩行，階段登り，ジャンプなどの動作を実行できました．

• スライド：https://drive.google.com/file/d/1JXMHQoOkRj0YW3g4sy1IIKsAg4c1UVuP/view?usp=sharing
• 動画：https://drive.google.com/file/d/1jP5NVx3h8Ve3DX90CJLhUdsz7ng8M9h_/view?usp=sharing

mmurooka

(defun test1
    (&aux a b c)
  a
  )

(defun test2
    ()
  (let (a b c)
    a
    ))

mmurooka

(load "models/tray-object.l")
(load "package://pr2eus/pr2.l")

(defun pr2-rotate-tray
    ()
  (setq *obj* (tray))
  (send *obj* :put :long-left-coords
        (make-cascoords :parent *obj* :pos (float-vector 0 200 0) :rpy (list 0 pi/2 0) :name :long-left-coords))
  (send *obj* :put :long-right-coords
        (make-cascoords :parent *obj* :pos (float-vector 0 -200 0) :rpy (list 0 pi/2 0) :name :long-right-coords))

mmurooka

(defun make-tube
  (&key
   (outer-r 100)
   (inner-r 90)
   (center-theta pi/2)
   (height 200)
   (sweep-vec (float-vector 0 0 height))
   (divide-num 10)
   )
  (let* ((outer-point-list)

mmurooka

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; util
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun make-default-robot-link
  (len
   radius
   axis
   name
   &key

mmurooka

leus@cygnus:~/ros/indigo_parent/src/euslisp/Euslisp$ find ./ -name "*.l" | xargs grep normalize-vector -A5 -B2
./lib/demo/hehehe.l-	(send pole4 :translate #f(15 -15 2))
./lib/demo/hehehe.l-	(setq r1 (body+ base pole1 pole2 pole3 pole4 top))
./lib/demo/hehehe.l:	(setq pln (make-plane :normal (normalize-vector #f(1 2 8))
./lib/demo/hehehe.l-			      :point #f(0 0 8)))
./lib/demo/hehehe.l-	(setq part1 (body/ r1 pln))
./lib/demo/hehehe.l-	(send part1 :magnify 10.0))
./lib/demo/hehehe.l-    )
./lib/demo/hehehe.l-
--

mmurooka

(setq *robot* (instance pr2-robot :init))

(setq *J*
      (send *robot* :calc-jacobian-from-link-list
            (send *robot* :link-list (send (send *robot* :larm :end-coords) :parent)
                  (send (car (send *robot* :larm :joint-list)) :child-link))
            :move-target (send *robot* :larm :end-coords)))
(setq *torque-vector* (float-vector 5 5 5 5 5 5 5))
;; (send *ri* :state :torque-vector)から左腕だけ取ってくる                                                                                                              

mmurooka

(defun draw-cube-edge-direction
  ()
  (setq *obj* (make-cube 200 200 200))
  (objects (list *obj*))
  (dotimes (i (length (send *obj* :edges)))
    (with-set-color-line-width
     *viewer* #f(1 1 1) 5
     (send *viewer* :draw-arrow
           (send (elt (send *obj* :edges) i) :point 0)
           (send (elt (send *obj* :edges) i) :point 1)))

mmurooka

(defun test
  ()
  (load "package://hrpsys_ros_bridge_tutorials/euslisp/samplerobot-interface.l")
  (setq *robot* (instance samplerobot-robot :init))
  (send *robot* :fix-leg-to-coords (make-coords))

  ;; add additional end-coords
  (let* ((parent-link
          (elt (send *robot* :links) 0))
         )

mmurooka

(defun convert-array-to-string
  (a)
  (let* ((ret "")
         )
    (dotimes (i (length a))
      (cond ((= i 0)
             (setq ret (format nil "~a" (elt a i)))
             )
            (t
             (setq ret (format nil "~a, ~a" ret (elt a i)))