Class project: Telemanipulation of Robot Hand using Human Gesture

Class project: Telemanipulation of Robot Hand using Human Gesture

นาย ธนธัส อนันต์ศิริโรจน์ 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 โดยจะมีหลักการทำงานดังนี้

  1. สร้าง socket object ไว้สำหรับเรียกใช้งาน API ของ socket
  2. server ของเราก็จะสามารถ bind() ตัว IP Address และ port เพื่อให้สามารถเข้าถึงด้วยค่าที่กำหนดขึ้นผ่านทางเน็ตเวิร์คได้
  3. หลังจาก bind() เรียบร้อยแล้ว ฟังก์ชัน listen() จะทำงานโดยรอรับคำขอเชื่อมต่อจาก client 
  4. client จะใช้ conenct() API เพื่อสร้างการเชื่อมต่อไปยัง server
  5. เมื่อคำขอเชื่อต่อจาก client มาถึง server ฝั่ง server จะยอมรับการเชื่อมต่อนั้นผ่าน accept() API ซึ่งจะใช้งานได้ก็ต่อเมื่อทำการ bind() และ listen() สำเร็จแล้วเท่านั้น
  6. เมื่อสร้างการเชื่อมต่อระหว่าง client กับ server สำเร็จแล้ว ทั้งสองฝั่งจะสามารถส่งข้อมูลหากัน
  7. ถ้า 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 จากผู้ใช้งานเป็นคะแนน ดังนี้

หัวข้อการ testuser 1user 2user 3user 4user 5user 6
ความง่ายในการใช้งาน4.52.54444
ความรู้สึกถึง delay44.54.5334.5
ความยากง่ายของท่าทาง544.5334
เวลาในการหยิบและปล่อย1.00 m1.30 m3.30 m2.00 m2.00 m1.05
เกณฑ์การให้คะแนน 0 – 5 โดย เรียงลำดับจาก 0 คือ ไม่พอพึงพอใจ ถึง 5 คือ พึงพอใจมากที่สุด

รูปการเก็บผลการทดลอง

สรุปการทดสอบผู้ใช้งาน

จากการสังเกตุในการทดสอบของผู้ใช้งานพบว่า การขยับไปด้านหน้าและด้านหลังเมื่อเทียบกับการควบคุมด้วยนิ้วมือแล้วระบบทำให้ผู้ใช้งานค่อนข้างสับสนกับทิศในการควบคุม อีกทั้งเนื่องจากหุ่นแขกกลทำงานค่อนข้างดีเลย์ ทำให้การหยิบจับค่อนข้างเล็งหยิบค่อนข้างยาก ดังนั้นการแก้ไขอาจจะต้องบอกตำแหน่งของหุ่นที่กำลังจะเคลื่อนที่ไป ณ ปัจจุบัน และปรับการเคลื่อนที่การไปข้างหน้าและข้างหลังของหุ่นให้มีความสับสนน้อยลง

รูปผู้จัดทำ

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