From 9722eefbe471960da8b3a4133891ab727feee1d9 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 23 Oct 2024 16:07:05 +0900 Subject: [PATCH 01/10] 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 02/10] 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 03/10] 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 04/10] =?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 05/10] 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 "ハンガーを掴みに行きます。") From bd89bd6a9d944cb0d83fe11d4bb6f5dc8fdeda39 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 27 Nov 2024 17:26:06 +0900 Subject: [PATCH 06/10] CHANGE clothes-box-ik.l SUCCESS --- jsk_2024_10_semi/clothes-box-ik.l | 330 ++++++++++++++++++++++++++++++ 1 file changed, 330 insertions(+) create mode 100644 jsk_2024_10_semi/clothes-box-ik.l diff --git a/jsk_2024_10_semi/clothes-box-ik.l b/jsk_2024_10_semi/clothes-box-ik.l new file mode 100644 index 0000000000..2874bae08e --- /dev/null +++ b/jsk_2024_10_semi/clothes-box-ik.l @@ -0,0 +1,330 @@ +#!/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-clothes () + (let* ((width-x 400) ;; 上辺の幅 + (height-top-z 100) ;; 服からハンガー部分までの高さ + (thickness-y (if (boundp '*target-dimensions*) + (* 1000 (send *target-dimensions* :y)) + 40)) ;;厚さ + (width-bottom-x (if (boundp '*target-dimensions*) + (* 1000 (send *target-dimensions* :x)) + 400));;服の認識された幅 + (height-bottom-z (if(boundp '*target-dimensions*) + (* 1000 (send *target-dimensions* :z)) + 600)) ;; 服の認識された高さ + + ; (clothes (make-prism + ; (list (float-vector (- (/ width-bottom-x 2)) 0 (/ height-bottom-z 2)) ;; 左上の頂点 + ; (float-vector (/ width-bottom-x 2) 0 (/ height-bottom-z 2)) ;; 右上の頂点 + ; ;;(float-vector (/ width-x 2) 0 (+ height-top-z (/ height-bottom-z 2))) ;;中央部頂点 + ; (float-vector (- (/ width-bottom-x 2)) 0 (- (/ height-bottom-z 2))) ;; 下手前頂点 + ; (float-vector (/ width-bottom-x 2) 0 (- (/ height-bottom-z 2))) ;; 下奥頂点 + ; ) + ; thickness-y))) ;; 厚さ + + (clothes (make-cube width-bottom-x thickness-y height-bottom-z)) + ) + + ;; 三角柱の下の角に座標系を設定（把持の手前の位置） + (send clothes :put :left-coords + (make-cascoords + :coords (send (send clothes :copy-worldcoords) + :translate (float-vector (- 50 (/ width-bottom-x 2)) 0 (- (/ height-bottom-z 2) 30)) ;; ハンガーの下端から相対位置 + ;;:translate (float-vector -150 -20 250) + ) + :parent clothes)) + + (when (boundp '*target-coords*) + (send clothes :move-to *target-coords* :world) + + clothes)) + + + ) + +;; 初期位置に配置 +;;(send *clothes* :translate (float-vector 800 100 1100)) + +(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 30) + (send *pr2* :larm :shoulder-r :joint-angle 0) + (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) 4000) + (send *ri* :wait-interpolation) + (send *ri* :speak-jp "準備完了しました") + (send *ri* :wait-interpolation) + ) + +(defun step2-approach () + (send *ri* :speak-jp "服に腕を伸ばすよ。") + + (labels ((try-approach () + (setq ik-ret (send *pr2* :larm :inverse-kinematics + (send (send *clothes* :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 *clothes* :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 *clothes* :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 clothes") + (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-clothes* nil) + (do-until-key + (when *found-clothes* + (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 "服を掴むよ。") + "ハンドを閉じる動作（シミュレーション用）" + ;(send *pr2* :larm :gripper :joint-angle 0) + (send *pr2* :larm :end-coords :assoc *clothes*) + (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 *clothes* :get :left-coords) :copy-worldcoords) + :translate (float-vector -100 0 200)) ;; 20cm上、10cm手前 + ;;:move-target (send *clothes* :get :left-coords) + ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis nil + :debug-view nil + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + ) + +(defun step5-pull () + (send *ri* :speak-jp "手元に引くよ。") + "手元に引く" + (send *pr2* + :inverse-kinematics (send (send (send *clothes* :get :left-coords) :copy-worldcoords) + :translate (float-vector -100 0 50)) ;; さらに30cm手前 + ;:move-target (send *clothes* :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 *ri* :wait-interpolation) + (send *irtviewer* :draw-objects)) + +(defun step6-release () + (send *ri* :speak-jp "服を離すよ。") + "ハンドを開く動作（シミュレーション用）" + ;(send *pr2* :larm :gripper :joint-angle 40) + + ; シミュレーションで服の関連付けを解除 + (send *pr2* :larm :end-coords :dissoc *clothes*) + (send *ri* :stop-grasp :larm) + (send *irtviewer* :draw-objects) + ) + + ;服を把持しようとしているかどうかのフラグ +(setq *found-clothes* nil) +(setq *is-moving* nil) + +;; コールバック関数 +;;検知した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) "bathrobe") + (string= (send label :name) "jacket") + (string= (send label :name) "coat") + (string= (send label :name) "shirt") + (string= (send label :name) "robe") + ) + (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)) + + (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") + + ;; 服の位置を更新して確認 + (setq *clothes* (make-clothes)) + (objects (append (list *pr2*) (list *clothes*))) + (send *irtviewer* :draw-objects) + ;; (send *clothes* :move-to *target-coords* :world) + ;; (format t "Updated clothes position: ~A~%" (send *clothes* :worldpos)) + ;; (send *irtviewer* :draw-objects) + ;; (unix:sleep 1) ;; 位置更新を確認するための待機 + (setq *found-clothes* t)) + ) + )) ;; 処理が完了したらループを抜ける + );; defun + +(defun grasp-clothes () + (setq *is-moving* t) + (unwind-protect + + (progn + (step2-approach) + (step3-grasp) + (unix:sleep 2) + (step4-lift) + ;;(step5-pull) + (unix:sleep 2) + (step6-release)) + (setq *is-moving* nil))) + +(defun main () + (ros::ros-info "start main loop") + (objects (list *pr2*)) + (step1-init) + (unix:sleep 1) + + (ros::ros-info "start searching clothes") + (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)))) + + (do-until-key + (when *found-clothes* + (return)) + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once)) + + (when *found-clothes* + (send *ri* :speak-jp "服を掴みに行きます。") + + + (grasp-clothes) + (send *ri* :speak-jp "終了しました。") + ) + ) + +;; メイン関数を実行 +(main) From 1a6e46314bfbd4352c34902204b0c3a4c84e909b Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 4 Dec 2024 16:24:19 +0900 Subject: [PATCH 07/10] CHANGE parameter of the place to grab --- jsk_2024_10_semi/clothes-box-ik.l | 132 ++++++++++++++++++++++++++---- 1 file changed, 114 insertions(+), 18 deletions(-) diff --git a/jsk_2024_10_semi/clothes-box-ik.l b/jsk_2024_10_semi/clothes-box-ik.l index 2874bae08e..686e9f99e0 100644 --- a/jsk_2024_10_semi/clothes-box-ik.l +++ b/jsk_2024_10_semi/clothes-box-ik.l @@ -41,7 +41,7 @@ (send clothes :put :left-coords (make-cascoords :coords (send (send clothes :copy-worldcoords) - :translate (float-vector (- 50 (/ width-bottom-x 2)) 0 (- (/ height-bottom-z 2) 30)) ;; ハンガーの下端から相対位置 + :translate (float-vector (- 60 (/ width-bottom-x 2)) 0 (- (/ height-bottom-z 2) 60)) ;; ハンガーの下端から相対位置 ;;:translate (float-vector -150 -20 250) ) :parent clothes)) @@ -134,7 +134,7 @@ (ros::ros-info "start searching clothes") (send *ri* :speak-jp "服を探します。") ;; (ros::subscribe "/synchronized_detic_label_boxes" - ;; jsk_recognition_msgs::BoundingBoxArray #'box-cb) + ;; jsk_recognition_msgs::BoundingBoxArray #'clothes-box-cb) ;;サブスクライブ (setq box-sync (instance box-label-synchronizer :init (list (list "/docker/detic_segmentor/output/boxes" jsk_recognition_msgs::BoundingBoxArray) @@ -170,13 +170,14 @@ (send *pr2* :larm :inverse-kinematics (send (send (send *clothes* :get :left-coords) :copy-worldcoords) - :translate (float-vector -100 0 200)) ;; 20cm上、10cm手前 + :translate (float-vector -100 0 150)) ;; 15cm上、10cm手前 ;;:move-target (send *clothes* :get :left-coords) ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) - :rotation-axis nil + :rotation-axis t + :use-torso t :debug-view nil ) - (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) (send *ri* :wait-interpolation) (send *irtviewer* :draw-objects) ) @@ -195,7 +196,7 @@ (send *ri* :wait-interpolation) (send *irtviewer* :draw-objects)) -(defun step6-release () +(defun release () (send *ri* :speak-jp "服を離すよ。") "ハンドを開く動作（シミュレーション用）" ;(send *pr2* :larm :gripper :joint-angle 40) @@ -219,10 +220,10 @@ (: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) + (clothes-box-cb box-msg label-msg) )) -;; (defun box-cb (msg) +;; (defun clothes-box-cb (msg) ;; (dolist (box (send msg :boxes)) ;; (when (member (send box :label) '(566 615 469 923)) @@ -235,7 +236,7 @@ ;;/docker/detic_segmentor/detected_classesというトピックにidの対応あり -(defun box-cb (box-msg label-msg) +(defun clothes-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) @@ -244,10 +245,11 @@ ;;(print (list box label)) (print (send label :name)) (when (or (string= (send label :name) "bathrobe") - (string= (send label :name) "jacket") + ;;(string= (send label :name) "jacket") (string= (send label :name) "coat") - (string= (send label :name) "shirt") + ;;(string= (send label :name) "shirt") (string= (send label :name) "robe") + (string= (send label :name) "sweatshirt") ) (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) (setq *target-dimensions* (send box :dimensions)) @@ -280,6 +282,8 @@ )) ;; 処理が完了したらループを抜ける );; defun + + (defun grasp-clothes () (setq *is-moving* t) (unwind-protect @@ -288,12 +292,88 @@ (step2-approach) (step3-grasp) (unix:sleep 2) - (step4-lift) - ;;(step5-pull) + ;;(step4-lift) (unix:sleep 2) - (step6-release)) + ) (setq *is-moving* nil))) + +(defun person-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 (string= (send label :name) "person") + (setq *person-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) + (setq *person-dimensions* (send box :dimensions)) + (format t "person:coords ~A, dimension ~A~%" (send *person-coords* :worldcoords) (* (send *person-dimensions* :x) (send *person-dimensions* :y) (send *target-dimensions* :z))) + ;; 座標を表示 + (format t "person coords: x=~A y=~A z=~A~%" + (elt (send *person-coords* :worldpos) 0) + (elt (send *person-coords* :worldpos) 1) + (elt (send *person-coords* :worldpos) 2)) + + ;;(when (and (< (elt (send *person-coords* :worldpos) 2) 1800) + ;; (> (elt (send *person-coords* :worldpos) 2) 0) + ;; (> (elt (send *person-coords* :worldpos) 0) 0) + ;; (< (elt (send *person-coords* :worldpos) 0) 1200) + ;; (> (elt (send *person-coords* :worldpos) 1) -1500) + ;; (< (elt (send *person-coords* :worldpos) 1) 1500) + ;; ) + ;;(ros::ros-info "person is in reachable area") + + ;; 人の位置を更新して確認 + (setq *person* (make-person)) + (objects (append (list *pr2*) (list *person*))) + (send *irtviewer* :draw-objects) + ;; (send *person* :move-to *target-coords* :world) + ;; (format t "Updated person position: ~A~%" (send *person* :worldpos)) + ;; (send *irtviewer* :draw-objects) + ;; (unix:sleep 1) ;; 位置更新を確認するための待機 + (setq *found-person* t)) + ;;) + )) ;; 処理が完了したらループを抜ける + );; defun + +(defun make-person () + (let ((person (make-cube 200 200 200))) + (when (boundp '*person-coords*) + (send person :move-to *person-coords* :world) + person))) + +(defun move-to-person () + (send *ri* :speak-jp "人に近づくよ。") + (let* (person-pos (send *person-coords* :worldpos)) + (robot-pos (send *pr2* :worldpos)) + (diff-x (- (elt person-pos 0) (elt robot-pos 0))) + (diff-y (- (elt person-pos 1) (elt robot-pos 1))) + (distance (sqrt (+ (* diff-x diff-x) (* diff-y diff-y)))) + ;;1.5mの距離まで近づく + (ratio (/ 1500 distance)) + (target-x (* ratio diff-x)) + (target-y (* ratio diff-y))) + + (send *ri* :go-pos target-x target-y 0) +) + +(defun search-person-with-timeout () + (let ((start-time (ros::time-now)) + (timeout 5.0)) ;; 5秒のタイムアウト + (setq *found-person* nil) + (do-until-key + (when (or *found-person* + (> (send (ros::time- (ros::time-now) start-time) :to-sec) + timeout)) + (return)) + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once)) + *found-person*)) ;; 人が見つかったかどうかを返す + (defun main () (ros::ros-info "start main loop") (objects (list *pr2*)) @@ -303,7 +383,7 @@ (ros::ros-info "start searching clothes") (send *ri* :speak-jp "服を探します。") ;; (ros::subscribe "/synchronized_detic_label_boxes" - ;; jsk_recognition_msgs::BoundingBoxArray #'box-cb) + ;; jsk_recognition_msgs::BoundingBoxArray #'clothes-box-cb) ;;サブスクライブ (setq box-sync (instance box-label-synchronizer :init (list (list "/docker/detic_segmentor/output/boxes" jsk_recognition_msgs::BoundingBoxArray) @@ -319,11 +399,27 @@ (when *found-clothes* (send *ri* :speak-jp "服を掴みに行きます。") + (grasp-clothes) + ) + ;; 人を探すプロセス + ; (send *ri* :speak-jp "人を探します。") + ; (setq *found-person* nil) + ; ;; main関数内の人を探すプロセスの前に追加 + ; (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)))) + ; (if (search-person-with-timeout) + ; (progn + ; (send *ri* :speak-jp "人を見つけました。") + ; (move-to-person)) + ; (progn ;;人ｇ見つからなかった場合 + ; (send *ri* :speak-jp "人を見つけられませんでした。"))) + + (release) - (grasp-clothes) - (send *ri* :speak-jp "終了しました。") - ) + (send *ri* :speak-jp "終了しました。") + ) ;; メイン関数を実行 From ecb524093cba2b09ecf267d2aed05c0ef96371fa Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 4 Dec 2024 21:02:49 +0900 Subject: [PATCH 08/10] ADD rarm --- jsk_2024_10_semi/clothes-box-ik.l | 126 +++++++++++++++++++++--------- 1 file changed, 90 insertions(+), 36 deletions(-) diff --git a/jsk_2024_10_semi/clothes-box-ik.l b/jsk_2024_10_semi/clothes-box-ik.l index 686e9f99e0..2e881d7609 100644 --- a/jsk_2024_10_semi/clothes-box-ik.l +++ b/jsk_2024_10_semi/clothes-box-ik.l @@ -11,7 +11,7 @@ (if (not (boundp '*pr2*)) (setq *pr2* (pr2))) -;; 三角柱を作成（かかっている服の形状を模擬） +;; 直方体を作成（かかっている服の形状を模擬） (defun make-clothes () (let* ((width-x 400) ;; 上辺の幅 (height-top-z 100) ;; 服からハンガー部分までの高さ @@ -35,13 +35,13 @@ ; thickness-y))) ;; 厚さ (clothes (make-cube width-bottom-x thickness-y height-bottom-z)) - ) + ) - ;; 三角柱の下の角に座標系を設定（把持の手前の位置） + ;; 直方体の下の角に座標系を設定（把持の手前の位置） (send clothes :put :left-coords (make-cascoords :coords (send (send clothes :copy-worldcoords) - :translate (float-vector (- 60 (/ width-bottom-x 2)) 0 (- (/ height-bottom-z 2) 60)) ;; ハンガーの下端から相対位置 + :translate (float-vector (- 80 (/ width-bottom-x 2)) 0 (- (/ height-bottom-z 2) 85)) ;; ハンガーの下端から相対位置 ;;:translate (float-vector -150 -20 250) ) :parent clothes)) @@ -50,8 +50,6 @@ (send clothes :move-to *target-coords* :world) clothes)) - - ) ;; 初期位置に配置 @@ -88,30 +86,53 @@ (setq ik-ret (send *pr2* :larm :inverse-kinematics (send (send *clothes* :get :left-coords) :copy-worldcoords) :rotation-axis nil - :debug-view nil)) + :debug-view nil + :use-torso t + )) (if ik-ret ;; ik solved (progn (format t "IK succeeded: ~A~%" ik-ret) + (send *pr2* :larm :inverse-kinematics + (send (send (send *clothes* :get :left-coords) :copy-worldcoords) + :translate (float-vector -100 0 0)) + :rotation-axis nil + :debug-view nil + :use-torso t + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) + (send *pr2* :larm :inverse-kinematics + (send (send *clothes* :get :left-coords) :copy-worldcoords) + :rotation-axis nil + :debug-view nil + :use-torso t + ) + + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) (unix:sleep 3) t) ;; 成功を返す - (progn ;;ik failed - (setq ik-ret-torso (send *pr2* :larm :inverse-kinematics - (send (send *clothes* :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))))) ;; 失敗を返す + ;; (progn ;;ik failed + ;; (setq ik-ret-torso (send *pr2* :larm :inverse-kinematics + ;; (send (send *clothes* :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) @@ -151,7 +172,7 @@ ;; 最終結果の確認 (if (>= attempt-count max-attempts) - (send *ri* :speak-jp "服に届きませんでした。") + (send *ri* :speak-jp "服に届きませんでした。") (send *ri* :speak-jp "服に到達しました。")))) ) @@ -162,21 +183,46 @@ (send *pr2* :larm :end-coords :assoc *clothes*) (send *ri* :start-grasp :larm) (send *irtviewer* :draw-objects) - (unix:sleep 1)) + (unix:sleep 1) + + ) + + (defun step4-lift () (send *ri* :speak-jp "持ち上げるよ。") - "斜め上に移動" + (send *pr2* :rarm :inverse-kinematics + (send (send (send *clothes* :get :left-coords) :copy-worldcoords) + :translate (float-vector -10 0 -150)) + :rotation-axis t + :debug-view nil + :use-torso t + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *pr2* :rarm :inverse-kinematics + (send (send (send *clothes* :get :left-coords) :copy-worldcoords) + :translate (float-vector 50 0 -100)) + :rotation-axis t + :debug-view nil + :use-torso t + ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + + (send *ri* :wait-interpolation) + + (send *pr2* - :larm - :inverse-kinematics (send (send (send *clothes* :get :left-coords) :copy-worldcoords) - :translate (float-vector -100 0 150)) ;; 15cm上、10cm手前 - ;;:move-target (send *clothes* :get :left-coords) - ;;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + ;;:larm + :inverse-kinematics (send (send (send (send *clothes* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0 0 150)) + :rotate (deg2rad -30) :y + ) :rotation-axis t :use-torso t :debug-view nil ) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) (send *ri* :wait-interpolation) (send *irtviewer* :draw-objects) @@ -186,14 +232,21 @@ (send *ri* :speak-jp "手元に引くよ。") "手元に引く" (send *pr2* + :larm :inverse-kinematics (send (send (send *clothes* :get :left-coords) :copy-worldcoords) - :translate (float-vector -100 0 50)) ;; さらに30cm手前 + :translate (float-vector -100 0 0)) ;; さらに30cm手前 ;:move-target (send *clothes* :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) + :rotation-axis nil + :debug-view nil) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) (send *ri* :wait-interpolation) + ;; (send *pr2* + ;; :rarm + ;; :reset-pose) + ;; (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects)) (defun release () @@ -246,9 +299,9 @@ (print (send label :name)) (when (or (string= (send label :name) "bathrobe") ;;(string= (send label :name) "jacket") - (string= (send label :name) "coat") + ;;(string= (send label :name) "coat") ;;(string= (send label :name) "shirt") - (string= (send label :name) "robe") + ;;(string= (send label :name) "robe") (string= (send label :name) "sweatshirt") ) (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) @@ -292,7 +345,8 @@ (step2-approach) (step3-grasp) (unix:sleep 2) - ;;(step4-lift) + (step4-lift) + (step5-pull) (unix:sleep 2) ) (setq *is-moving* nil))) From ffe97469982f7389204f8747f289eabd823d1870 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 11 Dec 2024 18:55:15 +0900 Subject: [PATCH 09/10] =?UTF-8?q?ADD=20=E3=81=A4=E3=81=8B=E3=82=81?= =?UTF-8?q?=E3=81=A6=E3=81=AA=E3=81=8B=E3=81=A3=E3=81=9F=E3=82=89=E5=86=8D?= =?UTF-8?q?=E3=83=81=E3=83=A3=E3=83=AC=E3=83=B3=E3=82=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- jsk_2024_10_semi/clothes-box-ik.l | 185 ++++++++++++++++++++---------- 1 file changed, 126 insertions(+), 59 deletions(-) diff --git a/jsk_2024_10_semi/clothes-box-ik.l b/jsk_2024_10_semi/clothes-box-ik.l index 2e881d7609..08cc47da78 100644 --- a/jsk_2024_10_semi/clothes-box-ik.l +++ b/jsk_2024_10_semi/clothes-box-ik.l @@ -80,6 +80,7 @@ ) (defun step2-approach () + (unix:sleep 2) (send *ri* :speak-jp "服に腕を伸ばすよ。") (labels ((try-approach () @@ -169,19 +170,26 @@ (x::window-main-one) (ros::spin-once)) ))) ;; 移動後少し待機 + + ;; 最終結果の確認 (if (>= attempt-count max-attempts) (send *ri* :speak-jp "服に届きませんでした。") - (send *ri* :speak-jp "服に到達しました。")))) + (send *ri* :speak-jp "服に到達しました。") + ) + )) ) (defun step3-grasp () + (send *ri* :stop-grasp :larm :wait t) + ;;"ハンドを閉じる動作（シミュレーション用）" + (send *ri* :wait-interpolation) + (send *ri* :speak-jp "服を掴むよ。") - "ハンドを閉じる動作（シミュレーション用）" - ;(send *pr2* :larm :gripper :joint-angle 0) - (send *pr2* :larm :end-coords :assoc *clothes*) - (send *ri* :start-grasp :larm) + + ;;(send *pr2* :larm :gripper :joint-angle 0) + (send *ri* :start-grasp :larm :wait t) (send *irtviewer* :draw-objects) (unix:sleep 1) @@ -202,16 +210,16 @@ (send *ri* :wait-interpolation) (send *pr2* :rarm :inverse-kinematics (send (send (send *clothes* :get :left-coords) :copy-worldcoords) - :translate (float-vector 50 0 -100)) + :translate (float-vector 40 0 -100)) :rotation-axis t :debug-view nil - :use-torso t + :use-torso nil ) (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) - + (send *ri* :wait-interpolation) - + (send *pr2* ;;:larm :inverse-kinematics (send (send (send (send *clothes* :get :left-coords) :copy-worldcoords) @@ -230,32 +238,46 @@ (defun step5-pull () (send *ri* :speak-jp "手元に引くよ。") - "手元に引く" + ;;"手元に引く" (send *pr2* :larm :inverse-kinematics (send (send (send *clothes* :get :left-coords) :copy-worldcoords) - :translate (float-vector -100 0 0)) ;; さらに30cm手前 + :translate (float-vector -200 0 0)) ;; さらに30cm手前 ;:move-target (send *clothes* :get :left-coords) ;:link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) :rotation-axis nil :debug-view nil) (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) (send *ri* :wait-interpolation) - ;; (send *pr2* - ;; :rarm - ;; :reset-pose) - ;; (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + ;; (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) (send *ri* :wait-interpolation) (send *irtviewer* :draw-objects)) +(defun step6-setpose () + (send *pr2* :reset-pose) + (send *pr2* :larm :collar-y :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 -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 *ri* :angle-vector (send *pr2* :angle-vector) 4000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + ) + (defun release () (send *ri* :speak-jp "服を離すよ。") - "ハンドを開く動作（シミュレーション用）" - ;(send *pr2* :larm :gripper :joint-angle 40) + ;;"ハンドを開く動作（シミュレーション用）" + ;;(send *pr2* :larm :gripper :joint-angle 40) - ; シミュレーションで服の関連付けを解除 + ;; シミュレーションで服の関連付けを解除 (send *pr2* :larm :end-coords :dissoc *clothes*) + (send *ri* :wait-interpolation) (send *ri* :stop-grasp :larm) (send *irtviewer* :draw-objects) ) @@ -265,7 +287,7 @@ (setq *is-moving* nil) ;; コールバック関数 -;;検知したboxとラベルを対応させるクラス +;;検知したboxとラベルを対応させるクラス (defclass box-label-synchronizer :super exact-time-message-filter) @@ -285,7 +307,7 @@ ;; (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の対応あり @@ -335,20 +357,67 @@ )) ;; 処理が完了したらループを抜ける );; defun +(defun find-main () + (setq *found-clothes* nil) + + (ros::ros-info "start searching clothes") + (send *ri* :speak-jp "服を探します。") + ;; (ros::subscribe "/synchronized_detic_label_boxes" + ;; jsk_recognition_msgs::BoundingBoxArray #'clothes-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-clothes* + (return)) + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once)) + ) +;; 服を掴む一連の動作 (defun grasp-clothes () (setq *is-moving* t) (unwind-protect (progn - (step2-approach) - (step3-grasp) + (setq *grasp-yet-tag* t) + + (while *grasp-yet-tag* + (find-main) + + (when *found-clothes* + (step2-approach) + (step3-grasp) + + (let ((gripper-distance (send *ri* :robot :larm :gripper :joint-angle))) + (ros::ros-info "gripper-distance ~A mm" gripper-distance) + (if (< gripper-distance 3) ;;3mmより指の間が小さいときはやり直し + (progn + (send *ri* :speak-jp "服を掴めていない気がするよ") + (send *ri* :wait-interpolation) + (send *ri* :stop-grasp :larm :wait t) + ) + (progn + (setq *grasp-yet-tag* nil) + (send *pr2* :larm :end-coords :assoc *clothes*) + ) + ) + ) + ) + ) + + (unix:sleep 2) (step4-lift) (step5-pull) (unix:sleep 2) + (step6-setpose) ) + (setq *is-moving* nil))) @@ -356,9 +425,9 @@ (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)) + ;;(print (list msg-conbined)) (setq box (car msg-conbined) label (cadr msg-conbined)) - ;;(print (list box label)) + ;;(print (list box label)) (print (send label :name)) (when (string= (send label :name) "person") (setq *person-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) @@ -434,41 +503,39 @@ (step1-init) (unix:sleep 1) - (ros::ros-info "start searching clothes") - (send *ri* :speak-jp "服を探します。") - ;; (ros::subscribe "/synchronized_detic_label_boxes" - ;; jsk_recognition_msgs::BoundingBoxArray #'clothes-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-clothes* - (return)) - (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) - (send *irtviewer* :draw-objects) - (x::window-main-one) - (ros::spin-once)) - - (when *found-clothes* - (send *ri* :speak-jp "服を掴みに行きます。") - (grasp-clothes) - ) - - ;; 人を探すプロセス - ; (send *ri* :speak-jp "人を探します。") - ; (setq *found-person* nil) - ; ;; main関数内の人を探すプロセスの前に追加 - ; (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)))) - ; (if (search-person-with-timeout) - ; (progn - ; (send *ri* :speak-jp "人を見つけました。") - ; (move-to-person)) - ; (progn ;;人ｇ見つからなかった場合 - ; (send *ri* :speak-jp "人を見つけられませんでした。"))) + ;; (ros::ros-info "start searching clothes") + ;; (send *ri* :speak-jp "服を探します。") + ;; ;; (ros::subscribe "/synchronized_detic_label_boxes" + ;; ;; jsk_recognition_msgs::BoundingBoxArray #'clothes-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-clothes* + ;; (return)) + ;; (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + ;; (send *irtviewer* :draw-objects) + ;; (x::window-main-one) + ;; (ros::spin-once)) + + (grasp-clothes) + + + ;; ;; 人を探すプロセス + ;; (send *ri* :speak-jp "人を探します。") + ;; (setq *found-person* nil) + ;; main関数内の人を探すプロセスの前に追加 + ;; (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)))) + ;; (if (search-person-with-timeout) + ;; (progn + ;; (send *ri* :speak-jp "人を見つけました。") + ;; (move-to-person)) + ;; (progn ;;人ｇ見つからなかった場合 + ;; (send *ri* :speak-jp "人を見つけられませんでした。"))) (release) From ebd42aab442287ef24dd881ad0d3ef4b07811beb Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 11 Dec 2024 22:43:00 +0900 Subject: [PATCH 10/10] =?UTF-8?q?CHANGE=20box-cb=E3=82=92=E4=B8=80?= =?UTF-8?q?=E8=88=AC=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- jsk_2024_10_semi/clothes-box-ik.l | 109 +++++++++++++++++++----------- 1 file changed, 70 insertions(+), 39 deletions(-) diff --git a/jsk_2024_10_semi/clothes-box-ik.l b/jsk_2024_10_semi/clothes-box-ik.l index 08cc47da78..88600d68f4 100644 --- a/jsk_2024_10_semi/clothes-box-ik.l +++ b/jsk_2024_10_semi/clothes-box-ik.l @@ -10,6 +10,14 @@ ;; PR2のインスタンスを作成 (if (not (boundp '*pr2*)) (setq *pr2* (pr2))) +;; (defun make-target-model (target) +;; (if (= target "clothes") +;; (make-clothes) +;; ) +;; (if (= target "person") +;; (make-person) +;; ) +;; ) ;; 直方体を作成（かかっている服の形状を模擬） (defun make-clothes () @@ -155,32 +163,35 @@ (send *ri* :wait-interpolation) (ros::ros-info "start searching clothes") (send *ri* :speak-jp "服を探します。") - ;; (ros::subscribe "/synchronized_detic_label_boxes" - ;; jsk_recognition_msgs::BoundingBoxArray #'clothes-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-clothes* nil) - (do-until-key - (when *found-clothes* - (return)) - (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) - (send *irtviewer* :draw-objects) - (x::window-main-one) - (ros::spin-once)) - ))) ;; 移動後少し待機 - + ;; (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* nil) + ;; (do-until-key + ;; (when *found* + ;; (return)) + ;; (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + ;; (send *irtviewer* :draw-objects) + ;; (x::window-main-one) + ;; (ros::spin-once)) + (find-main '("bathrobe" "swearshirt")) + + ) + ) + ) ;; 移動後少し待機 ;; 最終結果の確認 (if (>= attempt-count max-attempts) - (send *ri* :speak-jp "服に届きませんでした。") + (send *ri* :speak-jp "服に届きませんでした。") (send *ri* :speak-jp "服に到達しました。") ) - )) + ) + ) ) + (defun step3-grasp () (send *ri* :stop-grasp :larm :wait t) ;;"ハンドを閉じる動作（シミュレーション用）" @@ -283,20 +294,28 @@ ) ;服を把持しようとしているかどうかのフラグ -(setq *found-clothes* nil) +(setq *found* nil) (setq *is-moving* nil) ;; コールバック関数 ;;検知したboxとラベルを対応させるクラス (defclass box-label-synchronizer - :super exact-time-message-filter) + :super exact-time-message-filter + :slots (target-label-list) + ) (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)) - (clothes-box-cb box-msg label-msg) - )) + (box-cb box-msg label-msg target-label-list) + ) + ) + +(defmethod box-label-synchronizer + (:set-target-label-list (label-list) + (setq target-label-list label-list)) + ) ;; (defun clothes-box-cb (msg) @@ -311,7 +330,7 @@ ;;/docker/detic_segmentor/detected_classesというトピックにidの対応あり -(defun clothes-box-cb (box-msg label-msg) +(defun box-cb (box-msg label-msg label-list) (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) @@ -319,14 +338,15 @@ (setq box (car msg-conbined) label (cadr msg-conbined)) ;;(print (list box label)) (print (send label :name)) - (when (or (string= (send label :name) "bathrobe") - ;;(string= (send label :name) "jacket") - ;;(string= (send label :name) "coat") - ;;(string= (send label :name) "shirt") - ;;(string= (send label :name) "robe") - (string= (send label :name) "sweatshirt") - ) - (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) + ;; (when (or (string= (send label :name) "bathrobe") + ;; ;;(string= (send label :name) "jacket") + ;; ;;(string= (send label :name) "coat") + ;; ;;(string= (send label :name) "shirt") + ;; ;;(string= (send label :name) "robe") + ;; (string= (send label :name) "sweatshirt") + ;; ) + (when (contains label-list (send label :name)) + (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))) ;; 座標を表示 @@ -352,25 +372,34 @@ ;; (format t "Updated clothes position: ~A~%" (send *clothes* :worldpos)) ;; (send *irtviewer* :draw-objects) ;; (unix:sleep 1) ;; 位置更新を確認するための待機 - (setq *found-clothes* t)) + (setq *found* t)) ) )) ;; 処理が完了したらループを抜ける );; defun -(defun find-main () - (setq *found-clothes* nil) +(defun contains (list word) + (let ((tmp nil)) + (while list + (if (string= word (car list)) + (progn (setq tmp t) (return)) + (setq list (cdr list)))) + tmp) + ) + + +(defun find-main (label-list) + (setq *found* nil) - (ros::ros-info "start searching clothes") - (send *ri* :speak-jp "服を探します。") ;; (ros::subscribe "/synchronized_detic_label_boxes" ;; jsk_recognition_msgs::BoundingBoxArray #'clothes-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)))) + (send box-sync :set-target-label-list label-list) (do-until-key - (when *found-clothes* + (when *found* (return)) (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) (send *irtviewer* :draw-objects) @@ -387,15 +416,17 @@ (setq *grasp-yet-tag* t) (while *grasp-yet-tag* - (find-main) + (ros::ros-info "start searching clothes") + (send *ri* :speak-jp "服を探します。") + (find-main '("bathrobe" "swearshirt")) - (when *found-clothes* + (when *found* ;;服が見つかった時 (step2-approach) (step3-grasp) (let ((gripper-distance (send *ri* :robot :larm :gripper :joint-angle))) (ros::ros-info "gripper-distance ~A mm" gripper-distance) - (if (< gripper-distance 3) ;;3mmより指の間が小さいときはやり直し + (if (< gripper-distance 6.5) ;;6.5mmより指の間が小さいときはやり直し (progn (send *ri* :speak-jp "服を掴めていない気がするよ") (send *ri* :wait-interpolation)