Welcome to DOFBOT-Ultra repository

Finger Control Robot Arm

If using a depth camera (DABAI_DCW2), before running this example, be sure to execute ros2 run dofbot_ultra_driver calibrate_6point for a six-point calibration, otherwise the robot 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 / 3. Access the Terminal] section in [Close the web control program backend process started at boot]. After closing, please restart DofbotUltra!!!

Control the robot arm height through the opening angle of the thumb and index finger, while being able to switch screen effects.

1. Operation Instructions

1.1. Robot Arm Control

Control the robot arm posture through the angle between thumb and index finger. The current opening percentage is displayed on the right side of the screen:

1.2. Effect Switching

Press the f key to switch screen effects, while the finger opening angle controls the effect intensity:

1.3. Startup

• Enter the following command to start the program

xxxxxxxxxx
ros2 run dofbot_mediapipe 18_HandCtrlArm.py

Press the q key in the image to exit, press the f key to switch effects.

1.4. Source Code

Raspberry Pi source code location:

xxxxxxxxxx
~/dofbot_ultra_ws/src/dofbot_mediapipe/scripts/18_HandCtrlArm.py

RDK X5 source code location:

xxxxxxxxxx
~/dofbot_ultra_ws/src/2d/dofbot_mediapipe/scripts/18_HandCtrlArm.py

​x
#!/usr/bin/env python3
# encoding: utf-8
import math
import cv2 as cv
import numpy as np
import mediapipe as mp
from dofbot_utils.camera import create_camera
from dofbot_utils.fps import FPS
from Arm_Lib import Arm_Device
​
​
class HandDetector:
    def __init__(self, mode=False, maxHands=2, detectorCon=0.5, trackCon=0.5):
        self.tipIds = [4, 8, 12, 16, 20]
        self.mpHand = mp.solutions.hands
        self.mpDraw = mp.solutions.drawing_utils
        self.hands = self.mpHand.Hands(
            static_image_mode=mode, max_num_hands=maxHands,
            min_detection_confidence=detectorCon,
            min_tracking_confidence=trackCon)
        self.lmList = []
​
    def _get_dist(self, p1, p2):
        return math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2)
​
    def _calc_angle(self, pt1, pt2, pt3):
        p1 = (self.lmList[pt1][1], self.lmList[pt1][2])
        p2 = (self.lmList[pt2][1], self.lmList[pt2][2])
        p3 = (self.lmList[pt3][1], self.lmList[pt3][2])
        a, b, c = (self._get_dist(p1, p2), self._get_dist(p2, p3),
                    self._get_dist(p1, p3))
        try:
            return abs(math.acos((a**2 + b**2 - c**2) / (2*a*b))
                       / math.pi * 180)
        except (ValueError, ZeroDivisionError):
            return 0
​
    def findHands(self, frame, draw=True):
        img = np.zeros(frame.shape, np.uint8)
        img_RGB = cv.cvtColor(frame, cv.COLOR_BGR2RGB)
        self.results = self.hands.process(img_RGB)
        if self.results.multi_hand_landmarks:
            for handLms in self.results.multi_hand_landmarks:
                if draw:
                    self.mpDraw.draw_landmarks(
                        img, handLms, self.mpHand.HAND_CONNECTIONS)
        return img
​
    def findPosition(self, frame, draw=True):
        self.lmList = []
        if self.results.multi_hand_landmarks:
            for id, lm in enumerate(
                    self.results.multi_hand_landmarks[0].landmark):
                h, w, c = frame.shape
                cx, cy = int(lm.x * w), int(lm.y * h)
                self.lmList.append([id, cx, cy])
                if draw:
                    cv.circle(frame, (cx, cy), 15, (0, 0, 255), cv.FILLED)
        return self.lmList
