Skip to content

Commit

Permalink
ボール回収からちゃぶ台返しができる
Browse files Browse the repository at this point in the history
  • Loading branch information
@Rinno1478
Rinno1478 committed Dec 23, 2024
1 parent b7a3abd commit d782f0e
Show file tree
Hide file tree
Showing 5 changed files with 596 additions and 203 deletions.
325 changes: 236 additions & 89 deletions jsk_2024_10_semi/scripts/catch.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,18 @@
;;pr2生成
(if (not (boundp '*pr2*)) (pr2-init))
(setq *ri* (instance pr2-interface :init))
(objects (list *pr2*))
;; 一辺600mmの立方体を出現させる
(setq *cube* (make-cube 600 600 600))
;; 立方体を(700, 0, 300)移動
(send *cube* :translate (float-vector 700 0 300))
;;boxを適当な初期位置で配置
(setq *target-box* (make-cube 20 20 20))
(objects (list *pr2* *target-box* *cube*))

;; chabudaiフラグ
(setq *chabudai* nil)
;; ボールを回収した回数
(setq *ball-count* 0)

(setq *tfl* (instance ros::transform-listener :init))
;;検知したboxとラベルを対応させるクラス
Expand All @@ -28,22 +39,12 @@
(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))
(ros::ros-info "box ~A, label ~A" (send box :pose) (send label :name))
(when (or (string= (send label :name) "ball") (string= (send label :name) "soccer_ball") (string= (send label :name) "beachball"));; 41 -> ball, 1110 ->toy
(when (or (string= (send label :name) "ball") (string= (send label :name) "soccer_ball") (string= (send label :name) "tennis_ball") (string= (send label :name) "beachball") (string= (send label :name) "ping-pong_ball"))
(setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords))
;; (setq tfc (send *tfl* :lookup-transform "base_link" "head_mount_kinect_rgb_optical_frame" (ros::time 0)))
;; (print *target-coords*)
;; (when *tfl*
;; (setq *target-coords* (send tfc :transform *target-coords*))
;; )
;; (print *target-coords*)
(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) 400)
; (> (elt (send *target-coords* :worldpos) 2) -400))
(ros::ros-info "coords ~A, dimension ~A" (send *target-coords* :worldcoords) (* (send *target-dimensions* :x) (send *target-dimensions* :y) (send *target-dimensions* :z)))
(send *target-box* :move-to *target-coords* :world)
(ros::ros-info "update target position")
; )
Expand All @@ -52,116 +53,262 @@
)
)

;;コールバック関数
; (setq *tfl* (instance ros::transform-listener :init))
; (defun callback (msg)
; (ros::ros-info "detect circle ~A" (send msg :point))
; (let ((circle-pose (instance geometry_msgs::posestamped :init)))
; (send circle-pose :pose :position (send msg :point))
; (send circle-pose :pose :orientation :x 0.0)
; (send circle-pose :pose :orientation :y 0.0)
; (send circle-pose :pose :orientation :z 0.0)
; (send circle-pose :pose :orientation :w 1.0)
; (setq *target-coords* (send (ros::tf-pose->coords (send circle-pose :pose)) :copy-worldcoords))
; )
; (setq tfc (send *tfl* :lookup-transform "base_link" "head_mount_kinect_rgb_optical_frame" (ros::time 0)))
; (when tfc
; (setq *target-coords* (send tfc :transform *target-coords*))
; (send *target-box* :move-to *target-coords* :world)
; (ros::ros-info "update target position")
; )
; )
(defun chabudai-cb (msg)
(ros::ros-info "robot_mood: ~A" (send msg :data))
(ros::ros-info "ball_count: ~A" *ball-count*)
(when (> (send msg :data) (- 9 *ball-count*))
(setq *chabudai* t)
)
)

