Desktop Tracking and Grasping AprilTags

1. Course Content

The feature implements the program acquiring images through the camera and recognizing the AprilTag on the tabletop. When the AprilTag on the tabletop is moved slowly, the car will track the AprilTag using the chassis together with the robotic arm. After the AprilTag stops moving, the car adjusts the robotic arm back to the initial pose while adjusting the chassis so that the AprilTag's center appears in the middle of the image. After the adjustment is finished, use the keys to choose whether to continue tracking or grasp the AprilTag on the tabletop.

2. Preparation

The wooden block used in this section is: 40x40x40mm AprilTag wooden block

ros2 launch yahboomcar_bringup arm_demo.launch.py demo:=apriltag_track_desktop

3. Case Demonstration

• Hold the AprilTag wooden block and slide it smoothly on the tabletop; the robotic arm end-effector stays at a fixed safe height above the wooden block, and the car and robotic arm track the AprilTag moving above the tabletop;

• When the wooden block stops sliding, the system confirms that the wooden block has stopped stably and locks on, then adjusts the chassis to align with the target and performs grasping and placement

4. Core Source Code Analysis

• Program path:

xxxxxxxxxx
~/yahboomcar_ros2_ws/yahboomcar_ws/src/common_demo/common_demo/arm_demo/apriltag_track_desktop.py

• Parameter file

xxxxxxxxxx
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_bringup/param/common.yaml

Parameter Description:

Basic parameters:

• tag_size: 0.025: the physical side length of the AprilTag (meters), corresponding to a 4×4×4cm wooden block

• observation_pose: [90, 107, 0, 0, 90, 30]: the 6 joint angles (degrees) of the observation pose, at which the robotic arm has the best field of view

• grasp_offset: [0.0, 0.0, 0.0, 90.0]: grasping offset [x, y, z, pitch]; pitch=90° means the gripper grasps vertically downward

• gripping_angle: 140: the angle (degrees) when the gripper is closed; the larger the value, the tighter the gripper

Tracking parameters:

• track_distance_threshold: 0.13: tracking distance threshold (meters); when the AprilTag distance exceeds this value, the chassis automatically moves toward the target

• track_pixel_tolerance: 25: pixel alignment tolerance; alignment is considered achieved when the deviation between the AprilTag center and the image center is less than this value

• gaze_step: 2: the step (degrees) of each joint adjustment; the smaller the value, the smoother the tracking but the slower the response

• ema_alpha: 0.5: EMA filter coefficient (0~1); the smaller the value, the smoother the filtering but the slower the response; 0.5 is a balanced value

• alignment_stable_count: 35: consecutive alignment count threshold; tracking is considered stable only after reaching this count, which can then trigger grasping

Chassis PID control parameters:

• chassis_pid_kp_dist: 0.3: front-back distance PID proportional coefficient, controlling the response speed of the chassis moving forward/backward

• chassis_pid_ki_dist: 0.02: front-back distance PID integral coefficient, eliminating steady-state error

• chassis_pid_kd_dist: 0.03: front-back distance PID derivative coefficient, suppressing oscillation

• chassis_pid_kp_lateral: 1.5: lateral translation PID proportional coefficient, controlling the response speed of the chassis moving left/right

• chassis_pid_ki_lateral: 0.01: lateral translation PID integral coefficient

• chassis_pid_kd_lateral: 0.01: lateral translation PID derivative coefficient

• chassis_pid_rate: 10: PID control frequency (Hz); PID calculation is performed 10 times per second

• chassis_pid_output_limit: 0.15: PID output limit (m/s), controlling the chassis's maximum linear velocity to prevent moving too fast

• chassis_dist_dead_zone: 0.02: front-back distance dead zone (meters); the chassis movement is not triggered when the error is within this range

• chassis_lateral_dead_zone: 0.015: lateral offset dead zone (meters); lateral movement is not triggered when the error is within this range

• chassis_min_linear_speed: 0.05: minimum linear speed (m/s), acting on both front-back and lateral, ensuring that the chassis can overcome static friction to start moving

xxxxxxxxxx
apriltag_gesture_remove_height:
  ros__parameters:
    tag_size: 0.02                          # AprilTag side length (meters)
    apriltag_grasp_distance: [0.11, 0.0]    # measure coordinate frame x,y offset (meters)
    odom_timeout: 30.0                      # movement timeout (seconds)
    observation_pose: [90, 107, 0, 0, 90, 30]
    grasp_offset: [0.00,0.0,0.015,85.0]    # x,y,z axis offset distance and included angle
    gripping_angle: 140                     # gripper grasping angle, in degrees, integer
    detection_pose: [90, 150, 12, 20, 90, 30]  # robotic arm pose during gesture detectionapriltag_track_desktop:
  ros__parameters:
    tag_size: 0.025                         # 4*4*4 cm wooden block
    observation_pose: [90, 107, 0, 0, 90, 30]
    grasp_offset: [0.0, 0.0, 0.0, 90.0]
    gripping_angle: 140
    track_distance_threshold: 0.13          # tracking distance threshold (meters)
    track_pixel_tolerance: 25               # pixel alignment tolerance
    gaze_step: 2                             # joint adjustment step per cycle (degrees)
    ema_alpha: 0.5                           # EMA filter coefficient (0~1), the smaller the smoother
    alignment_stable_count: 35               # consecutive alignment count threshold; considered stable only after reaching it
    chassis_pid_kp_dist: 0.3
    chassis_pid_ki_dist: 0.02
    chassis_pid_kd_dist: 0.03
    chassis_pid_kp_lateral: 1.5             # lateral translation PID kp (outputs linear.y m/s)
    chassis_pid_ki_lateral: 0.01
    chassis_pid_kd_lateral: 0.01
    chassis_pid_rate: 10
    chassis_pid_output_limit: 0.15           # PID output limit (m/s), controlling the chassis's maximum linear velocity
    chassis_dist_dead_zone: 0.02            # front-back distance dead zone (meters)
    chassis_lateral_dead_zone: 0.015        # lateral offset dead zone (meters)
    chassis_min_linear_speed: 0.05          # minimum linear speed (m/s), acting on both front-back and lateral
    sorting_pose: [90, 107, 0, 0, 90, 30]   # robotic arm pose during sorting and grasping
    gesture_stable_threshold: 15            # gesture stable frame count threshold
    sorting_timeout: 3.0                    # sorting timeout (seconds); falls back to the detection pose after timeout
    height_unit: 0.01                       # height unit (meters), threshold=(gesture+1)*height_unit