🔍 KCF Target Tracking and Autonomous Grasping

1. Course Overview

The function implemented in this lesson can be simply understood as: mouse box-select any object, robotic arm automatically tracks the target, approaches and autonomously descends to grasp.

Unlike color recognition or AprilTag machine-code methods, KCF (Kernelized Correlation Filters) is a general-purpose prior-free target tracking algorithm — without needing to pre-enter specific color thresholds or paste black and white QR codes. Simply box-select an object of interest in the real-time image, and the system establishes an appearance feature model for continuous stable tracking.

2. Preparation and Running

This case requires opening 3 terminals to start the system sequentially:

2.1 Start Camera Driver and Kinematics Solver Environment

Run in Terminal 1:

xxxxxxxxxx
ros2 launch dofbot_pro_driver A1pro_camera_arm.launch.py

2.2 Start Robotic Arm Desktop Grasping Action Execution Node

Run in Terminal 2:

xxxxxxxxxx
# Depth camera version (Nuwa)
ros2 run dofbot_pro_driver grasp_desktop

# Or USB camera version
ros2 run dofbot_pro_usb grasp_usb

2.3 Start KCF Tracking Master Node

Run in Terminal 3:

xxxxxxxxxx
# Depth camera version (Nuwa)
ros2 run dofbot_pro_driver kcf_Tracker

# Or USB camera version
ros2 run dofbot_pro_usb kcf_follow

Box-select object to start tracking:

3. Operation Demo and Interaction Flow (Version Difference Key Notes)

Due to hardware characteristics and algorithm implementation differences, USB camera version and 3D depth camera version have significant differences in operation and robotic arm actions. Please read based on your device:

3.1 Solution A: USB Camera Version Interaction Flow (kcf_follow)

1. Robotic arm initial positioning: After program startup, the robotic arm smoothly moves to a high-angle downward pose: [90°, 100°, 0°, 0°, 90°, 90°], directly vertical top-down view of desktop;

2. Mouse box-select target: In the image window, hold left mouse button and drag to draw a rectangle selecting the object to grasp. After releasing mouse, KCF algorithm immediately locks the target and draws 🟢 green tracking box;

3. Manually press spacebar to trigger grasping:

   • Keep the image window active, press the spacebar (Space) on the keyboard;

   • Chassis receives command and starts forward/backward linear velocity PID control, automatically and smoothly approaching the target (approaching to approximately 190mm);

4. Grasping and unloading: After reaching grasping distance, chassis stops, program analyzes target contour to calculate gripper alignment angle, robotic arm descends to grasp;

5. Reset and reselect: After grasping, chassis briefly reverses to avoid collision. Press keyboard R to re-box-select a new object.

3.2 Solution B: 3D Depth Camera Version Interaction Flow (kcf_Tracker)

1. Robotic arm forward-looking patrol pose: After program startup, the robotic arm moves to forward-looking wide-range gimbal pose: [90°, 150°, 12°, 20°, 90°, 0°] (joint 2 raised high to 150°, elbow extended, wider field of view);

2. Mouse box-select target: In the image window hold left mouse button and drag to box-select the target. After releasing mouse, the system completes KCF feature modeling;

3. Robotic arm gimbal active dynamic servo tracking:

   • Manually press spacebar! If you slowly move the target object horizontally or up/down, the robotic arm's joint 1 base servo and joints 2/3/4 pitching will rotate automatically, always keeping the lens facing the target;

4. Continuous stable alignment auto-approach grasping (no spacebar needed):

   • When the target remains within the center tolerance range for continuous 10 frames (ALIGN_COUNT_THRESHOLD = 10), buzzer sounds to confirm lock;

   • Chassis automatically starts and drives toward the target without user button press (approaching to approximately 240mm optimal working zone);

   • After chassis is in position, directly read depth point cloud true distance, robotic arm automatically converts gripper deflection angle and descends to execute grasping;

5. Motion reset: After grasping, the robotic arm automatically returns to [90°, 150°, 12°, 20°, 90°, 0°] patrol pose, and resets alignment counter and PID status, ready to box-select the next target at any time.

4. Core Source Code Analysis

The system is completed by "KCF target tracking decision" and "IK execution" two nodes working in coordination:

The main technical implementation and control strategy comparison:

4.1 USB Monocular Camera Core Source Code Analysis (kcf_follow.py)

1. Mouse Drag Box Selection and KCF Initialization

When mouse is released, extract the box-selected rectangle ROI and call OpenCV tracker for initialization:

xxxxxxxxxx
# kcf_follow.py —— Mouse box selection and KCF initial feature modeling
elif event == cv2.EVENT_LBUTTONUP:
    x1, y1 = min(start_x, x), min(start_y, y)
    w, h = abs(x - start_x), abs(y - start_y)
    if w > 10 and h > 10:
        self.select_roi = (x1, y1, w, h)
        self.state = State.TRACKING

# Initialize KCF
self.tracker = Tracker(tracker_type='KCF')
self.tracker.initWorking(rgb_image, self.select_roi)

2. Spacebar Trigger Chassis PID Approach