(defun init-pose ()
(send *ri* :speak-jp "初期姿勢に移動します" :wait t)
; (send *ri* :speak-jp "初期姿勢に移動します" :wait t)
(send *pr2* :reset-pose)
(send *pr2* :torso :waist-z :joint-angle 150)
(send *pr2* :larm :shoulder-p :joint-angle 0)
(send *pr2* :larm :shoulder-r :joint-angle 90)
(send *pr2* :head :neck-p :joint-angle 60)
(send *ri* :stop-grasp :arms)
(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 3000)
(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
(send *ri* :wait-interpolation)
)

(defun chabudai-gaeshi ()
(send *cube* :put :left-coords
(make-cascoords
:coords (send (send *cube* :copy-worldcoords) :translate (float-vector 700 0 300))
:parent *cube*)
)

(send *pr2* :larm :inverse-kinematics
(send (send *cube* :get :left-coords) :copy-worldcoords)
:rotation-axis :z
)

(send *pr2* :reset-pose)
(send *pr2* :torso :waist-z :joint-angle 150)
(send *pr2* :larm :collar-y :joint-angle 100)
(send *pr2* :rarm :collar-y :joint-angle -100)
(send *ri* :angle-vector (send *pr2* :angle-vector) 2000)
(send *ri* :wait-interpolation)
(send *irtviewer* :draw-objects)

(send *pr2* :larm :shoulder-p :joint-angle 74)
(send *pr2* :larm :elbow-p :joint-angle -50)
(send *pr2* :rarm :shoulder-p :joint-angle 74)
(send *pr2* :rarm :elbow-p :joint-angle -50)
(send *ri* :angle-vector (send *pr2* :angle-vector) 2000)
(send *ri* :wait-interpolation)
(send *irtviewer* :draw-objects)

; (send *pr2* :larm :shoulder-p :joint-angle 70)
(send *pr2* :larm :shoulder-r :joint-angle 90)
(send *pr2* :larm :collar-y :joint-angle 60)
(send *pr2* :larm :elbow-p :joint-angle -110)
(send *pr2* :larm :elbow-r :joint-angle 180)
(send *pr2* :larm :wrist-p :joint-angle -25)
(send *pr2* :larm :wrist-r :joint-angle -15)
; (send *pr2* :rarm :shoulder-p :joint-angle 70)
(send *pr2* :rarm :shoulder-r :joint-angle -90)
(send *pr2* :rarm :collar-y :joint-angle -60)
(send *pr2* :rarm :elbow-p :joint-angle -110)
(send *pr2* :rarm :elbow-r :joint-angle 180)
(send *pr2* :rarm :wrist-p :joint-angle -25)
(send *pr2* :rarm :wrist-r :joint-angle 15)
(send *pr2* :head :neck-p :joint-angle 50)
(send *ri* :stop-grasp :arms)
(send *ri* :angle-vector (send *pr2* :angle-vector) 2000)
(send *ri* :wait-interpolation)
(send *irtviewer* :draw-objects)

; (send *pr2*
; :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords)
; :translate (float-vector 0.0 0.5 0.0))
; :move-target (send *cube* :get :left-coords)
; :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent))
; :rotation-axis t)
; (send *pr2*
; :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords)
; :translate (float-vector 1.0 -0.5 0.0))
; :move-target (send *cube* :get :left-coords)
; :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent))
; :rotation-axis t)

; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
; (send *ri* :wait-interpolation)
; (send *irtviewer* :draw-objects)

; (send *pr2*
; :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords)
; :translate (float-vector 0.0 -5.0 5.0))
; :move-target (send *cube* :get :left-coords)
; :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent))
; :rotation-axis t)
; (send *pr2*
; :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords)
; :translate (float-vector 0.0 5.0 5.0))
; :move-target (send *cube* :get :left-coords)
; :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent))
; :rotation-axis t)

(send *ri* :angle-vector (send *pr2* :angle-vector) 2000)
(send *ri* :wait-interpolation)
(send *irtviewer* :draw-objects)

;; sleep 1秒
(unix:sleep 1)

(send *pr2* :larm :shoulder-p :joint-angle -20)
(send *pr2* :larm :shoulder-r :joint-angle 20)
(send *pr2* :larm :elbow-p :joint-angle -100)
(send *pr2* :rarm :shoulder-p :joint-angle -20)
(send *pr2* :rarm :shoulder-r :joint-angle -20)
(send *pr2* :rarm :elbow-p :joint-angle -100)
(send *pr2* :head :neck-p :joint-angle -10)
(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
(send *ri* :wait-interpolation)
(send *irtviewer* :draw-objects)
)

(defun pick-up ()
(send *pr2* :head :neck-p :joint-angle 60)
(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
(send *ri* :wait-interpolation)
(setq is-pickup-successful nil)

(while (not is-pickup-successful)
(send *target-box* :newcoords (make-coords))
;;サブスクライブ
; (ros::subscribe "/pointcloud_screenpoint/output_point" geometry_msgs::PointStamped #'callback)
(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))))

;;boxを適当な初期位置で配置
(setq *target-box* (make-cube 0 0 -10))
(objects (list *pr2* *target-box*))
;;ボールが見つかるまでループ
; (send *ri* :speak-jp "ボールをさがしています" :wait t)
(ros::rate 3) ;; 3Hz
(while (< (elt (send *target-box* :worldpos) 2) 400)
(ros::ros-info "waiting... ~A" (send *target-box* :worldpos))
(send *pr2* :angle-vector (send *ri* :state :potentio-vector))
(send *irtviewer* :draw-objects)
(x::window-main-one)
(ros::spin-once)
(ros::sleep)
)
; (send *ri* :speak-jp "ボールを見つけました" :wait t)
(ros::ros-info "start grasping... ~A" (send *target-box* :worldpos))
(send *pr2* :larm :shoulder-p :joint-angle 0)
(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
(send *ri* :wait-interpolation)

;;ボールの上空へ移動
(setq arm :larm)

;;サブスクライブ
; (ros::subscribe "/pointcloud_screenpoint/output_point" geometry_msgs::PointStamped #'callback)
(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 *pr2* arm :elbow-p :joint-angle -100)
(send *pr2* arm :collar-y :joint-angle 20)

;;ボールが見つかるまでループ
(send *ri* :speak-jp "ボールをさがしています" :wait t)
(ros::rate 3) ;; 3Hz
(while (< (elt (send *target-box* :worldpos) 2) 100)
(ros::ros-info "waiting... ~A" (send *target-box* :worldpos))
(send *pr2* :angle-vector (send *ri* :state :potentio-vector))
(send *irtviewer* :draw-objects)
(x::window-main-one)
(ros::spin-once)
(ros::sleep)
)
(send *ri* :speak-jp "ボールを見つけました" :wait t)
(ros::ros-info "start grasping... ~A" (send *target-box* :worldpos))
(send *pr2* :larm :shoulder-p :joint-angle 0)
(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
(send *ri* :wait-interpolation)
(send *ri* :angle-vector (send *pr2* :angle-vector) 1500)
(send *ri* :wait-interpolation)

(send *pr2* arm :inverse-kinematics
(send (send (send *target-box* :copy-worldcoords) :translate #f(30 0 150)) :rotate (deg2rad 90) :y)
:rotation-axis t
)

;;自分の台座とぶつからないようにアームを動かす
(setq arm :larm)
(send *pr2* arm :inverse-kinematics
(send (send (send *target-box* :copy-worldcoords) :translate #f(0 0 100)) :rotate (deg2rad 90) :y)
:rotation-axis t
)
(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 1500)
(send *ri* :wait-interpolation)

;;ボールへ移動
(send *pr2* arm :inverse-kinematics
(send (send (send *target-box* :copy-worldcoords) :translate #f(30 0 -20)) :rotate (deg2rad 90) :y)
:rotation-axis t
)

(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 2000)
(send *ri* :wait-interpolation)

(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 2000)
(send *ri* :wait-interpolation)

;;掴む
(send *ri* :start-grasp arm :wait t)
(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
(send *ri* :wait-interpolation)

(setq is-pickup-successful (> (send *ri* :robot arm :gripper :joint-angle) 5))
(when (not is-pickup-successful)
(ros::ros-info "pickup failed ~%")

; (send *ri* :speak-jp "ボールを掴めませんでした" :wait t)
(send *pr2* arm :inverse-kinematics
(send (send (send *target-box* :copy-worldcoords) :translate #f(30 0 100)) :rotate (deg2rad 90) :y)
:rotation-axis t
)
(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
(send *ri* :wait-interpolation)
(init-pose)
)
)

(send *pr2* arm :inverse-kinematics
(send (send (send *target-box* :copy-worldcoords) :translate #f(0 0 0)) :rotate (deg2rad 90) :y)
(send (send (send *target-box* :copy-worldcoords) :translate #f(30 0 100)) :rotate (deg2rad 90) :y)
:rotation-axis t
)

(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 2000)
(send *ri* :wait-interpolation)

;;掴む
(send *ri* :start-grasp arm :wait t)
(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
(send *ri* :wait-interpolation)

(ros::ros-info "pickup successful ~%")
; (send *ri* :speak-jp "ボールを持ち上げました" :wait t)

;;足にぶつからないような軌道で中央へ
(send *pr2* arm :inverse-kinematics
(send (send *target-box* :copy-worldcoords) :translate #f(-100 0 200)))
;;
(send *pr2* :larm :collar-y :joint-angle 50)
(send *pr2* :larm :shoulder-p :joint-angle 0)
(send *pr2* :larm :shoulder-r :joint-angle 90)
(send *pr2* :larm :elbow-p :joint-angle -30)
(send *pr2* :larm :elbow-r :joint-angle 90)
(send *pr2* :larm :wrist-p :joint-angle -10)
(send *pr2* :larm :wrist-r :joint-angle 0)

(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 2000)
(send *ri* :wait-interpolation)
)

;;離す
(send *ri* :stop-grasp :arms)
(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
(send *ri* :wait-interpolation)
)


; (init-pose)
; (unix:sleep 1)
; (chabudai-gaeshi)
;;メイン処理の実行
(ros::ros-info "init-pose")
(init-pose)
(unix:sleep 1)
(ros::ros-info "pick up")
(pick-up)
(unix:sleep 1)
(init-pose)
(ros::advertise "ball_count" std_msgs::Int32 1)
(ros::subscribe "/robot_mood" std_msgs::Int32 #'chabudai-cb)

(send *ri* :speak-jp "ボールを片付けます" :wait t)
(while (not *chabudai*)
(x::window-main-one)
(ros::ros-info "init-pose")
(init-pose)
(unix:sleep 1)
(ros::ros-info "pick up")
(pick-up)

(setq *ball-count* (+ *ball-count* 1))
(setq msg (instance std_msgs::Int32 :init))
(send msg :data *ball-count*)
(ros::ros-info "ball_count: ~A" *ball-count*)
(ros::publish "ball_count" msg)
(ros::spin-once)

(when *chabudai*
(chabudai-gaeshi)
)
)
Loading

0 comments on commit d782f0e

Please sign in to comment.