[2024年度ゼミ] Tsubaki's work #1403
Conversation
jsk_2024_10_semi/surgery_motion_1.l
Outdated
|
|
||
| ;;Setting cube as a desk for surgery. | ||
| (setq *desk* (make-cube 500 500 600)) | ||
| (send *desk* :translate (float-vector 700 0 350)) ;; 質問：世界座標の原点がわからないので，机を床に設置(接地)できない． |
There was a problem hiding this comment.
質問とはちょっと違うけど
(send *pr2* :copy-worldcoords)
(send *desk* :copy-worldcoords)とかすると，その物体の現在の世界座標での位置がわかります．この結果を見ると両方とも，オブジェクトを作った時点では ( :translate していない時点では)
12.E3-irteusgl$ send *pr2* :copy-worldcoords
#<coordinates #X557a95e11a90 0.0 0.0 0.0 / 0.0 0.0 0.0>
13.E3-irteusgl$ send *desk* :copy-worldcoords
#<coordinates #X557a9526acb0 0.0 0.0 0.0 / 0.0 0.0 0.0>
となっていて，最初は原点に存在していることがわかります．
本題だけど，机を床に接地できないのは，(make-cube x y z) が 原点から +x方向にx/2 -x方向に x/2, +y方向にy/2 -y方向にy/2 ... という直方体を作るようになっているので，今回だと z方向に300だけ動かせばよいです． (という疑問であっている？)
There was a problem hiding this comment.
少し追加で，可視化的な話だと，この画像のようにIRT viewerだと原点の座標は実は小さく出ていて，RGBの順に赤：X軸，緑：Y軸，青：Z軸です．
原点に限らず座標を可視化したいときは
(load "models/arrow-object.l")
(setq *arrow* (arrow))
;; ここで移動させる
(send *arrow* :copy-worldcoords) ;;座標の値の確認
(objects (list *pr2* *desk* *needle* *hampen* *arrow*)) ;;中身はその時の可視化したい物体次第とかでわかりやすく見られたりします．
座標の描画自体には
(send (send (send *needle* :get :left-coords) :copy-worldcoords) :draw-on :flush t)
も使えます．
jsk_2024_10_semi/surgery_motion_1.l
Outdated
|
|
||
| ;;Setting hampen | ||
| (setq *hampen* (make-cube 60 60 20)) | ||
| (send *hampen* :translate (float-vector 700 0 660)) ; 質問：座標じゃなくてdeskの上面に設置させたい |
There was a problem hiding this comment.
超絶ナイーブな方法は send *hampen* :translate (v+ (float-vector 0 0 310) (send *desk* :pos)) とか？
jsk_2024_10_semi/surgery_motion_1.l
Outdated
| (send *needle* :put :left-coords | ||
| (make-cascoords | ||
| :coords (send (send *needle* :copy-worldcoords) :translate (float-vector 0 0 -5)) | ||
| :parent *needle*)) ;;質問：parent座標をpr2に設定したら収束しなくなった |
There was a problem hiding this comment.
;;質問：parent座標をpr2に設定したら収束しなくなった
はIKの収束の話ですかね？予測ですが，pr2のlarmとかにassocするとlarmを動かすとassocされている対象座標も動いて収束しないのかなという気もします．あとはシンプルにIKが解けない座標になっているとか？
あと関連しそうなのは，pr2の腕にassocした対象をend-effectorとしてIKを解く時には:move-targetを使います．
jsk_2024_10_semi/surgery_motion_1.l
Outdated
|
|
||
|
|
||
|
|
||
| ;;質問：ここでikが上手く行かなくなった。左では上手く行っているのに、右では下からアプローチする。assocの切り替えもわからない。あと，オフセット |
There was a problem hiding this comment.
左で上からアプローチしていたのは，reachするときの:rotation-axis :zでたまたまいい感じの姿勢になっていたからで
姿勢の方向を一致させたい時には:rotation-axis tが必要です．
この段階でrarmとlarmの両方を:rotation-axis tで (send (send *needle* :get :left-coords) :copy-worldcoords)にIKを解かせると
こんな感じです．
自分のおすすめはIKの引数に一発できれいに書くのではなくて，:translate :rotate あるいは:transformを一つずつしてarrowやdraw-onで描画しながら座標を確認して，作った座標にIKを解く方法ですが，例えば
(send *pr2* :rarm :inverse-kinematics
(send (send (send *needle* :get :left-coords) :copy-worldcoords) :rotate (deg2rad -90) :z)
:rotation-axis t)
とか:rotation-axis :yだと上からアプローチできそうです．
assocの切り替えには
(send (send *needle* :parent) :dissoc *needle*)
などでdissocしてから別の座標にassocする必要があります．
No description provided.