​
    def frame_combine(self, frame, src):
        if len(frame.shape) == 3:
            fH, fW = frame.shape[:2]
            sH, sW = src.shape[:2]
            dst = np.zeros((max(fH, sH), fW + sW, 3), np.uint8)
            dst[:fH, :fW] = frame
            dst[:sH, fW:fW + sW] = src
        else:
            src = cv.cvtColor(src, cv.COLOR_BGR2GRAY)
            fH, fW = frame.shape[:2]
            sH, sW = src.shape[:2]
            dst = np.zeros((max(fH, sH), fW + sW), np.uint8)
            dst[:fH, :fW] = frame
            dst[:sH, fW:fW + sW] = src
        return dst
​
​
def main():
    detector = HandDetector(detectorCon=0.75)
    fps = FPS()
    arm = Arm_Device()
    arm.Arm_serial_servo_write6_array([90, 180, 0, 0, 90, 30], 1000)
​
    effects = ["color", "thresh", "blur", "hue", "enhance"]
    index = 0
    volBar = 400
    volPer = 0
    value = 0
    state = 0
​
    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
​
            action = cv.waitKey(1) & 0xFF
            img = detector.findHands(frame)
            lmList = detector.findPosition(frame, draw=False)
            if len(lmList) != 0:
                angle = detector._calc_angle(4, 0, 8)
                x1, y1 = lmList[4][1], lmList[4][2]
                x2, y2 = lmList[8][1], lmList[8][2]
                cx, cy = (x1 + x2) // 2, (y1 + y2) // 2
                cv.circle(img, (x1, y1), 15, (255, 0, 255), cv.FILLED)
                cv.circle(img, (x2, y2), 15, (255, 0, 255), cv.FILLED)
                cv.line(img, (x1, y1), (x2, y2), (255, 0, 255), 3)
                cv.circle(img, (cx, cy), 15, (255, 0, 255), cv.FILLED)
                if angle <= 10:
                    cv.circle(img, (cx, cy), 15, (0, 255, 0), cv.FILLED)
                volBar = np.interp(angle, [0, 70], [400, 150])
                volPer = np.interp(angle, [0, 70], [0, 100])
                value = np.interp(angle, [0, 70], [0, 255])
​
            if effects[index] == "thresh":
                gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
                frame = cv.threshold(gray, value, 255, cv.THRESH_BINARY)[1]
            elif effects[index] == "blur":
                frame = cv.GaussianBlur(
                    frame, (21, 21), np.interp(value, [0, 255], [0, 11]))
            elif effects[index] == "hue":
                frame = cv.cvtColor(frame, cv.COLOR_BGR2HSV)
                frame[:, :, 0] = (frame[:, :, 0].astype(int) +
                                  int(value)) % 180
                frame = cv.cvtColor(frame, cv.COLOR_HSV2BGR)
            elif effects[index] == "enhance":
                enh_val = max(value / 40, 1.0)
                clahe = cv.createCLAHE(clipLimit=enh_val,
                                       tileGridSize=(8, 8))
                lab = cv.cvtColor(frame, cv.COLOR_BGR2LAB)
                lab[:, :, 0] = clahe.apply(lab[:, :, 0])
                frame = cv.cvtColor(lab, cv.COLOR_LAB2BGR)
​
            if int(volPer) < 40 and state != 1:
                state = 1
                arm.Arm_serial_servo_write6_array(
                    [90, 180, 0, 0, 90, 30], 1000)
            if int(volPer) > 50 and state != 2:
                state = 2
                arm.Arm_serial_servo_write6_array(
                    [90, 127, 46, 9, 90, 30], 1000)
​
            if action == ord('q'):
                break
            if action == ord('f'):
                index = (index + 1) % len(effects)
​
            cv.rectangle(img, (50, 150), (85, 400), (255, 0, 0), 3)
            cv.rectangle(img, (50, int(volBar)), (85, 400),
                         (0, 255, 0), cv.FILLED)
            cv.putText(img, f'{int(volPer)}%', (40, 450),
                       cv.FONT_HERSHEY_COMPLEX, 1, (0, 255, 0), 3)
            fps.update_fps()
            fps.show_fps(frame)
            dst = detector.frame_combine(frame, img)
            cv.imshow('HandCtrlArm', dst)
    finally:
        cap.release()
        cv.destroyAllWindows()
​
​
if __name__ == '__main__':
    main()