[ロボットシステム] ドアノブに対するinverse kinematicsが上手く行かない（日常生活空間のロボットモデルの課題）

[Michi-Tsubaki]

日常生活空間のロボットモデルの課題にところで、 授業資料にsample codeとして、kettleに対するikの例があったので、それを少し広げていって、

1. 部屋に入ってキッチンに移動する．
2. 左手でやかんを持つ．
3. 右手でマグカップを持つ．
4. やかんを少し傾けてマグカップに注ぐ．
5. やかんを離す
6. 右手にマグカップを持ったままテーブルに行く
7. マグカップをテーブルに置く
8. 部屋を去るために右手でドアノブを持つ

を雑に実装しています．

https://github.com/user-attachments/assets/059f4c36-daf0-4caa-9c3f-8f45c20f284c

最後の「ドアノブを持つ」のところのikの実行結果が error: all target-coords should be Coordinates or functions, but get (nil) になってしまって正しく実装できませんでした．

https://github.com/euslisp/EusLisp/blob/master/models/room73b2-door-left-object.l の :handleに対してikをしたいのですが， 以下のコードのどこに問題がありそうでしょうか？

該当箇所

**(send *robot* :larm :inverse-kinematics (send *door* :handle)
      :link-list (send *robot* :link-list (send *robot* :larm :end-coords :parent))
      :move-target (send *robot* :larm :end-coords)
      :rotation-axis :z
      :debug-view t)

(send *robot* :larm :end-coords :assoc *door*)
(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)**

フルスクリプト

(load "models/room73b2-scene.l")
(load "models/h7-robot.l")

;;Set Room and Robot
(setq *room* (room73b2))
(setq *robot* (h7))
(objects (list *robot* *room*))

;;Set Robot Pose
(send *robot* :reset-pose)
(send *irtviewer* :frush t)
(unix:sleep 1)

