Welcome to DOFBOT-Ultra repository
16. Fingertip trajectory control robotic arm
Fingertip Trajectory Control Robotic Arm
If using a depth camera (DABAI_DCW2), make sure to execute
ros2 run dofbot_ultra_driver calibrate_6pointfor 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!!!
Open the terminal and enter the tutorial command to run directly.
1、Startup
1.1、Program Description
After the program starts, the camera captures the image. Place your palm flat in the camera view with fingers spread and palm facing the camera, similar to a number 5 gesture. The image will draw all the joints on the palm. Adjust the palm position, preferably in the upper-middle part of the screen.
Now keep the index finger extended while retracting the other fingers, similar to a number 1 gesture.
While maintaining gesture 1, move the finger position. A red line appears on the screen, tracing the movement path of the index finger.
When the shape is complete, open all fingers (gesture 5), and the drawn shape will be generated below.
Note: The drawn shape needs to be a closed figure, otherwise parts may be missing.
There are four recognizable trajectory shapes, each corresponding to a different arm action:
| Drawn Shape | Recognition Name | Arm Action |
|---|---|---|
| Triangle | Triangle | Arm simulates triangular trajectory, swings left and right, then resets |
| Rectangle | Square | Arm extends forward, joint 4 swings back and forth 3 times, then resets |
| Circle | Circle | Joint 5 swings left and right 5 times (head-shaking effect), then resets |
| Star | Star | Arm performs "nodding" action, 3 round trips, then resets |
After successful recognition, the buzzer beeps once, then the arm performs the corresponding action. Press the spacebar to clear the currently drawn trajectory and start over.
1.2、Program Startup
Enter the following command to start the program
xxxxxxxxxxros2 run dofbot_mediapipe 16_FingerAction.py
Press the q key in the image window or Ctrl+C in the terminal to exit the program.
1.3、Source Code
Raspberry Pi code path:
xxxxxxxxxx~/dofbot_ultra_ws/src/dofbot_mediapipe/scripts/16_FingerAction.py
RDK X5 code path:
xxxxxxxxxx~/dofbot_ultra_ws/src/2d/dofbot_mediapipe/scripts/16_FingerAction.py
x#!/usr/bin/env python3# coding: utf8import enumimport cv2import timeimport numpy as npimport mediapipe as mpimport gcimport threadingfrom dofbot_utils.camera import create_camerafrom dofbot_utils.fps import FPSfrom dofbot_utils.vutils import distance, vector_2d_angle, get_area_max_contourfrom dofbot_utils.robot_controller import Robot_Controllerfrom Arm_Lib import Arm_Device
def get_hand_landmarks(img, landmarks): h, w, _ = img.shape return np.array([(lm.x * w, lm.y * h) for lm in landmarks])
def hand_angle(landmarks): angle_list = [] angle_list.append(vector_2d_angle( landmarks[3] - landmarks[4], landmarks[0] - landmarks[2])) angle_list.append(vector_2d_angle( landmarks[0] - landmarks[6], landmarks[7] - landmarks[8])) angle_list.append(vector_2d_angle( landmarks[0] - landmarks[10], landmarks[11] - landmarks[12])) angle_list.append(vector_2d_angle( landmarks[0] - landmarks[14], landmarks[15] - landmarks[16])) angle_list.append(vector_2d_angle( landmarks[0] - landmarks[18], landmarks[19] - landmarks[20])) return [abs(a) for a in angle_list]
def h_gesture(angle_list): thr_angle, thr_angle_s = 65.0, 49.0 if all(a < thr_angle_s for a in angle_list): return "five" elif (angle_list[0] > 5 and angle_list[1] < thr_angle_s and angle_list[2] > thr_angle and angle_list[3] > thr_angle and angle_list[4] > thr_angle): return "one" return "none"
class State(enum.Enum): NULL = 0 TRACKING = 1 RUNNING = 2
def draw_points(img, points, thickness=4, color=(255, 0, 0)): points = np.array(points).astype(dtype=np.int32) if len(points) > 2: for i in range(len(points) - 1): cv2.line(img, tuple(points[i]), tuple(points[i + 1]), color, thickness)
def get_track_img(points): points = np.array(points).astype(dtype=np.int32) x_min, y_min = np.min(points, axis=0).tolist() x_max, y_max = np.max(points, axis=0).tolist() track_img = np.full( [y_max - y_min + 100, x_max - x_min + 100, 1], 0, dtype=np.uint8) points = points - [x_min, y_min] + [50, 50] draw_points(track_img, points, 1, (255, 255, 255)) return track_img
def recognize_shape(points): track_img = get_track_img(points) contours = cv2.findContours( track_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)[-2] result = get_area_max_contour(contours, 300) if result is None: return 'Unknown', None contour = result[0] track_img = cv2.fillPoly(track_img, [contour], (255, 255, 255)) for _ in range(3): track_img = cv2.erode( track_img, cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))) track_img = cv2.dilate( track_img, cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))) contours = cv2.findContours( track_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)[-2] result = get_area_max_contour(contours, 300) if result is None: return 'Unknown', None contour = result[0] h, w = track_img.shape[:2] display = np.full([h, w, 3], 0, dtype=np.uint8) cv2.drawContours(display, [contour], -1, (0, 255, 0), 2) approx = cv2.approxPolyDP( contour, 0.026 * cv2.arcLength(contour, True), True) cv2.drawContours(display, [approx], -1, (0, 0, 255), 2)
n = len(approx) if n == 3: name = 'Triangle' elif n == 4 or n == 5: name = 'Square' elif 5 < n < 10: name = 'Circle' elif n == 10: name = 'Star' else: name = 'Unknown' cv2.putText(display, name, (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (255, 255, 0), 2) return name, display
class FingerAction: def __init__(self): self.drawing = mp.solutions.drawing_utils self.hand_detector = mp.solutions.hands.Hands( static_image_mode=False, max_num_hands=1, min_tracking_confidence=0.05, min_detection_confidence=0.6) self.fps = FPS() self.state = State.NULL self.points = [] self.start_count = 0 self.no_finger_timestamp = time.time() self.gc_stamp = time.time() self.move_state = False
self.Arm = Arm_Device() self.robot = Robot_Controller() self.robot.move_init_pose()
def process(self, frame): rgb_image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) rgb_image = cv2.flip(rgb_image, 1) result_image = np.copy(rgb_image)
try: results = (self.hand_detector.process(rgb_image) if self.state != State.RUNNING else None) if results is not None and results.multi_hand_landmarks: gesture = "none" index_finger_tip = [0, 0] self.no_finger_timestamp = time.time() for hand_landmarks in results.multi_hand_landmarks: self.drawing.draw_landmarks( result_image, hand_landmarks, mp.solutions.hands.HAND_CONNECTIONS) landmarks = get_hand_landmarks( rgb_image, hand_landmarks.landmark) gesture = h_gesture(hand_angle(landmarks)) index_finger_tip = landmarks[8].tolist()
if self.state == State.NULL: if gesture == "one": self.start_count += 1 if self.start_count > 20: self.state = State.TRACKING self.points = [] else: self.start_count = 0
elif self.state == State.TRACKING: if gesture == "five": self.state = State.NULL if len(self.points) > 10: name, display = recognize_shape(self.points) if display is not None: cv2.imshow('track', display) if not self.move_state and name != 'Unknown': self.move_state = True threading.Thread( target=self._arm_action, args=(name,), daemon=True).start() else: if len(self.points) > 0: if distance(self.points[-1], index_finger_tip) > 5: self.points.append(index_finger_tip) else: self.points.append(index_finger_tip) draw_points(result_image, self.points) else: if self.state == State.TRACKING: if time.time() - self.no_finger_timestamp > 2: self.state = State.NULL self.points = [] except Exception as e: print("Error:", e)
self.fps.update_fps() self.fps.show_fps(result_image) result_image = cv2.cvtColor(result_image, cv2.COLOR_RGB2BGR)
if time.time() > self.gc_stamp: self.gc_stamp = time.time() + 1 gc.collect()
return result_image
def _arm_action(self, name): self.robot.Arm_Buzzer_On(1) time.sleep(1) if name == 'Triangle': self.Arm.Arm_serial_servo_write6_array( [90, 131, 52, 0, 90, 180], 1500) time.sleep(1.5) self.Arm.Arm_serial_servo_write6_array( [45, 180, 0, 0, 90, 180], 1500) time.sleep(2) self.Arm.Arm_serial_servo_write6_array( [135, 180, 0, 0, 90, 180], 1500) time.sleep(2) self.Arm.Arm_serial_servo_write6_array( [90, 131, 52, 0, 90, 180], 1500) time.sleep(1.5) elif name == 'Square': self.Arm.Arm_serial_servo_write6_array( self.robot.P_ACTION_4, 1500) time.sleep(1.4) for _ in range(3): self.Arm.Arm_serial_servo_write(4, -15, 300) time.sleep(0.4) self.Arm.Arm_serial_servo_write(4, 20, 300) time.sleep(0.4) elif name == 'Circle': for _ in range(5): self.Arm.Arm_serial_servo_write(5, 60, 300) time.sleep(0.4) self.Arm.Arm_serial_servo_write(5, 120, 300) time.sleep(0.4) self.Arm.Arm_serial_servo_write(5, 90, 300) time.sleep(0.4) elif name == 'Star': for _ in range(3): self.Arm.Arm_serial_servo_write6_array( self.robot.P_ACTION_3, 1200) time.sleep(1.2) self.Arm.Arm_serial_servo_write6_array( self.robot.P_LOOK_AT, 1000) time.sleep(1) self.robot.move_init_pose() time.sleep(1.5) self.move_state = False
def main(): action = FingerAction()
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 result = action.process(frame) cv2.imshow('FingerAction', result) key = cv2.waitKey(1) & 0xFF if key == ord('q'): break if key == ord(' '): action.points = [] finally: cap.release() cv2.destroyAllWindows()
if __name__ == "__main__": main()