assocの使い方について（ロボット同士の手をつながせたい）

ロボット同士の手をつないで、腕を動かして手をつなげる範囲を調べたいのですが、２つの手の固定のさせ方がよく分からず困っています。ロボット１の　:rarm :end-coords　と　ロボット２の:larm :end-coords　を連結させたいです。ドキュメントを読むと使えそうなのですが、実際には出来ませんでした。（一つ目、二つ目）何かアドバイス等ございましたらご教示ください。

１つ目　手同士をassocする　片方の手を動かすともう一つはついてこない

roseus samplerobot-interface.l
setq *sr1* (samplerobot)
setq *sr2* (samplerobot)
send *sr1* :translate (float-vector 0 -800 0)
send *sr1* :larm :shoulder-r :joint-angle 12
send *sr2* :rarm :shoulder-r :joint-angle -12
send *sr1* :larm :end-coords :assoc (send *sr2* :rarm :end-coords)
send *sr1* :larm :move-end-pos (float-vector 30 0 10)

screenshot from 2016-12-08 13 56 36

2つ目　手の間に座標を置いて、座標に２つの手をassocする　矢印を動かすと手はついてこない

setq *sr1* (samplerobot)
setq *sr2* (samplerobot)
load "models/arrow-object.l"
 setq *arrow* (arrow)
send *sr1* :translate (float-vector 0 -800 0)
send *arrow* :translate (float-vector 38.639 -400 662.334)
objects (list *sr1* *sr2* *arrow*)
send *sr2* :rarm :shoulder-r :joint-angle -12
send *sr1* :larm :shoulder-r :joint-angle 12
send *arrow* :translate (float-vector 41 375 670)
objects (list *sr1* *sr2* *arrow*)
send *arrow* :assoc (send *sr2* :rarm :end-coords)
send *arrow* :assoc (send *sr1* :larm :end-coords)
send *arrow* :translate (float-vector 300 0 0)

screenshot from 2016-12-08 14 41 30

3つ目　手と矢印をassocして、手を動かすと動く

roseus samplerobot-interface.l
load "models/arrow-object.l"
setq *arrow* (arrow)
setq *sr1* (samplerobot)
send *arrow* :translate (float-vector 38.639 323.434 662.334)
send *arrow* :worldcoords
objects (list *sr1* *arrow*)
send *sr1* :larm :end-coords :assoc *arrow*
send *sr1* :larm :move-end-pos (float-vector 100 0 0)

screenshot from 2016-12-08 13 48 58

これは難しいですね．基本的にはロボットのリンク構造はTreeになっていて、そのルートは腰になっています．なので、手と手をつなぐとルートが２つで来てしまってうまくいっていないんだと思います．

(load "irteus/demo/sample-robot-model.l")
(defun samplerobot () (let (r) (setq r (instance sample-robot :init)) (send r :reset-pose) r))

(setq *sr1* (samplerobot))
(setq *sr2* (samplerobot))
(send *sr1* :translate (float-vector 0 -800 0))
(send *sr1* :larm :shoulder-r :joint-angle 30)
(send *sr2* :rarm :shoulder-r :joint-angle 30)
(objects (list *sr1* *sr2*))
(send *sr1* :larm :end-coords :assoc *sr2*)
(dotimes (i 20)
  (send *sr1* :larm :move-end-pos (float-vector 0 0 10))
  (send *irtviewer* :draw-objects)
  (x::window-main-one)
  (unix:usleep 10000)
  )

とすると、ロボット１腰 -> ロボット１手先 -> ロボット２腰　とつなげています．一応っぽいかんじになります．

walk_2_robots_1

でも、

(setq *sr1* (samplerobot))
(setq *sr2* (samplerobot))
(send *sr1* :translate (float-vector 0 -800 0))
(send *sr1* :larm :shoulder-r :joint-angle 30)
(send *sr2* :rarm :shoulder-r :joint-angle 30)
(send *sr1* :larm :end-coords :assoc *sr2*)
(objects (list *sr1* *sr2*))
(dotimes (i 20)
  (send *sr1* :locate (float-vector 10 0 0))
  (send *sr1* :larm :shoulder-p :joint-angle (* 50 (sin (/ i 2.0))))
  (send *irtviewer* :draw-objects)
  (x::window-main-one)
  (unix:usleep 10000)
  )

という風に手をふりだすと大変なことになります．

walk_2_robots_2

ということで、やっぱ　ロボット１腰 -> ロボット２腰　というつなぎ方しか無いのかなぁ、という気がします．

