[2024年度ゼミ] Tsubaki's work

jsk_2024_10_semi/README.md

@@ -1,3 +1,7 @@
 # jsk_2024_10_semi
 
+## 目標
+手術ロボット。縫合タスク。
 
+## サブ目標
+スポンジを縫う

jsk_2024_10_semi/detect_test/niku_detect_1.py

@@ -0,0 +1,73 @@
+#!/usr/bin/env python
+
+##切れ目のsegmentation
+
+##参考: https://qiita.com/ysdyt/items/5972c9520acf6a094d90
+## https://pystyle.info/opencv-distance-transform/#google_vignette
+
+
+#import common pkg in Python
+import cv2 as cv
+import numpy as np
+import matplotlib.pyplot as plt
+
+#import pkg for connecting to ROS
+import rospy
+from sensor_msgs.msg import Image, CompressedImage, CameraInfo
+
+
+#define call back function
+def callback(msg):
+    global niku
+    niku = msg[0]
+
+
+
+rospy.init_node('client')
+rospy.Subscriber("/kinect_head/rgb/image_raw/compressed", CompressedImage, callback)
+
+niku = cv.imread("./niku.jpg")
+niku = niku[1000: 3200, 500 : 2700]
+
+
+niku_gray = cv.cvtColor(niku, cv.COLOR_BGR2GRAY)
+
+
+
+plt.imshow(niku_gray,cmap='gray')
+plt.show()
+
+thresh,bin_img = cv.threshold(niku_gray,0,255,cv.THRESH_BINARY_INV+cv.THRESH_OTSU)
+plt.imshow(bin_img,cmap='gray')
+plt.show()
+print('大津法の二値化によって自動で決定された閾値:',thresh)
+
+## print(bin_img) #for debug
+
+kernel = np.ones((3,3),np.uint8)
+opening = cv.morphologyEx(bin_img,cv.MORPH_OPEN,kernel,iterations = 2)
+plt.imshow(opening,cmap='gray')
+plt.show()
+
+#モルフォロジー演算のDilationを使う
+sure_bg = cv.dilate(opening,kernel,iterations=2)
+plt.imshow(sure_bg,cmap='gray')
+plt.show()
+
+dist_transform = cv.distanceTransform(opening,cv.DIST_L2,5)
+plt.imshow(dist_transform)
+plt.show()
+
+ret, sure_fg = cv.threshold(dist_transform,0.05*dist_transform.max(),255,0)
+print('閾値（距離変換で得られた値の最大値×0.5）:',ret)
+plt.imshow(sure_fg,cmap='gray')
+plt.show()
+
+sure_fg = np.uint8(sure_fg)
+unknown = cv.subtract(sure_bg,sure_fg)
+plt.imshow(unknown,cmap='gray')
+
+# foregroundの1オブジェクトごとにラベル（番号）を振っていく
+ret, markers = cv.connectedComponents(sure_fg)
+plt.imshow(markers)
+plt.show()

jsk_2024_10_semi/first_motion.l

