Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[2024年度ゼミ] Tsubaki's work #1403

Draft
wants to merge 43 commits into
base: jsk_2024_10_semi
Choose a base branch
from

Conversation

mqcmd196
Copy link
Member

No description provided.

@mqcmd196 mqcmd196 mentioned this pull request Oct 30, 2024
@mqcmd196 mqcmd196 changed the base branch from master to jsk_2024_10_semi October 30, 2024 03:24

;;Setting cube as a desk for surgery.
(setq *desk* (make-cube 500 500 600))
(send *desk* :translate (float-vector 700 0 350)) ;; 質問:世界座標の原点がわからないので,机を床に設置(接地)できない.
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

質問とはちょっと違うけど

(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だけ動かせばよいです. (という疑問であっている?)

Copy link
Contributor

@Kanazawanaoaki Kanazawanaoaki Oct 30, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Screenshot from 2024-10-30 13-57-43
少し追加で,可視化的な話だと,この画像のように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)

も使えます.


;;Setting hampen
(setq *hampen* (make-cube 60 60 20))
(send *hampen* :translate (float-vector 700 0 660)) ; 質問:座標じゃなくてdeskの上面に設置させたい
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

超絶ナイーブな方法は send *hampen* :translate (v+ (float-vector 0 0 310) (send *desk* :pos)) とか?

(send *needle* :put :left-coords
(make-cascoords
:coords (send (send *needle* :copy-worldcoords) :translate (float-vector 0 0 -5))
:parent *needle*)) ;;質問:parent座標をpr2に設定したら収束しなくなった
Copy link
Contributor

@Kanazawanaoaki Kanazawanaoaki Oct 30, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

;;質問:parent座標をpr2に設定したら収束しなくなった

はIKの収束の話ですかね?予測ですが,pr2のlarmとかにassocするとlarmを動かすとassocされている対象座標も動いて収束しないのかなという気もします.あとはシンプルにIKが解けない座標になっているとか?
あと関連しそうなのは,pr2の腕にassocした対象をend-effectorとしてIKを解く時には:move-targetを使います.




;;質問:ここでikが上手く行かなくなった。左では上手く行っているのに、右では下からアプローチする。assocの切り替えもわからない。あと,オフセット
Copy link
Contributor

@Kanazawanaoaki Kanazawanaoaki Oct 30, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

左で上からアプローチしていたのは,reachするときの:rotation-axis :zでたまたまいい感じの姿勢になっていたからで
姿勢の方向を一致させたい時には:rotation-axis tが必要です.

この段階でrarmとlarmの両方を:rotation-axis t(send (send *needle* :get :left-coords) :copy-worldcoords)にIKを解かせると
Screenshot from 2024-10-30 14-27-56
こんな感じです.

自分のおすすめは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する必要があります.

@Michi-Tsubaki
Copy link

PR2が指定したTrajectoryにしたがって針を動かし,左腕と右腕で針の持ち替えを繰り返し行っている動画(11/21)

IMG_1965.mp4

@Michi-Tsubaki
Copy link

ちょっと縫えてきた?感じの動画

IMG_2007.mp4

@Michi-Tsubaki
Copy link

Michi-Tsubaki commented Nov 27, 2024

2針縫えて3針目で引きすぎてしまった動画

shareVideo.1.mp4

→糸が引けていることの確認が必要

今日やったこと・手伝っていただいたこと

  • 実験台をDIYした.
  • 台の高さやオフセットなどのパラメタを調整した.
  • kinectから傷口のy座標を取得. (hsi filter, clusteringとかのnodeを起動.launch fileを書いていただいた.)
  • 糸を引き抜く量を調整した.
  • 受け渡しポイントを調整することで糸の挙動を調整した.
  • 糸を再び受け取った左腕が一回左側に引くことで糸を調整するようにした★
  • メスと言ったら手術を開始するようにした.(speach_to_textのtopic)

来週までにやること

  • angle-vectorをangle-vector-sequence にして,なめらかな動きを実現する .
  • use-torsoをはじめに使うことで,途中は遅くならないようにする
  • angle-vectorのsimulatorとの差分によって,糸が引けていることを判定するようにする.

その先

  • 曲率をもって縫う軌跡を生成したい→平らなものにアプローチ

@Michi-Tsubaki
Copy link

Michi-Tsubaki commented Dec 2, 2024

12/25に発表で使えるかも知れないので,UMLを更新しました.

uml

ソースコード
https://github.com/jsk-ros-pkg/jsk_demos/blob/3ae7c36a14e6ecca5fb961bbacc1c9bcab5003f2/jsk_2024_10_semi/presentation/uml.act

@Michi-Tsubaki
Copy link

Michi-Tsubaki commented Dec 2, 2024

メモ:今は5針縫うプログラムにしているが,UCバークレーが今年2月のペーパーで6針縫うことに成功しているっぽいから,対抗して7針連続で縫えるようにしようかな
https://www.technologyreview.jp/s/330168/watch-this-robot-as-it-learns-to-stitch-up-wounds/

EECS-2024-127.pdf

@Michi-Tsubaki
Copy link

memo roslaunch virtual_force_publisher right_virtual_force_publisher.launch

@Michi-Tsubaki
Copy link

Michi-Tsubaki commented Dec 14, 2024

前回のゼミについて

  • 最大縫合回数が2針から3針に更新された.(さらに前回?)
IMG_2093.mp4

今日のコミットについて

af94995 で,

  • 糸の引張判定のループからすぐに抜けてしまうバグを修正.
  • pr1040に接続していなくてもコードが動くように,simlation modeかどうかで,voice detection+recognition woundの認識系のon offが自動で切り替わるようにした.
  • コードを見やすくしたり,引張判定のコードをサブルーチン化した.

1bdd78aで,

  • launch fileのindentがめちゃくちゃだったのを修正.
  • euclidean clustering のcluster_filterによって余計なものを認識しないように変更した

次回やること

@Michi-Tsubaki
Copy link

Michi-Tsubaki commented Dec 18, 2024

@Michi-Tsubaki
Copy link

Michi-Tsubaki commented Dec 18, 2024

↑ start-jsk/jsk_apc#143

@Michi-Tsubaki
Copy link

成功動画撮れました.右手の(仮想)力覚FBもしっかりと機能しています.

IMG_2624.mp4

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants