Skip to content

Commit

Permalink
add Dec 18 semi achievements
Browse files Browse the repository at this point in the history
  • Loading branch information
Michi-Tsubaki committed Dec 19, 2024
1 parent 30b2ee0 commit 1af3b77
Show file tree
Hide file tree
Showing 2 changed files with 29 additions and 21 deletions.
8 changes: 4 additions & 4 deletions jsk_2024_10_semi/pr2_surgery/config/table_marker.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -6,11 +6,11 @@ boxes:
frame_id: base_footprint
name: table
orientation:
- 3.9531035934096856e-10
- 3.9531047036326084e-10
- 0.008311730048626616
- 4.293287988035861e-12
- 0.9999654569751891
position:
- 0.6999190449714661
- -5.3048133850097656e-05
- 0.843737006187439
- 0.7592881917953491
- 0.015209555625915527
- 0.849845826625824
42 changes: 25 additions & 17 deletions jsk_2024_10_semi/pr2_surgery/src/pr2_surgery.l
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ GOAL: Trying to make unique motion for stitching sponge!

;; Set remain
(setq *remain* 450) ;;how long remaining thread?
(setq *diff_remain* 45)
(setq *diff_remain* 55)

;; Offset
(setq *rarm_offset* 2)
Expand Down Expand Up @@ -141,7 +141,7 @@ GOAL: Trying to make unique motion for stitching sponge!
(defun cb-rforce(msg)
(ros::ros-info "checking right-virtual-force")
(setq *rforce* (send (send (send msg :wrench) :force) :z))
(if (> (send (send (send msg :wrench) :force) :z) 0.10)
(if (> (send (send (send msg :wrench) :force) :z) 0.15)
(setq *right-drawing* t)
)
)
Expand Down Expand Up @@ -311,7 +311,7 @@ GOAL: Trying to make unique motion for stitching sponge!
(detect-wound)

;; Stitch
(dotimes (i 6)
(dotimes (i 4)
;; Make trajectory points.
(setq r (instance traj :init))
(send r :rotate pi/2 :z)
Expand Down Expand Up @@ -370,7 +370,7 @@ GOAL: Trying to make unique motion for stitching sponge!
;; Draw the needle from the object. (ๅฏพ่ฑก็‰ฉใ‹ใ‚‰้‡ใ‚’ๆŠœใๅ‡บ)
(if (= *is_first* 1)
(progn (send *pr2* :rarm :move-end-pos (float-vector 0 0 (- 0 *remain*)) :local)
(send *ri* :angle-vector (send *pr2* :angle-vector) 6000)
(send *ri* :angle-vector (send *pr2* :angle-vector) 7000)
(send *ri* :wait-interpolation)
(send *irtviewer* :draw-objects)
(setq *remain* (- *remain* *diff_remain*))
Expand All @@ -380,36 +380,44 @@ GOAL: Trying to make unique motion for stitching sponge!
(setq remain_tmp (- *remain* *diff_remain*))
(send *pr2* :rarm :move-end-pos (float-vector 0 0 -60) :local)
(send *irtviewer* :draw-objects)
(send *ri* :angle-vector (send *pr2* :angle-vector) 600)
(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
(send *ri* :wait-interpolation)
(send *irtviewer* :draw-objects)
;;(setq *remain* (- *remain* 60));; ใ‚ชใƒชใ‚ธใƒŠใƒชใƒ†ใ‚ฃใƒใ‚คใƒณใƒˆ: 1ๅ›ž็ธซใ†ใจ้‡ใ‚’ๅผ•ใ‘ใ‚‹้‡ใŒๆธ›ใ‚‹ใฎใ‚’ๅ†็พ
(format t "remaining ~A ~%" *remain*) ;; for debug! (*remain*ใŒ0ใซใชใฃใŸใ‚‰breakใ™ใ‚‹ใ‚ˆใ†ใซใ—ใŸใ„)
(if (> *remain* remain_tmp)
(progn
(setq *right-drawing* nil)
(ros::subscribe "/right_endeffector/wrench" geometry_msgs::WrenchStamped #'cb-rforce)
(ros::subscribe "/right_endeffector/wrench" geometry_msgs::WrenchStamped #'cb-rforce 1)
(send *pr2* :rarm :move-end-pos (float-vector 0 0 (- remain_tmp)) :local)
(send *ri* :angle-vector (send *pr2* :angle-vector) 30000)
(send *irtviewer* :draw-objects)
(while (not *right-drawing*)
;;(send *ri* :angle-vector (print (send *pr2* :angle-vector)) 10000)

(send *ri* :angle-vector-sequence (list (send *pr2* :angle-vector)) (list 10000)
:default-controller 0.001
:min-time 0.0001
:minjerk-interpolation t
)
(send *irtviewer* :draw-objects)
(ros::duration-sleep 1.5)
(while (null *right-drawing*)
;; (setq *remain* (- *remain* 5));; ใ‚ชใƒชใ‚ธใƒŠใƒชใƒ†ใ‚ฃใƒใ‚คใƒณใƒˆ: 1ๅ›ž็ธซใ†ใจ้‡ใ‚’ๅผ•ใ‘ใ‚‹้‡ใŒๆธ›ใ‚‹ใฎใ‚’ๅ†็พ
;; (format t "remaining ~A ~%" *remain*) ;; for debug! (*remain*ใŒ0ใซใชใฃใŸใ‚‰breakใ™ใ‚‹ใ‚ˆใ†ใซใ—ใŸใ„)********************************************************************************************
(ros::duration-sleep 0.01)
;; (format t "remaining ~A ~%" *remain*) ;; for debug! (*remain*ใŒ0ใซใชใฃใŸใ‚‰breakใ™ใ‚‹ใ‚ˆใ†ใซใ—ใŸใ„)
(ros::duration-sleep 0.05)
(ros::spin-once)
(if (not (send *ri* :interpolatingp))
(setq *right-drawing* t))
)
(send *ri* :cancel-angle-vector :wait t)
;;(send *ri* :cancel-angle-vector :wait t)
(send *ri* :stop-motion)
(send *pr2* :angle-vector (send *ri* :state :potentio-vector))
;; (send *ri* :update-robot-state)
;; (setq diff (v- (send *pr2* :rarm :end-coords :worldpos) (send *ri* :robot :rarm :end-coords :worldpos)))
;; diff -> *remain*
)
)
)
)




;; Move left hand to the delivery point.
(setq *pass-larm* (send (send (send (send (send *center* :copy-worldcoords)
Expand Down Expand Up @@ -451,11 +459,11 @@ GOAL: Trying to make unique motion for stitching sponge!
(ros::rate 10)
(setq *left-drawing* nil)
;;ไฝ™ใฃใฆใ„ใ‚‹็ณธใ‚’ๅทฆใซๅผ•ใไฝœๆฅญใ‚’็Ÿฅ็š„ๅŒ–ใ—ใŸ
(ros::subscribe "/left_endeffector/wrench" geometry_msgs::WrenchStamped #'cb-lforce)
(ros::subscribe "/left_endeffector/wrench" geometry_msgs::WrenchStamped #'cb-lforce 1)
(send *pr2* :larm :move-end-pos (float-vector 0 0 -200) :local)
(send *ri* :angle-vector (send *pr2* :angle-vector) 10000)
(while (null *left-drawing*)
(ros::duration-sleep 0.1)
(ros::duration-sleep 0.02)
(ros::spin-once)
(if (not (send *ri* :interpolatingp))
(setq *left-drawing* t))
Expand All @@ -470,4 +478,4 @@ GOAL: Trying to make unique motion for stitching sponge!
(send *ri* :angle-vector (send *pr2* :angle-vector) 10000)
(send *ri* :wait-interpolation)
(send *irtviewer* :draw-objects)
(send *ri* :speak-jp "็ธซๅˆใŒใŠใ‚ใ‚Šใพใ—ใŸใ—ใพใ—ใŸ๏ผŽใŠ็–ฒใ‚Œๆง˜ใงใ—ใŸ๏ผŽ" :wait t)
(send *ri* :speak-jp "็ธซๅˆใŒใŠใ‚ใ‚Šใ—ใพใ—ใŸ๏ผŽใŠ็–ฒใ‚Œๆง˜ใงใ—ใŸ๏ผŽ" :wait t)

0 comments on commit 1af3b77

Please sign in to comment.