(setq *sr1* (samplerobot))
(setq *sr2* (samplerobot))
(send *sr1* :translate (float-vector 0 -800 0))
(send *sr1* :larm :shoulder-r :joint-angle 30)
(send *sr2* :rarm :shoulder-r :joint-angle 30)
(send *sr1* :assoc *sr2*)
(objects (list *sr1* *sr2*))
(dotimes (i 20)
  (send *sr1* :locate (float-vector 10 0 0))
  (send *sr1* :larm :shoulder-p :joint-angle (* 50 (sin (/ i 2.0))))
  (send *sr2* :rarm :inverse-kinematics (send *sr1* :larm :end-coords) :rotation-axis nil)
  (send *irtviewer* :draw-objects)
  ;;                                                                                                 
  (x::window-main-one)
  (unix:usleep 10000)
  )

とい感じにして、結局ロボット１の手先に合わせてロボット２の手先をIkを解くという感じです．

walk_2_robots_3

結局、ロボット１と２はそれぞれ別のもので、assocとかしなくて、１が動かした手に合わせて、２の方も手先の位置と腰の位置をうまく合わせる、という感じで、上のサンプルプログラムはうまく移動距離を合わせているけど、手の振りに合わせて、腰を動かすか手を動かすか、逆に、２の方が手先に追従できなけれあ１も調整すべきか、とか、面白い問題にもなりそうですが、当面は見た目が動けばいいのではないかと思うので、上のどれかで使って見れるといいと思います．

ご教示ありがとうございました。最後の方法で、サンプルロボットでやりたいことができました。

(ペッパーと人のモデルに変えてやってみたところ、 send *pepper* :larm :move-end-pos (float-vector 10 0 0) すら失敗する別の問題が発生しました・・・これは何だったのか https://github.com/jsk-ros-pkg/jsk_robot/pull/637 https://github.com/jsk-ros-pkg/jsk_robot/pull/645 )

手に５軸しか無いので、:move-end-posだと回転の拘束がかかってうまく行かないですね．例えば、

（send *pepper* :larm :move-end-pos #f(10 0 0)  :world :rotation-axis nil)

でしょうか．

-- ◉ Kei Okada

2016-12-12 15:26 GMT+09:00 Kanae Kochigami notifications@github.com:

ご教示ありがとうございました。最後の方法で、サンプルロボットでやりたいことができました。

(ペッパーと人のモデルに変えてやってみたところ、 send pepper :larm :move-end-pos (float-vector 10 0 0) すら失敗する別の問題が発生しました・・・これは何だったのか jsk-ros-pkg/jsk_robot#637 https://github.com/jsk-ros-pkg/jsk_robot/pull/637 jsk-ros-pkg/jsk_robot#645 https://github.com/jsk-ros-pkg/jsk_robot/pull/645 )

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/euslisp/jskeus/issues/405#issuecomment-266352581, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3BCq9ZHmMTw3cR-1kPixAS1dP0Lsks5rHOkbgaJpZM4LHdIm .

すみません、assocの使い方ではないのですが、逆運動学の解き方で困っていることが２つあります。

:use-fullbody t にして、かがんで解こうとしそうな座標を与えても、人モデルがかがまない (下のコードで(main 180) などとして確認)
他のロボット（ペッパー）にぶつかったら失敗するようにしてほしい

人の身長を小さくすると、ペッパーの腕が動かしにくくなるはずで、その時の範囲を求めたいのですが、人が直立不動でロボットとの干渉を考えない逆運動学を解くと、背が小さいほうが有利になり幅広く出てきてしまいます。

ドキュメントを探しましたが、jmanualでメソッドがずらりと書かれているものしか見つけられていません・・・もし何かアドバイスがあればご教示ください。

#!/usr/bin/env roseus
(require :pepper-interface "package://peppereus/pepper-interface.l")
;; ファイルパスを直す必要がある
(load "/home/kochigami/catkin_ws/src/EusLisp/models/human-1100-robot.l") 
(load "/home/kochigami/catkin_ws/src/EusLisp/models/human-1200-robot.l") 
(load "/home/kochigami/catkin_ws/src/EusLisp/models/human-1300-robot.l")  
(load "/home/kochigami/catkin_ws/src/EusLisp/models/human-1400-robot.l") 
(load "/home/kochigami/catkin_ws/src/EusLisp/models/human-1500-robot.l") 
(load "/home/kochigami/catkin_ws/src/EusLisp/models/human-1600-robot.l") 
(load "/home/kochigami/catkin_ws/src/EusLisp/models/human-1700-robot.l") 
(load "/home/kochigami/catkin_ws/src/EusLisp/models/human-1800-robot.l") 

(defun main (size)
  (setq *pepper* (pepper))
  (make-human size)
  (calc-distance-between-human-and-robot size)
  (set-arm size)
  (objects (list *pepper* *human*))
  (send *human* :rarm :inverse-kinematics
        (send (send *pepper* :rleg :end-coords) :copy-worldcoords) ;; pepperの足元を指定
        :rotation-axis nil
        :use-fullbody t
        :debug-view :no-message)
  (send *irtviewer* :draw-objects)
  (x::window-main-one) 
 )

(defun make-human (size)
    (case size
        (110
         (setq *human* (human-1100)))
        (120
         (setq *human* (human-1200)))
        (130
         (setq *human* (human-1300)))
        (140
         (setq *human* (human-1400)))
    (150
         (setq *human* (human-1500)))
        (160
         (setq *human* (human-1600)))
        (170
         (setq *human* (human-1700)))
        (180
         (setq *human* (human-1800)))
        (t
         (setq *human* (human-1100)))
        )
    )

(defun calc-distance-between-human-and-robot (size)
  (let ((distance 400))
    (setq distance (+ 400 (* (* (- size 110) 0.1) 40)))
    (send *human* :translate (float-vector 0 distance -800))
    )
  )

(defun set-arm (size)
  (let ((angle 5))
    (setq angle (+ 5 (* (* (- size 110) 0.1))))
    (send *pepper* :larm :shoulder-r :joint-angle angle)
    (send *human* :rarm :shoulder-r :joint-angle angle)
    )
  )

なんかすごくかっこ悪いけど以下のようにしたら全身Ikできます

use-torso　は各ロボットで個別に定義されています．もうそろそろirtrobotに持ってきてもいいかな、という議論がどっかであったと思います．
fullbody-inverse-kinematics で:look-at-target t するとエラーになる．要確認．
ikとしては溶けているけど、姿勢は思ったものではない気がする．何が違うのかな？肩の関節角度かな．骨格模倣として面白いはず @yuki-asano

#!/usr/bin/env roseus
(require :pepper-interface "package://peppereus/pepper-interface.l")
;; ファイルパスを直す必要がある
(load "models/human-1100-robot.l") 
(load "models/human-1200-robot.l") 
(load "models/human-1300-robot.l")  
(load "models/human-1400-robot.l") 
(load "models/human-1500-robot.l") 
(load "models/human-1600-robot.l") 
(load "models/human-1700-robot.l") 
(load "models/human-1800-robot.l") 

(defun main (&optional (size 110))
  (setq *pepper* (pepper))
  (make-human size)
  (calc-distance-between-human-and-robot size)
  (set-arm size)
  (objects (list *pepper* *human*))
#|
  (send *human* :rarm :inverse-kinematics
        (send (send *pepper* :rleg :end-coords) :copy-worldcoords) ;; pepperの足元を指定
        :rotation-axis nil
        :use-fullbody t
        :debug-view :no-message)
|#
#|
  (send *human* :rarm :inverse-kinematics
        (send (send *pepper* :rleg :end-coords) :copy-worldcoords) ;; pepperの足元を指定
        :rotation-axis nil
        :link-list (send *human* :link-list (send *human* :rarm :end-coords :parent)) ;; use torso
        :debug-view t)
|#
  (send *human* :fullbody-inverse-kinematics
        (list (send (send *pepper* :rleg :end-coords) :copy-worldcoords) ;; pepperの足元を指定
              (send *human* :rleg :end-coords) ;; 足は動かさないように
              (send *human* :lleg :end-coords))
        :move-target (list (send *human* :rarm :end-coords)
                           (send *human* :rleg :end-coords)
                           (send *human* :lleg :end-coords))
        :rotation-axis (list nil  t t)
        ;; :look-at-target t
        :link-list
        (list (send *human* :link-list (send *human* :rarm :end-coords :parent)) ;; use torso
              (send *human* :link-list (send *human* :rleg :end-coords :parent))
              (send *human* :link-list (send *human* :lleg :end-coords :parent)))
        :debug-view t)

  (send *irtviewer* :draw-objects)
  (x::window-main-one) 
 )

(defun make-human (size)
    (case size
        (110
         (setq *human* (human-1100)))
        (120
         (setq *human* (human-1200)))
        (130
         (setq *human* (human-1300)))
        (140
         (setq *human* (human-1400)))
        (150
         (setq *human* (human-1500)))
        (160
         (setq *human* (human-1600)))
        (170
         (setq *human* (human-1700)))
        (180
         (setq *human* (human-1800)))
        (t
         (setq *human* (human-1100)))
        )
    )

(defun calc-distance-between-human-and-robot (size)
  (let ((distance 400))
    (setq distance (+ 400 (* (* (- size 110) 0.1) 40)))
    (send *human* :translate (float-vector 0 distance -800))
    )
  )

(defun set-arm (size)
  (let ((angle 5))
    (setq angle (+ 5 (* (* (- size 110) 0.1))))
    (send *pepper* :larm :shoulder-r :joint-angle angle)
    (send *human* :rarm :shoulder-r :joint-angle angle)
    )
  )

screenshot from 2016-12-26 14 41 34

:look-at-target の件はhttps://github.com/euslisp/jskeus/pull/408 かな．

euslisp / jskeus

assocの使い方について（ロボット同士の手をつながせたい） #405