Welcome to DOFBOT-Ultra repository

Gesture Grasp and Release Objects

If using a depth camera (DABAI_DCW2), make sure to execute ros2 run dofbot_ultra_driver calibrate_6point for a six-point calibration before running this example. Otherwise, the arm cannot accurately locate the target.

Please ensure that the web control program backend process is not running in the background. To close it, refer to the [Get Started Quickly - 2. Access the Terminal] section in [Disable the Auto-Start Web Control Backend Process]. After closing, please restart DofbotUltra!!!

Control the arm to grasp and place objects through gestures. The same gesture must be stably recognized for 5 consecutive frames to trigger.

For all functions involving grasping of square blocks, please use 30x30x30 mm wooden blocks. Non-standard sized objects may cause servo No. 6 stall and damage.

1、Gesture and Action Description

Workflow: First place an object in front of the arm → Make gesture Five to grasp → After grasping, make gesture One to place.

2、Startup

• Open the desktop terminal and enter the following command to start the program

xxxxxxxxxx
ros2 run dofbot_mediapipe 17_GestureGrasp.py

Place your hand in the camera view and make gesture Five (five fingers spread). The arm will go to the front to grasp the object.

After grasping the object, make gesture One (extend index finger). The arm will place the object in the upper left position.

Press the q key in the image window or Ctrl+C in the terminal to exit the program.

1.1、Source Code

Raspberry Pi code path:

xxxxxxxxxx
~/dofbot_ultra_ws/src/dofbot_mediapipe/scripts/17_GestureGrasp.py

RDK X5 code path:

xxxxxxxxxx
~/dofbot_ultra_ws/src/2d/dofbot_mediapipe/scripts/17_GestureGrasp.py

#!/usr/bin/env python3
# encoding: utf-8
import cv2 as cv
import time
import threading
from dofbot_utils.camera import create_camera
from dofbot_utils.fps import FPS
from dofbot_utils.robot_controller import Robot_Controller
from dofbot_utils.GestureRecognition import handDetector
from Arm_Lib import Arm_Device


class GestureGrasp:
    def __init__(self):
        self.hand_detector = handDetector(detectorCon=0.75)
        self.fps = FPS()
        self.arm = Arm_Device()
        self.robot = Robot_Controller()
        self.move_state = False

        self.Count_One = 0
        self.Count_Five = 0

        self.arm.Arm_serial_servo_write6_array(self.robot.P_LOOK_AT, 1000)

    def process(self, frame):
        frame, lmList = self.hand_detector.findHands(frame, draw=False)
        if len(lmList) != 0:
            gesture = self.hand_detector.get_gesture()
            if gesture == 'One':
                cv.putText(frame, gesture, (250, 30),
                           cv.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 1)
                self.Count_One += 1
                self.Count_Five = 0
                if self.Count_One >= 5 and not self.move_state:
                    self.move_state = True
                    self.Count_One = 0
                    threading.Thread(
                        target=self._arm_ctrl, args=('One',),
                        daemon=True).start()
            elif gesture == 'Five':
                cv.putText(frame, gesture, (250, 30),
                           cv.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 1)
                self.Count_Five += 1
                self.Count_One = 0
                if self.Count_Five >= 5 and not self.move_state:
                    self.move_state = True
                    self.Count_Five = 0
                    threading.Thread(
                        target=self._arm_ctrl, args=('Five',),
                        daemon=True).start()
        self.fps.update_fps()
        self.fps.show_fps(frame)
        return frame

    def _arm_ctrl(self, gesture):
        if gesture == 'Five':
            joints = [90, 35, 65, 0, 90, 30]
            self.arm.Arm_serial_servo_write(6, 30, 500)
            time.sleep(0.5)
            self.arm.Arm_serial_servo_write6_array(joints, 1000)
            time.sleep(1.2)
            self.arm.Arm_serial_servo_write(6, 135, 500)
            time.sleep(0.6)
            self.arm.Arm_serial_servo_write6_array(
                [90, 164, 18, 0, 90, 135], 1000)
            time.sleep(1.2)
        elif gesture == 'One':
            self.arm.Arm_serial_servo_write6_array(
                [163, 111, 0, 53, 90, 135], 1000)
            time.sleep(1.5)
            self.arm.Arm_serial_servo_write(6, 30, 500)
            time.sleep(0.8)
            self.arm.Arm_serial_servo_write6_array(
                [90, 164, 18, 0, 90, 135], 1000)
            time.sleep(1.2)
        self.move_state = False


def main():
    grasp = GestureGrasp()

    cap = create_camera()
    if not cap.isOpened():
        raise RuntimeError("Failed to open camera")

    try:
        while cap.isOpened():
            ret, frame = cap.read()
            if not ret:
                continue
            frame = grasp.process(frame)
            cv.imshow('GestureGrasp', frame)
            if cv.waitKey(1) & 0xFF == ord('q'):
                break
    finally:
        cap.release()
        cv.destroyAllWindows()


if __name__ == '__main__':
    main()