@@ -0,0 +1,248 @@
+;;Okada-sensei seminar practice task ;;last editted oct 17 ;; Michitoshi TSUBAKI
+;;building specific motions using euslisp command
+
+(require "package://pr2eus/pr2.l")  ;;import pr2 package
+(require "package://pr2eus/pr2-utils.l")
+(require "package://pr2eus/pr2-interface.l") 
+(if (not (boundp '*pr2*)) (pr2-init)) ;; (setq *pr2* (pr2)))  ;; (pr2) is alternated by  *pr2*
+;;boudp 'xx : xxという変数に値が代入されているか
+;;setq xx yy : xxとう変数にyyという値を代入する
+
+(send *pr2* :reset-pose) ;; 初期姿勢
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :stop-grasp :arms)
+(send *ri* :wait-interpolation)
+
+
+(objects (list *pr2*))
+
+;;徐々に腕（肩）を広げる(Oct 17)
+(send *pr2* :larm :shoulder-p :joint-angle 74)
+(send *pr2* :larm :shoulder-p :joint-angle 74)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+(send *pr2* :larm :shoulder-p :joint-angle 54)
+(send *pr2* :rarm :shoulder-p :joint-angle 54)
+
+(send *pr2* :larm :gripper :joint-angle 30)
+(send *pr2* :rarm :gripper :joint-angle 30)
+
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :start-grasp :arms)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 34)
+(send *pr2* :rarm :shoulder-p :joint-angle 34)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 20)
+(send *pr2* :rarm :shoulder-p :joint-angle 20)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle 0)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+;;徐々に型を下げる(Oct 22)
+(send *pr2* :rarm :shoulder-r :joint-angle -80)
+(send *pr2* :larm :shoulder-r :joint-angle 80)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :rarm :shoulder-r :joint-angle -95)
+(send *pr2* :larm :shoulder-r :joint-angle 95)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :rarm :shoulder-r :joint-angle -110)
+(send *pr2* :larm :shoulder-r :joint-angle 100)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+
+(send *pr2* :rarm :shoulder-r :joint-angle -130)
+(send *pr2* :larm :shoulder-r :joint-angle 120)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+
+
+(send *pr2* :larm :elbow-p :joint-angle -110)
+(send *pr2* :rarm :elbow-p :joint-angle -100)
+(send *pr2* :rarm :shoulder-r :joint-angle -130)
+(send *pr2* :larm :shoulder-r :joint-angle 120)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *irtviewer* :draw-objects)
+(unix:sleep 1)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(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 *pr2* :larm :shoulder-p :joint-angle 10)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *pr2* :rarm :shoulder-r :joint-angle -100)
+(send *pr2* :larm :shoulder-r :joint-angle 100)
+(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 *pr2* :larm :shoulder-p :joint-angle 10)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *pr2* :rarm :shoulder-r :joint-angle -80)
+(send *pr2* :larm :shoulder-r :joint-angle 80)
+(send *pr2* :larm :elbow-p :joint-angle -110)
+(send *pr2* :rarm :elbow-p :joint-angle -100)
+(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 *pr2* :larm :shoulder-p :joint-angle 10)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *pr2* :rarm :shoulder-r :joint-angle -100)
+(send *pr2* :larm :shoulder-r :joint-angle 100)
+(send *pr2* :larm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-p :joint-angle -120)
+(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 *pr2* :larm :shoulder-p :joint-angle 10)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+;;fixing now last edited Oct 22 by Michitoshi TSUBAKI
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *pr2* :rarm :shoulder-r :joint-angle -100)
+(send *pr2* :larm :shoulder-r :joint-angle 100)
+(send *pr2* :larm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-r :joint-angle 0)
+(send *pr2* :rarm :wrist-p :joint-angle -40)
+(send *pr2* :larm :wrist-r :joint-angle 0)
+(send *pr2* :rarm :wrist-r :joint-angle 0)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *pr2* :rarm :shoulder-r :joint-angle -120)
+(send *pr2* :larm :shoulder-r :joint-angle 120)
+(send *pr2* :larm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-r :joint-angle 0)
+(send *pr2* :rarm :wrist-p :joint-angle -40)
+(send *pr2* :larm :wrist-r :joint-angle 90)
+(send *pr2* :rarm :wrist-r :joint-angle 90)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :torso :waist-z :joint-angle 200)
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :rarm :shoulder-p :joint-angle -20)
+(send *pr2* :rarm :shoulder-r :joint-angle -120)
+(send *pr2* :larm :shoulder-r :joint-angle 120)
+(send *pr2* :larm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-p :joint-angle -120)
+(send *pr2* :rarm :elbow-r :joint-angle 0)
+(send *pr2* :rarm :wrist-p :joint-angle -40)
+(send *pr2* :larm :wrist-r :joint-angle 0)
+(send *pr2* :rarm :wrist-r :joint-angle 0)
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *pr2* :torso :waist-z :joint-angle 300)
+
+(send *irtviewer* :draw-objects)
+;;(unix:sleep 1)
+(send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+(send *ri* :wait-interpolation)
+
+
+

jsk_2024_10_semi/first_task/demo_first.l

@@ -0,0 +1,5 @@
+(require "package://pr2eus/pr2.l")
+(if (not (boundp '*pr2*)) (setq *pr2* (pr2)))
+(objects (list *pr2*))
+(send *pr2* :larm :shoulder-p :joint-angle 10)
+(send *irtviewer* :draw-objects)

jsk_2024_10_semi/first_task/homework_uml_1.act

@@ -0,0 +1,42 @@
+@startuml
+|医者役|
+   start
+  :傷口を早く閉じて，休憩しないと死にそうだ〜:
+
+  :針を通して，ロボットに渡す;
+
+|手術ロボット|
+
+  :針を受け取る;
+
+|医者役|
+
+  :「自動縫合モード」スタートさせる;
+
+|手術ロボット|
+
+    :傷口を認識する（画像認識 segmantation? detection?）;
+
+  repeat 
+     :アームを目標（傷口）に近づける;
+     :1針縫う;
+
+
+  repeat while (傷口が塞がれているか確認する（画像認識）)
+    :傷口が塞がれているなら，プログラムを停止する;
+
+
+
+
+|医者役|
+   :休憩から帰ってきてちゃんと縫えているか確認する;
+
+|手術ロボット|
+   :お医者さんに褒められながらCtl+Cを押される（嬉しい？）;
+
+|医者役|
+   :糸を結ぶ;
+   :手術終了;
+
+end
+@enduml