นาย ธนธัส อนันต์ศิริโรจน์ 63340500022 รับผิดชอบส่วนการ control Universal Robot (Co-op)
นาย ภรภัทร พัชรพิมพ์พิสุทธิ์ 63340500046 รับผิดชอบส่วนการ Input Hand-Tracking Gesture
นาย ธนัท บำรุงวงศ์สิริ 63340500026 รับผิดชอบส่วนการ Socket communication on LAN
การสร้าง Hand Tracking Module สำหรับการ Track Hand และ Gesture โดยใช้กล้อง
ผ่าน Library MediaPipe และ OpenCV
โดยการ Track มือโดยใช้ Library Mediapipe จะช่วยทำให้สามารถ Track มือได้โดยจะมีตำแหน่งที่เรียนกว่า Landmark ของมือทั้งหมด 21 จุดจากรูปโดยที่ตัว Library mediapipe จะ return ค่าตำแหน่งของ Landmark อยู่ในช่วงค่าระหว่าง 0 – 1 ดังนั้นในการเทียบค่าตำแหน่งกับภาพถ่ายที่ได้จากกล้องจำเป็นที่จะต้องสเกลหรือปรับขนาดให้ข่งตำแหน่งอยู่ในช่วงความละเอียดหรือ Resolution ของภาพด้วย
ตัวอย่าง Code การหาตำแหน่งของ Landmark ทั้ง 21 จุดโดยการเก็บค่าไว้ใน List
def positionFinder(self,image, handNo=0, draw=True):
lmlist = []
if self.results.multi_hand_landmarks:
Hand = self.results.multi_hand_landmarks[handNo]
for id, lm in enumerate(Hand.landmark):
h,w,c = image.shape
cx,cy,cz = int(lm.x*w), int(lm.y*h),lm.z
lmlist.append([id,cx,cy,cz])
if id == 8:
if draw:
cv2.circle(image,(cx,cy), 15 , (255,0,0), cv2.FILLED)
return lmlistโดยจากตำแหน่ง Landmark ที่เห็นจากในรูป จะถูกนำมาใช้การตรวจสอบลักษณะหรือท่าทาง Gesture ที่ทำโดยวัดจากการหุบนิ้วของมือ โดยที่การหุบนิ้วหรือไม่หุบนั้นจะวัดจากตำแหน่ง Landmark ของมือส่วนปลายเทียบกับ ตำแหน่ง Landmark ของโคนนิ้วแต่ละนิ้ว ตัวอย่างเช่น นิ้วชี้ ที่จะวัดตำแหน่งของ Landmark ส่วนของปลายนิ้วชี้ซึ่งก็คือ หมายเลขแปด เทียบกับตำแหน่งของโคนนิ้วชี้ซึ่งตรงกับ Landmark ตำแหน่งที่ 5 โดยเมื่อ ตำแหน่ง Landmark ของตำแหน่งที่ 8 มีตำแหน่งในแนวแกน y ที่น้อยกว่า Landmark ตำแหน่งที่ 5 เราจะสามารถบอกได้ว่า ณ ขณะนั้นผู้ใช้งานกำลังหุบนิ้วชี้อยู่ ซึ่งก็จะใช้หลักการดังกล่าวนี้ในการเทียบกับทุก ๆ นิ้วเพื่อตรวจสอบการลักษณะของมือในขณะนั้น ๆ
โดยเมื่อเราสามารถบอกการหุบของนิ้วมือแต่ละนิ้ว ณ ขณะนั้น ๆ ได้แล้ว เมื่อเราลองทำ Gesture หรือสัญลักษณ์มือต่าง ๆ เราจะสังเกตุได้ว่า สัญลักษณ์มือบางอย่างสามารถตรวจสอบได้ด้วยนิ้วที่กำลังหุบอยู่ ยกตัวอย่างเช่น การชูสองนิ้ว ที่ถ้าหากโปรแกรมสามารถบอกได้ว่า นิ้วที่เรากำลังชูอยู่คือ นิ้วชี้ และนิ้วกลาง ในขณะ ที่นิ้วอื่น ๆ กำลังหุบอยู่เราก็จะสามารถบอกได้ว่า ณ ขณะนี้ผู้ใช้งานกำลังทำสัญลักษณธชูสองนิ้วอยู่เป็นต้น
ตัวอย่าง Code ของ Program
การตรวจสอบ Hand Status จาก การ Tracking ของ Hand *lmlist คือ List ตำแหน่งของ Landmark ทั้ง 21 จุด
def HandStatus(self,lmlist):
Status = [False,False,False,False,False]
Thumb_stat = lmlist[4][1] - lmlist[10][1]
Index_stat = lmlist[8][2] - lmlist[5][2]
Middle_stat = lmlist[12][2] -lmlist[9][2]
Ring_stat = lmlist[16][2] - lmlist[13][2]
Pinky_stat = lmlist[20][2] - lmlist[17][2]
Finger_distance = [Thumb_stat,Index_stat,Middle_stat,Ring_stat,Pinky_stat]
for i in range(len(Status)):
if Finger_distance[i]<0:
Status[i]=True
else: Status[i]=False
if Thumb_stat <=25 and Thumb_stat >=-25:
Status[0] = False
else:
Status[0] = True
return Statusการตรวจสอบ Hand Gesture จาก Hand Status สำหรับการใช้เป็น Command ของ Universal Robot
def HandGestureCheck(self,Status):
Start = [False,True,False,False,False]
Stop = [True,True,True,True,True]
Home = [False,False,False,False,False]
Gripper = [True,True,False,False,True]
if Status == Start or Status == [True,True,False,False,False]:
return "Start"
elif Status == Home:
return "Home"
elif Status == Stop:
return "Stop"
elif Status == Gripper:
return "Gripper"
else:
return Noneการทำ Socket communication โดย python สำหรับการควบคุมตัวหุ่นยนต์ผ่านระบบ LAN
การทำตัว Socket communication จะทำการส่งข้อมูลของตัวการเคลื่อนไหวของมือส่งไปควบคุมที่ตัว UR โดยจะแบ่งการทำงานออกเป็น 2 ส่วน คือ ตัว Client และตัว Server ซี่งที่ตัว Client ทำหน้าที่ในการรับข้อมูลการเคลื่อนไหว และท่าทางของมือ และส่งไปที่ตัวของ Server ซี่งตัวของ Server จะทำการประมวลผลข้อมูลของท่าทางที่ได้รับมา และ นำไปขยับตัว UR โดยจะมีหลักการทำงานดังนี้
- สร้าง socket object ไว้สำหรับเรียกใช้งาน API ของ socket
- server ของเราก็จะสามารถ bind() ตัว IP Address และ port เพื่อให้สามารถเข้าถึงด้วยค่าที่กำหนดขึ้นผ่านทางเน็ตเวิร์คได้
- หลังจาก bind() เรียบร้อยแล้ว ฟังก์ชัน listen() จะทำงานโดยรอรับคำขอเชื่อมต่อจาก client
- client จะใช้ conenct() API เพื่อสร้างการเชื่อมต่อไปยัง server
- เมื่อคำขอเชื่อต่อจาก client มาถึง server ฝั่ง server จะยอมรับการเชื่อมต่อนั้นผ่าน accept() API ซึ่งจะใช้งานได้ก็ต่อเมื่อทำการ bind() และ listen() สำเร็จแล้วเท่านั้น
- เมื่อสร้างการเชื่อมต่อระหว่าง client กับ server สำเร็จแล้ว ทั้งสองฝั่งจะสามารถส่งข้อมูลหากัน
- ถ้า client กับ server ส่งข้อมูลกันเรียบร้อยแล้วหรือต้องการจะหยุดการเชื่อมต่อ ก็จะเรียกใช้ close() API เพื่อตัดการเชื่อมต่อและคืนทรัพยากรที่ถูกใช้โดย socket ให้กับระบบ
โดยจัดทำเป็น Python Class ไว้ใช้เพื่อให้ใช้การงานสะดวกมากยิ่งขึ้น
ทำการสร้างตัว Constructor ไว้สำหรับการรับค่า IP และ PORT และทำการสร้าง Socket Object ไว้
class Socket:
def __init__(self,IP="localhost",PORT=1119) -> None:
self.HOST_IP = IP
self.PORT = PORT
self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)สร้าง Function สำหรับการเรียกใช้งานตัว Server โดยเขียนให้มีการทำการ Pass Function บางอย่างเข้ามาเพื่อที่จะได้ใช้งานระหว่างการ Run Server
def echo_server(self,function=print): #pass function
self.s.bind((self.HOST_IP,self.PORT))
self.s.listen()
print(f"Watting for connect")
conn, addr = self.s.accept()
if conn:
print(f"Connected by {addr}")
## starttime = time.time() for timer
while True:
rev = conn.recv(1024)
if rev:
data=pickle.loads(rev)
## Funtion Something
function(data)
# conn.sendall(b"PBestkodlnw")
else :
#for didnt get any data from client
connect = self.is_still_connected(self.s)
if not connect :
break;สร้าง function สำหรับตัว Client ที่ใช้ในการ Conncet และส่งข้อมูลหาตัว Sever โดย ซี่งปกติลำดับการทำงานปกติของตัว Client จะทำการรอรับ data ที่ส่งมาจาก Server ก่อนค่อยทำการส่งข้อมูลต่อไป แต่ในที่นี้จะทำการส่งข้อมูลต่อโดยที่จะไม่รอรับ data ของตัว Server ที่ส่งมา เพื่อให้สามารถทำงานได้ไวมากขึ้น
##### FOR CLIENT ########
def client_connect(self):
self.s.connect((self.HOST_IP, self.PORT))
def client_send_data(self,array):
print("sendded")
self.send_array_data(self.s,array)
# self.s.recv(1024)การนำตัว Socket ไปใช้กับตัว URClient
ใช้เป็น URClient สำหรับการรับข้อมูลจาก Userที่ออกท่าทาง และส่งไปที่ Server
ทำการ Import module ต่างๆที่ใช้
import HandTrackingModule as htm
import cv2
from SocketSever import Socketทำการสร้าง Object และ Setup IP Address ที่จะใช้งานโดยในที่นี้ใช้เป็น LAN IPv4 Address
HandDetector = htm.handTracker(detectionCon=0.7)
Client = Socket(IP="10.61.5.232")
# Client = Socket()
Client.client_connect()
draw = True
cap = cv2.VideoCapture(0,cv2.CAP_DSHOW)ทำการ run การใช้งานกล้องในการ Track การเคลื่อนไหวของมือ และ ส่งค่าไปที่ server ไปพร้อมกัน
HandDetector = htm.handTracker(detectionCon=0.7)
Client = Socket(IP="10.61.5.232")
# Client = Socket()
Client.client_connect()
draw = True
cap = cv2.VideoCapture(0,cv2.CAP_DSHOW)ทำการ run การใช้งานกล้องในการ Track การเคลื่อนไหวของมือ และ ส่งค่าไปที่ server ไปพร้อมกัน
if __name__ == "__main__":
while 1 :
success ,img = cap.read()
HandDetector.handsFinder(img,draw=draw)
lmlist = HandDetector.positionFinder(img,draw=draw)
if len(lmlist)>=21:
HandStatus = HandDetector.HandStatus(lmlist)
Command = HandDetector.CommandforUR(HandStatus)
arr = [Command,lmlist[8][2],lmlist[8][1],lmlist[4][2],lmlist[4][1]]
Client.client_send_data(arr)
cv2.imshow("Img",img)
cv2.waitKey(1)การนำตัว Socket ไปใช้กับตัว URserver
ใช้เป็น URSerevr สำหรับการรับข้อมูลจาก Client และทำการประมวลผลคำสั่งที่ได้รับมาและนำไปควบคุมตัว UR
ทำการ Setup ตัว Socket และ ทำการใส่ IP ให้เรียบร้อย
import URclass as UR
import math
from SocketSever import Socket
###### Setup ###########
Server = Socket() ## Argument IP="...." for setup ip
UR_Rb = UR.URRobot()
UR_Rb.sethome()
#### Parameter Setting ####
x2,y2 = 0,0
x2_1,y2_1 = 0,0สร้าง function การทำงานรับคำสั่งต่างๆจากตัว Client และนำไปควบคุมตัว UR
def Command_Selection(data):
Command,lm8_2,lm8_1,lm4_2,lm4_1 = data
if Command=="Start":
x = -(((lm8_2/6400)*3)+0.2)
y = -(((lm8_1/4800)*4)-0.2)
UR_Rb.moveUR(x,y)
# print("UR_Rb.moveUR(x,y)")
elif Command=="Gripper":
x1,y1 = lm4_1 , lm4_2
x2,y2 = lm8_1 , lm8_2
length = math.dist([x1,y1],[x2,y2])
# print("UR_Rb.GripperControl(length)")
UR_Rb.GripperControl(length)
elif Command=="Stop":
pass
elif Command=="Home":
# print("UR_Rb.sethome()")
UR_Rb.sethome()
else:
passทำการ Run Server โดย Pass function Commnad_selection
###### Run Server ###########
if __name__ == "__main__":
Server.echo_server(function=Command_Selection)การ Control Universal robot โดยการรับค่า input X Y ที่ได้จาก Socket communication ทำให้หุ่นสามารถเคลื่อนที่ในแนวระนาบ X Y
การ set up การทำงานของ Library ur_rtde เพื่อให้สามารถสั่งการ Universal robot ได้มีการ set up IP, rtde_control, rtde_receive, rtde_io, และ AsyncOperationProgress เพื่อเช็ค state ของหุ่นว่าทำงานอยู่หรือไม่
def __init__(self):
self.ip = "192.168.20.35"
self.rtde_c = rtde_control.RTDEControlInterface(self.ip)
self.rtde_r = rtde_receive.RTDEReceiveInterface(self.ip)
self.rtde_io_ = rtde_io.RTDEIOInterface(self.ip)
self.state_stop = self.rtde_c.getAsyncOperationProgress()
self.msg = ""โดยการ Control ได้ใช้ Library ur_rtde ที่จะช่วยในการ control หุ่นไปยังตำแหน่งที่กำหนดได้โดยจะมีการ control ได้ หลายรูปแบบซึ่งคือการเคลื่อนที่แบบที่ forward kinematic การใส่ joint configuration ไปให้หุ่นทั้ง 6 Joint เป็น radius โดยจะใช้ในการ setup workspace และ ทำ sethome ของหุ่น
def sethome(self):
qbase = np.pi*0
qshouder = -np.pi/3
qelbow = np.pi/3
qwrist1 = np.pi*0
qwrist2 = np.pi/2
qwrist3 = np.pi/3
self.rtde_c.moveJ([qbase, qshouder, qelbow, qwrist1, qwrist2, qwrist3], 0.5, 0.3)และการ control หุ่นโดยใช้ inverse kinematic จะเป็นการ control ให้หุ่นสามารถเคลื่อนที่ไปยังตำแหน่งที่กำหนดโดย กำหนด position และ orientation ของ TCP ของ gripper ไปให้หุ่นและเพื่อให้หุ่นและเพื่อที่จะให้หุ่นสามารถทำงานได้จึงได้กำหนด work space การทำงานของหุ่นไว้โดยพื้นที่การทำงานเป็นพื้นที่สี่เหลี่ยมผืนผ้าที่มีความกว้าง 0.3 เมตร และความยาว 0.4 เมตร โดยระยะจะถูกอ้างอิงจากตำแหน่ง sethome และ map resolution กับขนาดของ frame ของกล้อง
def moveUR(self,x,y):
if x > -0.2 :
x = -0.2
elif x <-0.5:
x = -0.5
if y > 0.2:
y = 0.2
elif y <-0.2:
y = -0.2
if self.state_stop < 0:
self.msg = "robot work"
print(self.msg)
self.rtde_c.moveJ_IK([x, y, 0.2771877876472226, 0.4189044692262709, -1.52750664380437, -0.4114795078640724], 0.5, 0.3)การ map resolution
x = -(((camera_frame_vertical/6400)*3)+0.2)
y = -(((camera_frame_horizontal/4800)*4)-0.2)การ control gripper โดยใช้ IO ของ Library ur_rtde โดย gripper จะมีการทำงาน 2 state คือ open และ close โดยจะเชคระยะจาก input ที่เข้ามาว่่าเป็นระยะระหว่างนิ้วชี้และนิ้วโป้งว่าถ้าระยะน้อยกว่า ค่าหนึ่งให้ปิด gripper
def GripperControl(self,length):
if length <= 50: # close
self.rtde_io_.setStandardDigitalOut(0, True)
self.rtde_io_.setStandardDigitalOut(1, False)
else: # open
self.rtde_io_.setStandardDigitalOut(0, False)
self.rtde_io_.setStandardDigitalOut(1, True)System performance
- Frame Rate 30 FPS
- Resolution 640 x 480
- workspace 30 cm x 40 cm
Propose
- เพื่อศึกษาการควบคุมหุ่นยนต์ UR(Universal Robot) ด้วยท่าทาง
- เพื่อเสริมสร้างการมีปฏิสัมพันธ์ของมนุษย์ และ หุ่นยนต์
- เพื่อใช้ในการพัฒนาการควมคุมหุ่นยนต์ในระยะไกลโดยที่จะสามารถให้หุ่นอยู่ในห้องปฎิบัติการและผู้ควบคุมสามารถอยู่จุดที่ WIFI เข้าถึง
User test
ทำการทดสอบการใช้งานโดยการบังคับหุ่นให้หยิบสิ่งของจากมือของคนที่กำลังถือและนำไปปล่อยนำบริเวณที่ถูกำหนดโดยผู้พัฒนาอยู่โดยได้ผลลัพธ์ Feedback จากผู้ใช้งานเป็นคะแนน ดังนี้
| หัวข้อการ test | user 1 | user 2 | user 3 | user 4 | user 5 | user 6 |
| ความง่ายในการใช้งาน | 4.5 | 2.5 | 4 | 4 | 4 | 4 |
| ความรู้สึกถึง delay | 4 | 4.5 | 4.5 | 3 | 3 | 4.5 |
| ความยากง่ายของท่าทาง | 5 | 4 | 4.5 | 3 | 3 | 4 |
| เวลาในการหยิบและปล่อย | 1.00 m | 1.30 m | 3.30 m | 2.00 m | 2.00 m | 1.05 |
รูปการเก็บผลการทดลอง
สรุปการทดสอบผู้ใช้งาน
จากการสังเกตุในการทดสอบของผู้ใช้งานพบว่า การขยับไปด้านหน้าและด้านหลังเมื่อเทียบกับการควบคุมด้วยนิ้วมือแล้วระบบทำให้ผู้ใช้งานค่อนข้างสับสนกับทิศในการควบคุม อีกทั้งเนื่องจากหุ่นแขกกลทำงานค่อนข้างดีเลย์ ทำให้การหยิบจับค่อนข้างเล็งหยิบค่อนข้างยาก ดังนั้นการแก้ไขอาจจะต้องบอกตำแหน่งของหุ่นที่กำลังจะเคลื่อนที่ไป ณ ปัจจุบัน และปรับการเคลื่อนที่การไปข้างหน้าและข้างหลังของหุ่นให้มีความสับสนน้อยลง
รูปผู้จัดทำ
GitHub
https://github.com/tanutb/URTele_HumanGesture
อ้างอิง
https://www.ibm.com/docs/en/i/7.1?topic=programming-how-sockets-work
https://google.github.io/mediapipe/solutions/pose.html
https://sdurobotics.gitlab.io/ur_rtde/installation/installation.html