🖥️ Desktop 2D Plane Tracking and Grasping AprilTags

1. Course Overview

The function implemented in this lesson can be simply understood as: vehicle and robotic arm follow the AprilTag as it moves, automatically descend to grasp when the block stops.

Unlike 3D omnidirectional tracking, this lesson constrains the robotic arm end-effector to move within a fixed-height horizontal top-down plane. The program uses AprilTag detection to obtain the tag's desktop position in real-time, driving the vehicle and robotic arm to track the AprilTag moving above the table. When the AprilTag stops sliding and remains continuously still for 15 frames, the chassis automatically and smoothly approaches, and the robotic arm vertically descends to execute in-place grasping.

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:

ros2 launch dofbot_pro_driver A1pro_camera_arm.launch.py

2.2 Start Robotic Arm Desktop Grasping Action Execution Node

Run in Terminal 2:

​x
# 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 Desktop Plane Tracking Node

Run in Terminal 3:

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

# Or USB camera version
ros2 run dofbot_pro_usb apriltag_track_desktop

3. Operation Demo and Interaction Flow

1. Handheld block desktop translation: Hold the AprilTag block and slide it steadily on the table. The robotic arm end-effector stays at a fixed safe height above the block. The vehicle and robotic arm will track the AprilTag moving above the table;

2. Stationary detection:

   • When the block stops sliding, the algorithm starts detecting pixel displacement;

   • When pixel displacement is less than 3 pixels for all 15 consecutive frames (approximately 0.5~1.0 seconds), the system confirms the block has stopped and locks it;

3. Automatic approach and in-place descent:

   • Buzzer sounds, chassis automatically and smoothly advances to align with the material;

   • Robotic arm automatically deflects the gripper based on tag rotation angle and vertically descends to grasp;

4. Motion reset: After grasping, the robotic arm resets to the initial patrol pose, resuming monitoring and tracking for the next block movement.

4. Core Source Code Analysis

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

The core business logic comparison between the two:

4.1 Continuous 15 Frames Stationary Anti-Jitter Detection Algorithm

To avoid falsely triggering grasping during block movement, both versions implement highly sensitive stationary detection:

xxxxxxxxxx
# apriltag_track_desktop.py —— Stationary displacement detection
dist_moved = math.hypot(center_x - self.last_tag_x, center_y - self.last_tag_y)

if dist_moved < STATIC_THRESHOLD:  # Displacement less than 3 pixels
    self.static_counter += 1
    if self.static_counter >= STATIC_FRAMES_REQ:  # Continuous stationary reaches 15 frames
        self.adjust_dist = True                   # Activate chassis approach
        self.XY_Track_flag = False                # Lock end-effector plane tracking
else:
    # As long as the block is still moving, immediately reset counter and update reference coordinates
    self.static_counter = 0
    self.last_tag_x = center_x
    self.last_tag_y = center_y

4.2 Plane PID Following and Chassis Approach

1. Fixed-Height Plane Gimbal Following (XY_track)

Fix the robotic arm pitch angle, only use base left/right rotation and front/rear link extension to adjust following target, keeping the gripper end-effector sliding parallel at a fixed table height:

xxxxxxxxxx
# apriltag_track_desktop.py —— Plane following
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 smoothing filter
angular_x = self.last_angular_x + max(-self.max_step_change, min(self.max_step_change, diff_x))

target_j1 = 90  + (10 * angular_x * gain_j1)  # Adjust left/right yaw
target_j2 = 120 + (angular_y * gain_j2)        # Adjust front/rear extension, height remains constant

2. Depth Camera Hardware Ranging and Grasping Publishing

After confirming stationary status, chassis drives smoothly toward the target block based on true physical distance. After arriving, solve gripper deflection angle and publish 3D grasping pose:

xxxxxxxxxx
# apriltag_track_desktop.py —— Depth camera grasping publishing
cz = depth_image[int(cy), int(cx)] / 1000.0
if cz != 0:
    vx = int(tag.corners[0][0]) - int(tag.corners[1][0])
    vy = int(tag.corners[0][1]) - int(tag.corners[1][1])
    self.joint5.data = int(compute_joint5(vx, vy))  # Gripper adaptive angle

    pos = AprilTagInfo()
    pos.id = tag.tag_id
    pos.x = float(cx)
    pos.y = float(cy)
    pos.z = float(cz)

    self.TargetJoint5_pub.publish(self.joint5)
    self.pos_info_pub.publish(pos)  # Trigger grasp_desktop to execute descent

After grasping completes, the node resets all status and resets the robotic arm to initial pose, smoothly resuming desktop plane tracking for the next sliding block.

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)