From 9722eefbe471960da8b3a4133891ab727feee1d9 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 23 Oct 2024 16:07:05 +0900 Subject: [PATCH 1/5] add demo 1023 --- jsk_2024_10_semi/demo.l | 17 +++++++++++++++++ jsk_2024_10_semi/demo1023.l | 0 2 files changed, 17 insertions(+) create mode 100644 jsk_2024_10_semi/demo.l create mode 100644 jsk_2024_10_semi/demo1023.l diff --git a/jsk_2024_10_semi/demo.l b/jsk_2024_10_semi/demo.l new file mode 100644 index 0000000000..108750e637 --- /dev/null +++ b/jsk_2024_10_semi/demo.l @@ -0,0 +1,17 @@ +(require "package://pr2eus/pr2.l") +(if (not (boundp '*pr2*)) (setq *pr2* (pr2))) + +(objects (list *pr2*)) +(send *pr2* :larm :shoulder-p :joint-angle 10) + +(send *pr2* :rarm :shoulder-p :joint-angle -20) +(send *pr2* :larm :shoulder-p :joint-angle 50) +(send *pr2* :rarm :shoulder-r :joint-angle -130) +(send *pr2* :larm :shoulder-r :joint-angle 120) +(send *pr2* :larm :elbow-p :joint-angle -100) +(send *pr2* :rarm :elbow-p :joint-angle -70) +(send *pr2* :rarm :elbow-r :joint-angle 0) +(send *pr2* :rarm :wrist-p :joint-angle -40) +(send *pr2* :larm :wrist-r :joint-angle 30) + +(send *irtviewer* :draw-objects) diff --git a/jsk_2024_10_semi/demo1023.l b/jsk_2024_10_semi/demo1023.l new file mode 100644 index 0000000000..e69de29bb2 From a98044921e706fc833f6a9a2afd7ae2dd06cd314 Mon Sep 17 00:00:00 2001 From: ushidakyotaro <148761542+ushidakyotaro@users.noreply.github.com> Date: Wed, 6 Nov 2024 15:27:36 +0900 Subject: [PATCH 2/5] Add files via upload --- jsk_2024_10_semi/hanger-ik.l | 151 +++++++++++++++++++++++++++++++++++ 1 file changed, 151 insertions(+) create mode 100644 jsk_2024_10_semi/hanger-ik.l diff --git a/jsk_2024_10_semi/hanger-ik.l b/jsk_2024_10_semi/hanger-ik.l new file mode 100644 index 0000000000..27aded287c --- /dev/null +++ b/jsk_2024_10_semi/hanger-ik.l @@ -0,0 +1,151 @@ +#!/usr/bin/env roseus + +;; PR2のモデルを読み込む +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") + +(pr2-init) +;; PR2のインスタンスを作成 +(if (not (boundp '*pr2*)) (setq *pr2* (pr2))) + + +;; 三角柱を作成（薄いハンガー形状を模擬） +(setq *hanger* (make-prism + (list (float-vector 0 00 200) ;; 下の頂点 + (float-vector 200 0 0) ;; 右上の頂点 + (float-vector -200 0 0)) ;; 左上の頂点 + 20)) ;; 厚さ20mm + +;; 三角柱を適切な位置に移動 +(send *hanger* :translate (float-vector 1100 100 1100)) + +;; 三角柱の下の角に座標系を設定（把持の手前の位置） +(send *hanger* :put :left-coords + (make-cascoords + :coords (send (send *hanger* :copy-worldcoords) + :translate (float-vector -220 0 0)) ;; ハンガーの下端から相対位置 + :rot #2f((0 0 1) ;; y軸方向を向くための回転行列 + (-1 0 0) + (0 -1 0)) + + :parent *hanger*)) + +; (send *hanger* :put :right-coords +; (make-cascoords +; :coords (send (send *hanger* :copy-worldcoords) +; :translate (float-vector -20 0 0)) ;; ハンガーの下端から相対位置 +; :rot #2f((1 0 0) (0 1 0) (0 0 1)) ;; 地面と平行な姿勢 +; :parent *hanger*)) + + +;; ビューワを表示 +(objects (list *pr2* *hanger*)) + +(send *pr2* :reset-pose) +(send *pr2* :larm :collar-y :joint-angle 0) +(send *pr2* :larm :shoulder-p :joint-angle 0) +(send *pr2* :larm :shoulder-r :joint-angle 0) +(send *pr2* :larm :elbow-p :joint-angle -90) +(send *pr2* :larm :elbow-r :joint-angle 180) +(send *pr2* :larm :wrist-p :joint-angle -30) +(send *pr2* :larm :wrist-r :joint-angle 180) +(send *pr2* :larm :gripper :joint-angle 40) +(send *irtviewer* :draw-objects) + +; これ書けばロボットが動く +(send *ri* :stop-grasp :arms) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) + +; ;; 左腕で三角柱を掴む +(send *pr2* :larm :inverse-kinematics + (send (send *hanger* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) + +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) + +; ;; PR2の左腕で三角柱を持ち上げて前に突き出す +; (send *pr2* :larm :end-coords :assoc *hanger*) +; (send *pr2* +; :inverse-kinematics (send (send (send *hanger* :get :left-coords) :copy-worldcoords) +; :translate (float-vector 100.0 0.0 100.0)) +; :move-target (send *hanger* :get :left-coords) +; :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) +; :rotation-axis t +; :debug-view t) + + + +(defun step2-grasp () + "ハンドを閉じる動作（シミュレーション用）" + ;(send *pr2* :larm :gripper :joint-angle 0) + (send *pr2* :larm :end-coords :assoc *hanger*) + (send *ri* :start-grasp :larm) + (send *irtviewer* :draw-objects)) + + +; (defun step3-lift () +; "斜め上に移動" +; (send *pr2* :larm :move-end-pos #f(-50 0 -50) :world +; :debug-view nil) +; (send *irtviewer* :draw-objects)) + +; (defun step4-pull () +; "手元に引く" +; (send *pr2* :larm :move-end-pos #f(-300 0 0) :world +; :debug-view nil) +; (send *irtviewer* :draw-objects)) + + + +(defun step3-lift () + "斜め上に移動" + (send *pr2* + :larm + :inverse-kinematics (send (send (send *hanger* :get :left-coords) :copy-worldcoords) + :translate (float-vector -100 0 100)) ;; 5cm上、5cm手前 + ;;:move-target (send *hanger* :get :left-coords) + ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view nil + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *irtviewer* :draw-objects) + ) + +(defun step4-pull () + "手元に引く" + (send *pr2* + :inverse-kinematics (send (send (send *hanger* :get :left-coords) :copy-worldcoords) + :translate (float-vector -300 0 50)) ;; さらに30cm手前 + ;:move-target (send *hanger* :get :left-coords) + ;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *irtviewer* :draw-objects)) + +(defun step5-release () + "ハンドを開く動作（シミュレーション用）" + ;(send *pr2* :larm :gripper :joint-angle 40) + (send *ri* :stop-grasp :larm) + (send *irtviewer* :draw-objects) +) + + + + +; (step2-grasp) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) + +; (step3-lift) + +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) + +; (step4-pull) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) From 218a9a886dab1b9bd581a20a5d237952fb2f677e Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 13 Nov 2024 15:51:47 +0900 Subject: [PATCH 3/5] ADD files 11/13 --- jsk_2024_10_semi/hanger-box-ik.l | 226 +++++++++++++++++++++++++++++++ jsk_2024_10_semi/hanger-box.l | 46 +++++++ jsk_2024_10_semi/hanger-ik.l | 155 +++++++++++++++++++++ 3 files changed, 427 insertions(+) create mode 100644 jsk_2024_10_semi/hanger-box-ik.l create mode 100644 jsk_2024_10_semi/hanger-box.l create mode 100644 jsk_2024_10_semi/hanger-ik.l diff --git a/jsk_2024_10_semi/hanger-box-ik.l b/jsk_2024_10_semi/hanger-box-ik.l new file mode 100644 index 0000000000..118bcc93c7 --- /dev/null +++ b/jsk_2024_10_semi/hanger-box-ik.l @@ -0,0 +1,226 @@ +#!/usr/bin/env roseus + +;; PR2のモデルを読み込む +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(require "package://pr2eus/speak.l") +(ros::load-ros-manifest "jsk_recognition_msgs") + +(pr2-init) +;; PR2のインスタンスを作成 +(if (not (boundp '*pr2*)) (setq *pr2* (pr2))) + + +;; 三角柱を作成（薄いハンガー形状を模擬） +(defun make-hanger () + (let ((hanger (make-prism + (list (float-vector 0 0 0) ;; 下の頂点 + (float-vector 150 0 -100) ;; 右上の頂点 + (float-vector -150 0 -100)) ;; 左上の頂点 + 10))) ;; 厚さ10mm + + ;; 三角柱の下の角に座標系を設定（把持の手前の位置） + (send hanger :put :left-coords + (make-cascoords + :coords (send (send hanger :copy-worldcoords) + :translate (float-vector 80 0 -50) ;; ハンガーの + ) ;; ハンガーの下端から相対位置 + :rot #2f((0 0 1) ;; y軸方向を向くための回転行列 + (-1 0 0) + (0 -1 0)) + :parent hanger)) + hanger)) + +;; ハンガーをグローバル変数として保持 +(setq *hanger* (make-hanger)) +;; 初期位置に配置 +;(send *hanger* :translate (float-vector 800 100 1100)) + +(defun step1-init() + (send *ri* :speak-jp "今日は何を着たい?") + + (send *pr2* :reset-pose) + (send *pr2* :larm :collar-y :joint-angle 0) + (send *pr2* :larm :shoulder-p :joint-angle 0) + (send *pr2* :larm :shoulder-r :joint-angle 0) + (send *pr2* :larm :elbow-p :joint-angle -90) + (send *pr2* :larm :elbow-r :joint-angle 180) + (send *pr2* :larm :wrist-p :joint-angle -30) + (send *pr2* :larm :wrist-r :joint-angle 180) + (send *pr2* :larm :gripper :joint-angle 40) + (send *irtviewer* :draw-objects) + + + ; これ書けばロボットが動く + (send *ri* :stop-grasp :arms) + (send *ri* :angle-vector (send *pr2* :angle-vector) 5000) + (send *ri* :wait-interpolation) + (send *ri* :speak-jp "準備完了しました") +) + +(defun step2-approach () + (send *ri* :speak-jp "ハンガーに腕を伸ばすよ。") + + ; ;; ハンガーの位置を*target-coords*に合わせて更新 + ; (when (boundp '*target-coords*) + ; (send *hanger* :move-to *target-coords* :world) + ; (send *irtviewer* :draw-objects)) + + ;; 左腕で三角柱を掴む + (send *pr2* :larm :inverse-kinematics + (send (send *hanger* :get :left-coords) :copy-worldcoords) + :rotation-axis :y) + + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) + (unix:sleep 3) + (send *ri* :speak-jp "ハンガーに手を伸ばしました。") + + + ; ;; 左腕で三角柱を掴む - 逆運動学の結果を変数に保存 + ; (let ((ik-ret (send *pr2* :larm :inverse-kinematics + ; (send (send *hanger* :get :left-coords) :copy-worldcoords) + ; :rotation-axis t + ; :debug-view t))) ;; デバッグビューを有効に + + ; (if ik-ret + ; (progn + ; (format t "IK succeeded: ~A~%" ik-ret) + ; (send *ri* :angle-vector (send *pr2* :angle-vector) 5000) ;; 5秒に延長 + ; (send *ri* :wait-interpolation) + ; (unix:sleep 3)) + ; (send *ri* :speak-jp "ハンガーに手が届きません。"))) ;; IKが解けない場合 + +) + +(defun step3-grasp () + (send *ri* :speak-jp "ハンガーを掴むよ。") + "ハンドを閉じる動作（シミュレーション用）" + ;(send *pr2* :larm :gripper :joint-angle 0) + (send *pr2* :larm :end-coords :assoc *hanger*) + (send *ri* :start-grasp :larm) + (send *irtviewer* :draw-objects) + (unix:sleep 1)) + + +(defun step4-lift () + (send *ri* :speak-jp "持ち上げるよ。") + "斜め上に移動" + (send *pr2* + :larm + :inverse-kinematics (send (send (send *hanger* :get :left-coords) :copy-worldcoords) + :translate (float-vector -100 0 150)) ;; 15cm上、10cm手前 + ;;:move-target (send *hanger* :get :left-coords) + ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view nil + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *irtviewer* :draw-objects) + ) + +(defun step5-pull () + (send *ri* :speak-jp "手元に引くよ。") + "手元に引く" + (send *pr2* + :inverse-kinematics (send (send (send *hanger* :get :left-coords) :copy-worldcoords) + :translate (float-vector -100 0 50)) ;; さらに30cm手前 + ;:move-target (send *hanger* :get :left-coords) + ;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *irtviewer* :draw-objects)) + +(defun step6-release () + (send *ri* :speak-jp "離すよ。") + "ハンドを開く動作（シミュレーション用）" + ;(send *pr2* :larm :gripper :joint-angle 40) + + ; シミュレーションでハンガーの関連付けを解除 + (send *pr2* :larm :end-coords :dissoc *hanger*) + (send *ri* :stop-grasp :larm) + (send *irtviewer* :draw-objects) +) + +;ハンガーを把持しようとしているかどうかのフラグ +(setq *found-hanger* nil) +(setq *is-moving* nil) + +;; コールバック関数 +(defun box-cb (msg) + + (dolist (box (send msg :boxes)) + (when (member (send box :label) '(566 615 469 923)) + (ros::ros-info "hook is detected") + ;(send *ri* :speak-jp "ハンガー発見！") + (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) + (setq *target-dimensions* (send box :dimensions)) + + ;; 座標を表示 + (format t "Target coords: x=~A y=~A z=~A~%" + (elt (send *target-coords* :worldpos) 0) + (elt (send *target-coords* :worldpos) 1) + (elt (send *target-coords* :worldpos) 2)) + + (when (and (< (elt (send *target-coords* :worldpos) 2) 1800) + (> (elt (send *target-coords* :worldpos) 2) 0) + (> (elt (send *target-coords* :worldpos) 0) 500) + (< (elt (send *target-coords* :worldpos) 0) 1500) + (> (elt (send *target-coords* :worldpos) 1) -1500) + (< (elt (send *target-coords* :worldpos) 1) 1500)) + + (ros::ros-info "target is in reachable area") + (send *ri* :speak-jp "ハンガーが手の届く範囲にありそうます。") + + ;; ハンガーの位置を更新して確認 + (send *hanger* :move-to *target-coords* :world) + (format t "Updated hanger position: ~A~%" (send *hanger* :worldpos)) + (send *irtviewer* :draw-objects) + (unix:sleep 1) ;; 位置更新を確認するための待機 + (setq *found-hanger* t))))) ;; 処理が完了したらループを抜ける + +(defun grasp-hanger () + (setq *is-moving* t) + (unwind-protect + (progn + (step2-approach) + (step3-grasp) + (unix:sleep 2) + ;(step4-lift) + ;(step5-pull) + (step6-release)) + (setq *is-moving* nil))) + +(defun main () + (ros::ros-info "start main loop") + (objects (list *pr2* *hanger*)) + (step1-init) + (unix:sleep 1) + + (ros::ros-info "start searching hanger") + (send *ri* :speak-jp "ハンガーを探します。") + (ros::subscribe "/synchronized_detic_label_boxes" + jsk_recognition_msgs::BoundingBoxArray #'box-cb) + + (do-until-key + (when *found-hanger* + (return)) + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once)) + + (when *found-hanger* + (send *ri* :speak-jp "ハンガーを掴みに行きます。") + (grasp-hanger) + (send *ri* :speak-jp "終了しました。") + ) +) + +;; メイン関数を実行 +(main) + + + diff --git a/jsk_2024_10_semi/hanger-box.l b/jsk_2024_10_semi/hanger-box.l new file mode 100644 index 0000000000..2560f76b52 --- /dev/null +++ b/jsk_2024_10_semi/hanger-box.l @@ -0,0 +1,46 @@ +(require "package://pr2eus/pr2-interface.l") +(require "package://pr2eus/speak.l") +(pr2-init) + +(send *pr2* :reset-pose) + +(defclass box-label-synchronizer + :super exact-time-message-filter) + +(defmethod box-label-synchronizer + (:callback (box label) + (print (list box label)) + (print (send-all (list box label) :header :stamp)) + )) + +;; ;; test +;; (setq *box-label* (instance box-label-synchronizer :init +;; (list (list "/docker/detic_segmentor/output/boxes" jsk_recognition_msgs::BoundingBoxArray) +;; (list "/docker/detic_segmentor/detected_classes" jsk_recognition_msgs::LabelArray)))) + +(ros::load-ros-manifest "jsk_recognition_msgs") +(setq *target-box* (make-cube 100 100 100)) +(objects (list *pr2* *target-box*)) +(defun box-cb (msg) + (ros::ros-info "received ~A boxes" (length (send msg :boxes))) + (dolist (box (send msg :boxes)) + (when (member (send box :label) '(566 615 469 923)) ;; 566 -> hook ;615:knife 469:fork 923:scissors + (ros::ros-info "hook is detected" ) + (send *ri* :speak-jp "ハンガー発見！") + (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) + (setq *target-dimensions* (send box :dimensions)) + (format t "coords ~A, dimension ~A~%" (send *target-coords* :worldcoords) (* (send *target-dimensions* :x) (send *target-dimensions* :y) (send *target-dimensions* :z))) + (when (and (< (elt (send *target-coords* :worldpos) 2) 2000) + (> (elt (send *target-coords* :worldpos) 2) 0)) + (send *target-box* :move-to *target-coords* :world) + (print "update target position") + )))) + +(ros::subscribe "/synchronized_detic_label_boxes" jsk_recognition_msgs::BoundingBoxArray #'box-cb) +;;(ros::subscribe "/kinect_head/depth_registered/boxes" jsk_recognition_msgs::BoundingBoxArray #'box-cb) +(do-until-key + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once) + ) diff --git a/jsk_2024_10_semi/hanger-ik.l b/jsk_2024_10_semi/hanger-ik.l new file mode 100644 index 0000000000..e0f998d87f --- /dev/null +++ b/jsk_2024_10_semi/hanger-ik.l @@ -0,0 +1,155 @@ +#!/usr/bin/env roseus + +;; PR2のモデルを読み込む +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(require "package://pr2eus/speak.l") + +(pr2-init) +;; PR2のインスタンスを作成 +(if (not (boundp '*pr2*)) (setq *pr2* (pr2))) + + +;; 三角柱を作成（薄いハンガー形状を模擬） +(setq *hanger* (make-prism + (list (float-vector 0 00 200) ;; 下の頂点 + (float-vector 200 0 0) ;; 右上の頂点 + (float-vector -200 0 0)) ;; 左上の頂点 + 20)) ;; 厚さ20mm + +;; 三角柱を適切な位置に移動 +(send *hanger* :translate (float-vector 1100 100 1100)) + +;; 三角柱の下の角に座標系を設定（把持の手前の位置） +(send *hanger* :put :left-coords + (make-cascoords + :coords (send (send *hanger* :copy-worldcoords) + :translate (float-vector -220 0 0)) ;; ハンガーの下端から相対位置 + :rot #2f((0 0 1) ;; y軸方向を向くための回転行列 + (-1 0 0) + (0 -1 0)) + + :parent *hanger*)) + +; (send *hanger* :put :right-coords +; (make-cascoords +; :coords (send (send *hanger* :copy-worldcoords) +; :translate (float-vector -20 0 0)) ;; ハンガーの下端から相対位置 +; :rot #2f((1 0 0) (0 1 0) (0 0 1)) ;; 地面と平行な姿勢 +; :parent *hanger*)) + + +;; ビューワを表示 +(objects (list *pr2* *hanger*)) + +(send *ri* :speak-jp "今日は何を着たい?") + +(send *pr2* :reset-pose) +(send *pr2* :larm :collar-y :joint-angle 0) +(send *pr2* :larm :shoulder-p :joint-angle 0) +(send *pr2* :larm :shoulder-r :joint-angle 0) +(send *pr2* :larm :elbow-p :joint-angle -90) +(send *pr2* :larm :elbow-r :joint-angle 180) +(send *pr2* :larm :wrist-p :joint-angle -30) +(send *pr2* :larm :wrist-r :joint-angle 180) +(send *pr2* :larm :gripper :joint-angle 40) +(send *irtviewer* :draw-objects) + + +; これ書けばロボットが動く +(send *ri* :stop-grasp :arms) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) + +; ;; 左腕で三角柱を掴む +(send *pr2* :larm :inverse-kinematics + (send (send *hanger* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) + +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) + +; ;; PR2の左腕で三角柱を持ち上げて前に突き出す +; (send *pr2* :larm :end-coords :assoc *hanger*) +; (send *pr2* +; :inverse-kinematics (send (send (send *hanger* :get :left-coords) :copy-worldcoords) +; :translate (float-vector 100.0 0.0 100.0)) +; :move-target (send *hanger* :get :left-coords) +; :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) +; :rotation-axis t +; :debug-view t) + + + +(defun step2-grasp () + "ハンドを閉じる動作（シミュレーション用）" + ;(send *pr2* :larm :gripper :joint-angle 0) + (send *pr2* :larm :end-coords :assoc *hanger*) + (send *ri* :start-grasp :larm) + (send *irtviewer* :draw-objects)) + + +; (defun step3-lift () +; "斜め上に移動" +; (send *pr2* :larm :move-end-pos #f(-50 0 -50) :world +; :debug-view nil) +; (send *irtviewer* :draw-objects)) + +; (defun step4-pull () +; "手元に引く" +; (send *pr2* :larm :move-end-pos #f(-300 0 0) :world +; :debug-view nil) +; (send *irtviewer* :draw-objects)) + + + +(defun step3-lift () + "斜め上に移動" + (send *pr2* + :larm + :inverse-kinematics (send (send (send *hanger* :get :left-coords) :copy-worldcoords) + :translate (float-vector -100 0 100)) ;; 5cm上、5cm手前 + ;;:move-target (send *hanger* :get :left-coords) + ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view nil + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *irtviewer* :draw-objects) + ) + +(defun step4-pull () + "手元に引く" + (send *pr2* + :inverse-kinematics (send (send (send *hanger* :get :left-coords) :copy-worldcoords) + :translate (float-vector -300 0 50)) ;; さらに30cm手前 + ;:move-target (send *hanger* :get :left-coords) + ;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *irtviewer* :draw-objects)) + +(defun step5-release () + "ハンドを開く動作（シミュレーション用）" + ;(send *pr2* :larm :gripper :joint-angle 40) + (send *ri* :stop-grasp :larm) + (send *irtviewer* :draw-objects) +) + + + + +; (step2-grasp) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) + +; (step3-lift) + +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) + +; (step4-pull) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) From f9d1151480377e02f9108419e641a04dc5997063 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 20 Nov 2024 16:16:19 +0900 Subject: [PATCH 4/5] =?UTF-8?q?ADD=20ik=E3=81=A7=E5=88=A4=E5=88=A5?= =?UTF-8?q?=E3=81=99=E3=82=8B=E3=82=B3=E3=83=BC=E3=83=89=E3=82=92=E4=BD=9C?= =?UTF-8?q?=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- jsk_2024_10_semi/hanger-box-ik.l | 198 +++++++++++++++++++++---------- 1 file changed, 133 insertions(+), 65 deletions(-) diff --git a/jsk_2024_10_semi/hanger-box-ik.l b/jsk_2024_10_semi/hanger-box-ik.l index 118bcc93c7..6ec170224d 100644 --- a/jsk_2024_10_semi/hanger-box-ik.l +++ b/jsk_2024_10_semi/hanger-box-ik.l @@ -1,5 +1,4 @@ #!/usr/bin/env roseus - ;; PR2のモデルを読み込む (require "package://pr2eus/pr2.l") (require "package://pr2eus/pr2-utils.l") @@ -14,17 +13,17 @@ ;; 三角柱を作成（薄いハンガー形状を模擬） (defun make-hanger () - (let ((hanger (make-prism + (let ((hanger (make-prism (list (float-vector 0 0 0) ;; 下の頂点 (float-vector 150 0 -100) ;; 右上の頂点 (float-vector -150 0 -100)) ;; 左上の頂点 10))) ;; 厚さ10mm - + ;; 三角柱の下の角に座標系を設定（把持の手前の位置） (send hanger :put :left-coords (make-cascoords :coords (send (send hanger :copy-worldcoords) - :translate (float-vector 80 0 -50) ;; ハンガーの + :translate (float-vector -20 0 -20) ;; ハンガーの-80 0 -50 ) ;; ハンガーの下端から相対位置 :rot #2f((0 0 1) ;; y軸方向を向くための回転行列 (-1 0 0) @@ -39,22 +38,23 @@ (defun step1-init() (send *ri* :speak-jp "今日は何を着たい?") - (send *pr2* :reset-pose) + (send *ri* :wait-interpolation) (send *pr2* :larm :collar-y :joint-angle 0) - (send *pr2* :larm :shoulder-p :joint-angle 0) + (send *pr2* :larm :shoulder-p :joint-angle 30) (send *pr2* :larm :shoulder-r :joint-angle 0) - (send *pr2* :larm :elbow-p :joint-angle -90) + (send *pr2* :larm :elbow-p :joint-angle -120) (send *pr2* :larm :elbow-r :joint-angle 180) (send *pr2* :larm :wrist-p :joint-angle -30) (send *pr2* :larm :wrist-r :joint-angle 180) (send *pr2* :larm :gripper :joint-angle 40) + (send *pr2* :head :neck-p :joint-angle 20) (send *irtviewer* :draw-objects) ; これ書けばロボットが動く (send *ri* :stop-grasp :arms) - (send *ri* :angle-vector (send *pr2* :angle-vector) 5000) + (send *ri* :angle-vector (send *pr2* :angle-vector) 4000) (send *ri* :wait-interpolation) (send *ri* :speak-jp "準備完了しました") ) @@ -67,31 +67,60 @@ ; (send *hanger* :move-to *target-coords* :world) ; (send *irtviewer* :draw-objects)) - ;; 左腕で三角柱を掴む - (send *pr2* :larm :inverse-kinematics - (send (send *hanger* :get :left-coords) :copy-worldcoords) - :rotation-axis :y) - - (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) - (send *ri* :wait-interpolation) - (unix:sleep 3) - (send *ri* :speak-jp "ハンガーに手を伸ばしました。") + ;; 左腕で三角柱を掴む 既存のコード + ; (send *pr2* :larm :inverse-kinematics + ; (send (send *hanger* :get :left-coords) :copy-worldcoords) + ; :rotation-axis nil + ; :use-torso t + ; ) ;y + ; (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + ; (send *ri* :wait-interpolation) + ; (unix:sleep 3) + ; (send *ri* :speak-jp "ハンガーに手を伸ばしました。") + ; ;; 左腕で三角柱を掴む - 逆運動学の結果を変数に保存 - ; (let ((ik-ret (send *pr2* :larm :inverse-kinematics - ; (send (send *hanger* :get :left-coords) :copy-worldcoords) - ; :rotation-axis t - ; :debug-view t))) ;; デバッグビューを有効に - - ; (if ik-ret - ; (progn - ; (format t "IK succeeded: ~A~%" ik-ret) - ; (send *ri* :angle-vector (send *pr2* :angle-vector) 5000) ;; 5秒に延長 - ; (send *ri* :wait-interpolation) - ; (unix:sleep 3)) - ; (send *ri* :speak-jp "ハンガーに手が届きません。"))) ;; IKが解けない場合 - + (setq ik-ret (send *pr2* :larm :inverse-kinematics + (send (send *hanger* :get :left-coords) :copy-worldcoords) + :rotation-axis nil + :debug-view nil )) ;; デバッグビューを有効に + + (if ik-ret ;; ik solved + (progn + (format t "IK succeeded: ~A~%" ik-ret) + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) + (unix:sleep 3) + ) + (progn ;;ik failed + (setq ik-ret-torso (send *pr2* :larm :inverse-kinematics + (send (send *hanger* :get :left-coords) :copy-worldcoords) + :rotation-axis nil + :use-torso t ;; 腰を使う + :debug-view nil)) + (if ik-ret-torso ;;腰を使うときのikが成功 + (progn + (format t "IK succeeded with torso: ~A~%" ik-ret-torso) + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) + (unix:sleep 3) + ) + (progn + (let* target-pos (send (send *hanger* :get :left-coords) :worldpos) + (robot-pos (send *pr2* :worldpos)) + (diff-x (- (elt target-pos 0) (elt robot-pos 0))) + (diff-y (- (elt target-pos 1) (elt robot-pos 1))) + + (ros::ros-warn "send :go-velocity") + (send *ri* :go-velocity + (if (> diff-x 0) 0.1 -0.1) + (if (> diff-y 0)0.1 -0.1) + 0));; + ) + ) + ) + ) ) (defun step3-grasp () @@ -103,7 +132,6 @@ (send *irtviewer* :draw-objects) (unix:sleep 1)) - (defun step4-lift () (send *ri* :speak-jp "持ち上げるよ。") "斜め上に移動" @@ -149,41 +177,78 @@ (setq *is-moving* nil) ;; コールバック関数 -(defun box-cb (msg) - - (dolist (box (send msg :boxes)) - (when (member (send box :label) '(566 615 469 923)) - (ros::ros-info "hook is detected") - ;(send *ri* :speak-jp "ハンガー発見！") - (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) - (setq *target-dimensions* (send box :dimensions)) +;;検知したboxとラベルを対応させるクラス +(defclass box-label-synchronizer + :super exact-time-message-filter) + +(defmethod box-label-synchronizer + (:callback (box-msg label-msg) + (print (list box-msg label-msg)) + (print (send-all (list box-msg label-msg) :header :stamp)) + (box-cb box-msg label-msg) + )) + +;; (defun box-cb (msg) + +;; (dolist (box (send msg :boxes)) +;; (when (member (send box :label) '(566 615 469 923)) +;; (ros::ros-info "hook is detected") +;; ;(send *ri* :speak-jp "ハンガー発見！") +;; (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) +;; (setq *target-dimensions* (send box :dimensions)) + +;;コールバック関数 +;;/docker/detic_segmentor/detected_classesというトピックにidの対応あり + + +(defun box-cb (box-msg label-msg) + (ros::ros-info "received ~A boxes, ~A labels" (length (send box-msg :boxes)) (length (send label-msg :labels))) + (dolist (msg-conbined (map cons #'(lambda (x y) (list x y)) (send box-msg :boxes) (send label-msg :labels))) + (let (box label) + ;;(print (list msg-conbined)) + (setq box (car msg-conbined) label (cadr msg-conbined)) + ;;(print (list box label)) + (print (send label :name)) + (when (or (string= (send label :name) "hook") + (string= (send label :name) "coat-hanger") + (string= (send label :name) "hose") + (string= (send label :name) "handle") + (string= (send label :name) "scissors") + ) + (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) + (setq *target-dimensions* (send box :dimensions)) + (format t "target:coords ~A, dimension ~A~%" (send *target-coords* :worldcoords) (* (send *target-dimensions* :x) (send *target-dimensions* :y) (send *target-dimensions* :z))) + ;; 座標を表示 + (format t "Target coords: x=~A y=~A z=~A~%" + (elt (send *target-coords* :worldpos) 0) + (elt (send *target-coords* :worldpos) 1) + (elt (send *target-coords* :worldpos) 2)) - ;; 座標を表示 - (format t "Target coords: x=~A y=~A z=~A~%" - (elt (send *target-coords* :worldpos) 0) - (elt (send *target-coords* :worldpos) 1) - (elt (send *target-coords* :worldpos) 2)) - - (when (and (< (elt (send *target-coords* :worldpos) 2) 1800) - (> (elt (send *target-coords* :worldpos) 2) 0) - (> (elt (send *target-coords* :worldpos) 0) 500) - (< (elt (send *target-coords* :worldpos) 0) 1500) - (> (elt (send *target-coords* :worldpos) 1) -1500) - (< (elt (send *target-coords* :worldpos) 1) 1500)) + (when (and (< (elt (send *target-coords* :worldpos) 2) 1800) + (> (elt (send *target-coords* :worldpos) 2) 0) + (> (elt (send *target-coords* :worldpos) 0) 0) + (< (elt (send *target-coords* :worldpos) 0) 1200) + (> (elt (send *target-coords* :worldpos) 1) -1500) + (< (elt (send *target-coords* :worldpos) 1) 1500)) - (ros::ros-info "target is in reachable area") - (send *ri* :speak-jp "ハンガーが手の届く範囲にありそうます。") - - ;; ハンガーの位置を更新して確認 - (send *hanger* :move-to *target-coords* :world) - (format t "Updated hanger position: ~A~%" (send *hanger* :worldpos)) - (send *irtviewer* :draw-objects) - (unix:sleep 1) ;; 位置更新を確認するための待機 - (setq *found-hanger* t))))) ;; 処理が完了したらループを抜ける + (ros::ros-info "target is in reachable area") + + (send *ri* :speak-jp "ハンガーが手の届く範囲にありそうます。") + + ;; ハンガーの位置を更新して確認 + (send *hanger* :move-to *target-coords* :world) + (format t "Updated hanger position: ~A~%" (send *hanger* :worldpos)) + (send *irtviewer* :draw-objects) + (unix:sleep 1) ;; 位置更新を確認するための待機 + (setq *found-hanger* t)) + ) + )) ;; 処理が完了したらループを抜ける + );; defun (defun grasp-hanger () (setq *is-moving* t) (unwind-protect + (progn (step2-approach) (step3-grasp) @@ -201,8 +266,12 @@ (ros::ros-info "start searching hanger") (send *ri* :speak-jp "ハンガーを探します。") - (ros::subscribe "/synchronized_detic_label_boxes" - jsk_recognition_msgs::BoundingBoxArray #'box-cb) + ;; (ros::subscribe "/synchronized_detic_label_boxes" + ;; jsk_recognition_msgs::BoundingBoxArray #'box-cb) + ;;サブスクライブ + (setq box-sync (instance box-label-synchronizer :init + (list (list "/docker/detic_segmentor/output/boxes" jsk_recognition_msgs::BoundingBoxArray) + (list "/docker/detic_segmentor/detected_classes" jsk_recognition_msgs::LabelArray)))) (do-until-key (when *found-hanger* @@ -214,6 +283,8 @@ (when *found-hanger* (send *ri* :speak-jp "ハンガーを掴みに行きます。") + + (grasp-hanger) (send *ri* :speak-jp "終了しました。") ) @@ -221,6 +292,3 @@ ;; メイン関数を実行 (main) - - - From fc48d56f6419c5cab0c53a8acdee27b56ce0b04b Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 20 Nov 2024 20:40:13 +0900 Subject: [PATCH 5/5] CHANGE hanger-box-ik.l to move if ik could not solve --- jsk_2024_10_semi/hanger-box-ik.l | 194 ++++++++++++++++++------------- 1 file changed, 111 insertions(+), 83 deletions(-) diff --git a/jsk_2024_10_semi/hanger-box-ik.l b/jsk_2024_10_semi/hanger-box-ik.l index 6ec170224d..ef0bf8a1b6 100644 --- a/jsk_2024_10_semi/hanger-box-ik.l +++ b/jsk_2024_10_semi/hanger-box-ik.l @@ -1,4 +1,5 @@ #!/usr/bin/env roseus +;; load "hanger-box-ik.l" ;; PR2のモデルを読み込む (require "package://pr2eus/pr2.l") (require "package://pr2eus/pr2-utils.l") @@ -14,22 +15,23 @@ ;; 三角柱を作成（薄いハンガー形状を模擬） (defun make-hanger () (let ((hanger (make-prism - (list (float-vector 0 0 0) ;; 下の頂点 - (float-vector 150 0 -100) ;; 右上の頂点 - (float-vector -150 0 -100)) ;; 左上の頂点 + (list (float-vector 0 0 0) ;; 上の頂点;;hookの場所 + (float-vector 200 0 -200) ;; 右下の頂点 + (float-vector -200 0 -200)) ;; 左下の頂点 10))) ;; 厚さ10mm ;; 三角柱の下の角に座標系を設定（把持の手前の位置） (send hanger :put :left-coords (make-cascoords :coords (send (send hanger :copy-worldcoords) - :translate (float-vector -20 0 -20) ;; ハンガーの-80 0 -50 + :translate (float-vector -150 0 -150) ;; ハンガーの-80 0 -50 ) ;; ハンガーの下端から相対位置 :rot #2f((0 0 1) ;; y軸方向を向くための回転行列 (-1 0 0) (0 -1 0)) :parent hanger)) - hanger)) + hanger) +) ;; ハンガーをグローバル変数として保持 (setq *hanger* (make-hanger)) @@ -57,72 +59,99 @@ (send *ri* :angle-vector (send *pr2* :angle-vector) 4000) (send *ri* :wait-interpolation) (send *ri* :speak-jp "準備完了しました") -) + ) (defun step2-approach () (send *ri* :speak-jp "ハンガーに腕を伸ばすよ。") - - ; ;; ハンガーの位置を*target-coords*に合わせて更新 - ; (when (boundp '*target-coords*) - ; (send *hanger* :move-to *target-coords* :world) - ; (send *irtviewer* :draw-objects)) - - ;; 左腕で三角柱を掴む 既存のコード - ; (send *pr2* :larm :inverse-kinematics - ; (send (send *hanger* :get :left-coords) :copy-worldcoords) - ; :rotation-axis nil - ; :use-torso t - ; ) ;y - ; (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) - ; (send *ri* :wait-interpolation) - ; (unix:sleep 3) - ; (send *ri* :speak-jp "ハンガーに手を伸ばしました。") - - - - ; ;; 左腕で三角柱を掴む - 逆運動学の結果を変数に保存 - (setq ik-ret (send *pr2* :larm :inverse-kinematics - (send (send *hanger* :get :left-coords) :copy-worldcoords) - :rotation-axis nil - :debug-view nil )) ;; デバッグビューを有効に - - (if ik-ret ;; ik solved - (progn - (format t "IK succeeded: ~A~%" ik-ret) - (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) - (send *ri* :wait-interpolation) - (unix:sleep 3) - ) - (progn ;;ik failed - (setq ik-ret-torso (send *pr2* :larm :inverse-kinematics - (send (send *hanger* :get :left-coords) :copy-worldcoords) - :rotation-axis nil - :use-torso t ;; 腰を使う - :debug-view nil)) - (if ik-ret-torso ;;腰を使うときのikが成功 - (progn - (format t "IK succeeded with torso: ~A~%" ik-ret-torso) - (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) - (send *ri* :wait-interpolation) - (unix:sleep 3) - ) - (progn - (let* target-pos (send (send *hanger* :get :left-coords) :worldpos) - (robot-pos (send *pr2* :worldpos)) - (diff-x (- (elt target-pos 0) (elt robot-pos 0))) - (diff-y (- (elt target-pos 1) (elt robot-pos 1))) - - (ros::ros-warn "send :go-velocity") - (send *ri* :go-velocity - (if (> diff-x 0) 0.1 -0.1) - (if (> diff-y 0)0.1 -0.1) - 0));; - ) - ) - ) - ) + ;; ;; ハンガーの位置を*target-coords*に合わせて更新 + ;; (when (boundp '*target-coords*) + ;; (send *hanger* :move-to *target-coords* :world) + ;; (send *irtviewer* :draw-objects)) + +;; 左腕で三角柱を掴む 既存のコード + ;; (send *pr2* :larm :inverse-kinematics + ;; (send (send *hanger* :get :left-coords) :copy-worldcoords) + ;; :rotation-axis nil + ;; :use-torso t + ;; ) ;y + ;; (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + ;; (send *ri* :wait-interpolation) + ;; (unix:sleep 3) + ;; (send *ri* :speak-jp "ハンガーに手を伸ばしました。") + + + (labels ((try-approach () + (setq ik-ret (send *pr2* :larm :inverse-kinematics + (send (send *hanger* :get :left-coords) :copy-worldcoords) + :rotation-axis nil + :debug-view nil)) + + (if ik-ret ;; ik solved + (progn + (format t "IK succeeded: ~A~%" ik-ret) + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) + (unix:sleep 3) + t) ;; 成功を返す + + (progn ;;ik failed + (setq ik-ret-torso (send *pr2* :larm :inverse-kinematics + (send (send *hanger* :get :left-coords) :copy-worldcoords) + :rotation-axis nil + :use-torso t ;; 腰を使う + :debug-view nil)) + (if ik-ret-torso ;;腰を使うときのikが成功 + (progn + (format t "IK succeeded with torso: ~A~%" ik-ret-torso) + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) + (unix:sleep 3) + t) ;; 成功を返す + nil))))) ;; 失敗を返す + + ;; 最大試行回数を設定 + (let ((max-attempts 5) + (attempt-count 0)) + (while (not (or (try-approach) (>= attempt-count max-attempts))) + (progn + (incf attempt-count) + (let* ((target-pos (send (send *hanger* :get :left-coords) :worldpos)) + (robot-pos (send *pr2* :worldpos)) + (diff-x (- (elt target-pos 0) (elt robot-pos 0))) + (diff-y (- (elt target-pos 1) (elt robot-pos 1)))) + + (ros::ros-warn "Moving closer to target (attempt ~A/~A)" attempt-count max-attempts) + (send *ri* :speak-jp "ハンガーに近づくよ。") + (send *ri* :go-pos + (if (> diff-x 0) 0.1 -0.1) + (if (> diff-y 0) 0.1 -0.1) + 0) + (send *ri* :wait-interpolation) + (ros::ros-info "start searching hanger") + (send *ri* :speak-jp "ハンガーを探します。") + ;; (ros::subscribe "/synchronized_detic_label_boxes" + ;; jsk_recognition_msgs::BoundingBoxArray #'box-cb) + ;;サブスクライブ + (setq box-sync (instance box-label-synchronizer :init + (list (list "/docker/detic_segmentor/output/boxes" jsk_recognition_msgs::BoundingBoxArray) + (list "/docker/detic_segmentor/detected_classes" jsk_recognition_msgs::LabelArray)))) + (setq *found-hanger* nil) + (do-until-key + (when *found-hanger* + (return)) + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once)) + ))) ;; 移動後少し待機 + + ;; 最終結果の確認 + (if (>= attempt-count max-attempts) + (send *ri* :speak-jp "ハンガーに届きませんでした。") + (send *ri* :speak-jp "ハンガーに到達しました。"))))) ) + (defun step3-grasp () (send *ri* :speak-jp "ハンガーを掴むよ。") "ハンドを閉じる動作（シミュレーション用）" @@ -138,7 +167,7 @@ (send *pr2* :larm :inverse-kinematics (send (send (send *hanger* :get :left-coords) :copy-worldcoords) - :translate (float-vector -100 0 150)) ;; 15cm上、10cm手前 + :translate (float-vector -100 0 200)) ;; 20cm上、10cm手前 ;;:move-target (send *hanger* :get :left-coords) ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) :rotation-axis t @@ -223,17 +252,16 @@ (elt (send *target-coords* :worldpos) 0) (elt (send *target-coords* :worldpos) 1) (elt (send *target-coords* :worldpos) 2)) - - (when (and (< (elt (send *target-coords* :worldpos) 2) 1800) - (> (elt (send *target-coords* :worldpos) 2) 0) - (> (elt (send *target-coords* :worldpos) 0) 0) - (< (elt (send *target-coords* :worldpos) 0) 1200) - (> (elt (send *target-coords* :worldpos) 1) -1500) - (< (elt (send *target-coords* :worldpos) 1) 1500)) - - (ros::ros-info "target is in reachable area") + + ;; (when (and (< (elt (send *target-coords* :worldpos) 2) 1800) + ;; (> (elt (send *target-coords* :worldpos) 2) 0) + ;; (> (elt (send *target-coords* :worldpos) 0) 0) + ;; (< (elt (send *target-coords* :worldpos) 0) 1200) + ;; (> (elt (send *target-coords* :worldpos) 1) -1500) + ;; (< (elt (send *target-coords* :worldpos) 1) 1500)) - (send *ri* :speak-jp "ハンガーが手の届く範囲にありそうます。") + ;; (ros::ros-info "target is in reachable area") + ;; (send *ri* :speak-jp "ハンガーが手の届く範囲にありそうます。") ;; ハンガーの位置を更新して確認 (send *hanger* :move-to *target-coords* :world) @@ -274,12 +302,12 @@ (list "/docker/detic_segmentor/detected_classes" jsk_recognition_msgs::LabelArray)))) (do-until-key - (when *found-hanger* - (return)) - (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) - (send *irtviewer* :draw-objects) - (x::window-main-one) - (ros::spin-once)) + (when *found-hanger* + (return)) + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once)) (when *found-hanger* (send *ri* :speak-jp "ハンガーを掴みに行きます。")