(send *robot* :move-to (send *room* :spot "cook-spot") :world)
(send *robot* :locate #f(0 0 550))
(send *irtviewer* :frush t)
(unix:sleep 1)

(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

(setq *kettle* (send *room* :object "room73b2-kettle"))
(setq *mug-cup* (send *room* :object "room73b2-mug-cup"))
(setq *kitchen* (send *room* :object "room73b2-kitchen"))
**(setq *door* (send *room* :object "room73b2-door-left"))**

(if (send *kettle* :parent) (send (send *kettle* :parent) :dissoc *kettle*))
(if (send *mug-cup* :parent) (send (send *mug-cup* :parent) :dissoc *mug-cup*))
(if (send *kitchen* :parent) (send (send *kitchen* :parent) :dissoc *kitchen*))
**(if (send *door* :parent) (send (send *door* :parent) :dissoc *door*))**

(send *kettle* :rotate (deg2rad 180) :z)
;;(setq kettle-coord (send (send *kettle* :get :left-coords) :copy-worldcoords))

(send *robot* :head :look-at (send *kettle* :worldpos))
(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

(send *robot* :larm :inverse-kinematics (car (send *kettle* :handle))
      :link-list (send *robot* :link-list (send *robot* :larm :end-coords :parent))
      :move-target (send *robot* :larm :end-coords)
      :rotation-axis :z
      :debug-view t)

(send *robot* :larm :end-coords :assoc *kettle*)
(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

(send *robot* :larm :move-end-pos #f(-50 0 50) :world
      :debug-view t :look-at-target t)
(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

(send *robot* :head :look-at (send *mug-cup* :worldpos))
(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

(send *robot* :rarm :inverse-kinematics (car (send *mug-cup* :handle))
      :link-list (send *robot* :link-list (send *robot* :rarm :end-coords :parent))
      :move-target (send *robot* :rarm :end-coords)
      :rotation-axis :z
      :debug-view t)
(send *robot* :rarm :end-coords :assoc *mug-cup*)

(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

(send *robot* :rarm :move-end-pos #f(0 0 50) :world
      :debug-view t :look-at-target t)
(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

(send *robot* :rarm :inverse-kinematics (car (send *mug-cup* :attention))
      :link-list (send *robot* :link-list (send *robot* :larm :end-coords :parent))
      :move-target (send *kettle* :attention)
      :rotation-axis :z
      :debug-view t)

(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

(send *robot* :rarm :inverse-kinematics (car (send *mug-cup* :attention))
      :link-list (send *robot* :link-list (send *robot* :larm :end-coords :parent))
      :move-target (send *kettle* :attention)
      :rotation-axis :z
      :debug-view t)

(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

(send *robot* :larm :inverse-kinematics (car (send *kitchen* :attention))
      :link-list (send *robot* :link-list (send *robot* :larm :end-coords :parent))
      :move-target (send *kettle* :attention)
      :rotation-axis :z
      :debug-view t)

(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

(send *robot* :larm :end-coords :dissoc *kettle*)
;;(send *kettle* :translate (float-vector 500.0 1500.0 825.0))
(unix:sleep 1)

(send *robot* :move-to (send *room* :spot "table-spot") :world)
(send *robot* :locate #f(0 0 550))
(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

(send *robot* :move-to (send *room* :spot "table-spot") :world)
(send *robot* :locate #f(0 0 550))
(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

(send *robot* :rarm :end-coords :dissoc *mug-cup*)
(unix:sleep 1)

(send *robot* :move-to (send *room* :spot "door-spot") :world)
(send *robot* :locate #f(0 0 550))
(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

(send *robot* :head :look-at (send *door* :worldpos))
(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)

;;ここから下が動いていなさそう
**(send *robot* :larm :inverse-kinematics (send *door* :handle)
      :link-list (send *robot* :link-list (send *robot* :larm :end-coords :parent))
      :move-target (send *robot* :larm :end-coords)
      :rotation-axis :z
      :debug-view t)

(send *robot* :larm :end-coords :assoc *door*)
(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))
(unix:sleep 1)**

[k-okada]

以下のようにしてエラーが出た後のプロンプトでデバッグすることが出来ます。

1.irteusgl$ (load "euslisp-issue-515.l")
（中略）
;; error: all target-coords should be Coordinates or functions, but get (nil)
2.irteusgl$ (send *door* :handle)
nil

とすると、nil　になっているので、目標となる座標系が無いことがわかります。 そもそも、*door* が何なのか？は

54.irteusgl$ (send-all (send *door* :bodies):draw-on :flush t :color #f(1 0 0))

として表示することができます。こうすると、部屋の中からみて右側のドアになっていて、こちらには取っ手はなさそうです。

59.irteusgl$ (setq *door* (send *room* :object "room73b2-door-right"))
#<room73b2-door-right-object #X55fc6e908588 room73b2-door-right  0.0 -405.0 0.0 / 0.0 0.0 0.0>
60.irteusgl$ (if (send *door* :parent) (send (send *door* :parent) :dissoc *door*))
#<room73b2-door-right-object #X55fc6e908588 room73b2-door-right  0.0 -405.0 0.0 / 0.0 0.0 0.0>
61.irteusgl$ (send-all (send *door* :bodies) :draw-on :flush t :color #f(1 0 0))

とすると、右側の左のドアが *door* にセットされていそうです。 これで、:handle を確認しましょう。

62.irteusgl$ (send *door* :handle)
(#<cascaded-coords #X55fc6f6d2ba0 rear-knob  75.0 -140.0 960.0 / -3.142 1.225e-16 1.571> #<cascaded-coords #X55fc\
6f8dcfc8 front-knob  -115.0 -140.0 960.0 / -3.142 1.225e-16 1.571> #<cascaded-coords #X55fc6f8dcb60 lock  65.0 -6\
0.0 1060.0 / 3.142 0.0 0.0>)

となにかの座標系が２つ入っていそうです。座標系をviewerで確認するためには

64.irteusgl$ (send (elt (send *door* :handle) 0) :draw-on :flush t :color #f(1 0 0) :size 100)
1

として、最初の値が部屋の中、次の値が廊下側になっていそうです。 ということで、

65.irteusgl$ (send *robot* :larm :inverse-kinematics (elt (send *door* :handle) 0)
      :link-list (send *robot* :link-list (send *robot* :larm :end-coords :parent))
      :move-target (send *robot* :larm :end-coords)
      :rotation-axis :z
      :debug-view t)

とすると、IKが解けると思います。

インタプリタ型の言語では、プロンプトで一個ずつコマンドを打って確認するとよいです。

(send *door* :methods) とすると、メンバ関数一覧が、(describe *door*) とすると、メンバ変数一覧を見ることが出来ます。名前しか出ませんが、何をやっているか予測はつくと思います。Pythonですと (dir a) などとして確認することができます。