Welcome to DOFBOT-Ultra repository

Robotic Arm Tracks The Palm

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!!!

1、Startup

1.1、Preparation Before Starting the Program

Note: When this program is running, the arm has a relatively large range of motion. Ensure there are no other objects around the arm to avoid being hit by it.

1.2、Program Description

After the program starts, once the camera captures the image, the arm will follow the movement of the palm in the frame. Do not move the palm too fast, otherwise the arm may not keep up.

1.3、Program Startup

• Enter the following command to start the program

xxxxxxxxxx
ros2 run dofbot_mediapipe 14_HandFollow.py

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

1.4、Source Code

Raspberry Pi code path:

xxxxxxxxxx
~/dofbot_ultra_ws/src/dofbot_mediapipe/scripts/14_HandFollow.py

RDK X5 code path:

xxxxxxxxxx
~/dofbot_ultra_ws/src/2d/dofbot_mediapipe/scripts/14_HandFollow.py

​x
#!/usr/bin/env python3
# encoding: utf-8
import threading
import numpy as np
import cv2 as cv
from time import sleep
from dofbot_utils.robot_controller import Robot_Controller
from dofbot_utils.camera import create_camera
from dofbot_utils.fps import FPS
from dofbot_utils import pid as PID
from media_library import HandDetector


class HandFollow:
    def __init__(self):
        self.robot = Robot_Controller()
        self.robot.move_init_pose()
        self.hand_detector = HandDetector()
        self.fps = FPS()

        self.target_servox = 0
        self.target_servoy = 0
        self.xservo_pid = PID.PositionalPID(1.0, 0.04, 0.5)
        self.yservo_pid = PID.PositionalPID(1.0, 0.1, 0.5)
        sleep(2)

    def process(self, frame):
        frame, lmList, bbox = self.hand_detector.findHands(frame)
        if len(lmList) != 0:
            threading.Thread(
                target=self._follow, args=(bbox,), daemon=True).start()
        self.fps.update_fps()
        self.fps.show_fps(frame)
        return frame

    def _follow(self, bbox):
        hand_x = (bbox[0] + bbox[2]) / 2
        hand_y = (bbox[1] + bbox[3]) / 2

        if not (self.target_servox >= 90 and hand_x <= 320 or
                self.target_servox <= -90 and hand_x >= 320):
            self.xservo_pid.SystemOutput = hand_x
            self.xservo_pid.SetStepSignal(320)
            self.xservo_pid.SetInertiaTime(0.01, 0.1)
            target_valuex = int(1500 + self.xservo_pid.SystemOutput)
            self.target_servox = int((target_valuex - 500) / 10) - 90
            self.target_servox = max(-90, min(90, self.target_servox))

        if not (self.target_servoy >= 90 and hand_y <= 240 or
                self.target_servoy <= 0 and hand_y >= 240):
            self.yservo_pid.SystemOutput = hand_y
            self.yservo_pid.SetStepSignal(240)
            self.yservo_pid.SetInertiaTime(0.01, 0.1)
            target_valuey = int(1500 + self.yservo_pid.SystemOutput)
            self.target_servoy = int((target_valuey - 500) / 10) - 45
            self.target_servoy = max(0, min(90, self.target_servoy))

        joints_0 = [
            self.target_servox + 90,
            135,
            self.target_servoy // 2,
            self.target_servoy // 2,
            90, 30
        ]
        self.robot.arm_move_6(joints_0, 500)


def main():
    follower = HandFollow()

    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 = follower.process(frame)
            cv.imshow('HandFollow', frame)
            if cv.waitKey(1) & 0xFF == ord('q'):
                break
    finally:
        cap.release()
        cv.destroyAllWindows()


if __name__ == '__main__':
    main()