Skip to content

Commit

Permalink
CHANGE hanger-box-ik.l to move if ik could not solve
Browse files Browse the repository at this point in the history
  • Loading branch information
Your Name committed Nov 20, 2024
1 parent f9d1151 commit fc48d56
Showing 1 changed file with 111 additions and 83 deletions.
194 changes: 111 additions & 83 deletions jsk_2024_10_semi/hanger-box-ik.l
View file
Original file line number Diff line number Diff line change
@@ -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")
Expand All @@ -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))
Expand Down Expand Up @@ -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 "ハンガーを掴むよ。")
"ハンドを閉じる動作（シミュレーション用）"
Expand All @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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 "ハンガーを掴みに行きます。")
Expand Down

0 comments on commit fc48d56

Please sign in to comment.