After user presses spacebar, the program switches from TRACKING to APPROACHING state. Chassis advances based on pinhole model estimated horizontal physical distance to 190mm:

xxxxxxxxxx
# kcf_follow.py —— Spacebar trigger and chassis approach
if abs(dist_mm - GRASP_DIST_MM) > GRASP_TOLERANCE:
    linear_x = self.linearx_pid.compute(dist_mm, GRASP_DIST_MM)
    angle_z = self.angle_pid.compute(self.track_cx, ALIGN_TARGET_X)
    self._pub_vel(linear_x, 0, -angle_z)
else:
    self._pub_vel(0, 0, 0)
    self._trigger_grasp(rgb_image, cam_coords, est_depth)

4.2 3D Depth Camera Core Source Code Analysis (KCF_Tracker.py)

1. Robotic Arm Gimbal Adaptive Dynamic Closed-Loop Tracking (XY_track)

Unlike USB monocular solution, after box selection is complete in the depth camera solution, the robotic arm base and pitching joints perform active gimbal servo compensation following the target:

xxxxxxxxxx
# KCF_Tracker.py —— Gimbal adaptive dynamic tracking
def XY_track(self, center_x, center_y):
    # Calculate deviation between target center and image center (320, 240)
    raw_angular_x = self.angular_x_PID.compute(center_x, TARGET_X)
    raw_angular_y = self.angular_y_PID.compute(center_y, TARGET_Y)
    
    # Slope limiter prevents severe overshoot and jitter
    diff_x = raw_angular_x - self.last_angular_x
    angular_x = self.last_angular_x + max(-self.max_step_change, min(self.max_step_change, diff_x))
    
    # Dynamically calculate joint 1 base and joints 2/3/4 angles, lens always follows
    target_j1 = 90 + (10 * angular_x * -90.0)
    target_j2 = 150 + (angular_y * -150.0)
    target_j3 = 12 + (angular_y * -60.0)
    target_j4 = 20 + (angular_y * -100.0)

2. Continuous 10 Frames Lock Auto-Trigger Chassis Approach (Button-Free Mechanism)

The program detects whether the target enters the center tolerance range (PIXEL_TOLERANCE = 10). Once continuously stable for 10 frames, the program automatically switches to approach phase without user spacebar operation:

xxxxxxxxxx
# KCF_Tracker.py —— 10 frames continuous center detection and auto-advance
err_x = abs(center_x - TARGET_X)
err_y = abs(center_y - TARGET_Y)
is_aligned = (err_x < PIXEL_TOLERANCE) and (err_y < PIXEL_TOLERANCE)

# Continuously meet alignment threshold, automatically enter approach
if is_aligned and self.Done_flag and not self.adjust_dist and not self.start_grasp:
    self.aligned_count += 1
    if self.aligned_count >= ALIGN_COUNT_THRESHOLD:  # Reach 10 frames
        self.Beep_Loop()        # Buzzer response confirmation
        self.adjust_dist = True # Automatically enter chassis approach phase
        self.XY_Track_flag = False

if self.adjust_dist:
    # Drive chassis smoothly toward target based on depth map extracted true distance (240mm)
    if self.cur_distance > GRASP_DIST + 0.2:
        self.move_dist(self.cur_distance)
    else:
        self.adjust_dist = False
        self.start_grasp = True
        self.pubVel(0, 0, 0)

3. Hardware-Level Depth Point Cloud Reading and 3D IK Grasping

After chassis stops, directly extract the physical ranging data at the target center pixel grasp_depth = depth_image[gy, gx] / 1000.0, and calculate gripper alignment angle compute_joint5(vx, vy) through vertex slope vector. Publish coordinates to grasp_desktop node to execute descent closure and material grasping.

Through the above comparison analysis, users can clearly understand the deep-level differences in robotic arm pose and interaction control logic between the two hardware solutions, ensuring standardized operation under different hardware configurations.

4.3 Grasping Offset Tuning (Both Camera Models)

If position deviation occurs during actual grasping (such as missing, knocking over objects, offset left/right, or hitting the table), you can directly modify the spatial offset configuration file for the corresponding camera:

1. Where to Modify

Configuration files are located in the ~/yahboomcar_ros2_ws/dofbot_pro_ws/src/dofbot_pro_driver/config/ directory. Select based on camera type:

• 3D Depth Camera (Nuwa-HP60C): offset_value.yaml

• USB Camera: usb_offset_value.yaml

2. How to Modify

The file contains offset parameters in X, Y, Z directions (unit: meters, recommended adjustment per tweak is 0.005~0.01, i.e., 5~10mm):

• x_offset (forward/backward direction, positive toward vehicle front):

  • Gripper extends too far and knocks over object decrease value (adjust smaller or set negative)

  • Gripper doesn't extend far enough and misses increase value (adjust larger)

• y_offset (left/right direction, positive toward vehicle left):

  • Gripper deviates right increase value (correct left)

  • Gripper deviates left decrease value (correct right)

• z_offset (up/down height, positive toward ceiling):

  • Gripper too high in air and can't grasp decrease value (probe deeper)

  • Gripper hits or scrapes table increase value (raise higher)