Welcome to DOFBOT-Ultra repository

Multimodal Large Model + Robotic Arm Sorting

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

Before running the feature, you need to close the web control program backend process. To close it, refer to [4. Preparation] -> [1. Manage Web control services].

1. Feature Description

After the program runs, enter the color block sorting order through the terminal. The large model will plan the steps to complete the color block sorting task. Then the program will control the robotic arm to sort the color blocks in order and place them at the designated positions.

2. Startup

To calibrate colors:

xxxxxxxxxx
Raspberry Pi:python3 /home/yahboom/dofbot_ultra/2d/dofbot_color_identify/scripts/hsv_calibration_gui.py
RDK:python3 /home/sunrise/dofbot_ultra_ws/src/2d/dofbot_color_identify/scripts/hsv_calibration_gui.py

For the Raspberry Pi version, you need to enter the Docker container and run:

xxxxxxxxxx
ros2 launch largemodel largemodel_control.launch.py text_chat_mode:=True

Then open a second terminal and enter the following command:

xxxxxxxxxx
ros2 run text_chat text_chat

Then enter the color block sorting order in the terminal. Refer to the following example:

xxxxxxxxxx
Sort the color blocks on the desktop in the order of yellow, red,  green

As shown in the figure above, before sorting the yellow color block, first check if it is in the removal list. If it is, then directly grip it from the removal list and place it at the yellow color block placement point; if not, then determine if there are other color blocks on top of the yellow color block. If there are, first remove the other color blocks above the yellow color block. If there are none, then directly grip it and place it at the yellow color block placement point. Other color blocks are also sorted according to this execution order until the task is completed.

3. Task Planning

0. At task start, call the scan_blocks_hsv() function to scan all block positions on the table using HSV color recognition, and cache the bounding box (bbox) coordinates of each color for subsequent grasping/removal.

For each color in the sorting order (color, values: red, blue, green, or yellow), execute the following process sequentially:

1. Call the check_remove(color) function to check if the target color block to be sorted exists in the removal list;

2. If the target color block exists in the removal list, perform the following: (1) Call the grasp_from_rm_list(color) function to grasp the block from the temporary position recorded in the removal list; (2) Call arm_stack(color) to place the target block at the sorting destination position corresponding to that color.

3. If the target block does not exist in the removal list: (1) Call the seewhat() function to capture the current desktop image, and let the large model determine whether there are other objects blocking the top of the target block to be sorted (the large model analyzes the image, no additional function call); (2) If there is no obstruction on top of the target block: (i) Call the grasp_color(color) function to grasp the target block (the function internally reads the bbox of that color from the HSV cache automatically, no need to manually pass coordinates); (ii) Call arm_stack(color) to place the target block at the sorting destination position corresponding to that color. (3) If there is an obstruction on top of the target block (obstructing color is top_color): (i) Call the remove_color(top_color) function to remove the obstruction (the function internally reads the bbox of that color from the HSV cache); (ii) Re-call scan_blocks_hsv() to update the block cache; (iii) Re-call seewhat() to re-check until no obstruction remains on top of the target block; (iv) Call grasp_color(color) to grasp the target block, then call arm_stack(color) to place it at the destination position.

4. After all colors in the sorting order have been processed, the task ends.

4. Core Code Analysis

4.1. scan_blocks_hsv() Function

Source path: src/llm/largemodel/largemodel/action_service.py

​x
def scan_blocks_hsv(self):
    # Call auto_detect_blocks for HSV color recognition, write results to latest_detections.json
    self.auto_detect_blocks(publish_status=False)

    found = {}
    missing = []
    for color in ("red", "green", "blue", "yellow"):
        box = self._load_latest_detections_box(color)
        if box:
            found[color] = box   # Cache format: {color: (x1, y1, x2, y2)}
        else:
            missing.append(color)

    # Store detection results in HSV cache for subsequent grasp_color/remove_color usage
    self._HSV_BOXES_CACHE = found

    if not found:
        # No color blocks found, terminate the task
        self.action_status_pub("scan_blocks_hsv_no_block")
        self.finishtask()
        return

    # Generate readable summary to feed back to the large model
    color_cn = {"red": "红色", "green": "绿色", "blue": "蓝色", "yellow": "黄色"}
    parts = [f"{color_cn.get(c, c)}(bbox={x1},{y1},{x2},{y2})"
             for c, (x1, y1, x2, y2) in found.items()]
    if missing:
        parts.append(f"未检测到：{','.join(color_cn.get(c, c) for c in missing)}")
    summary = "；".join(parts)

    # Note: do not call finishtask() here, the instruction cycle continues, the execution LLM proceeds to the next step
    self.action_status_pub("scan_blocks_hsv_done", summary=summary)

Feedback content example:

x
Robot feedback: HSV color block scan complete. Detected: Red (bbox=412,176,484,232); Blue (bbox=422,312,506,389); Yellow (bbox=226,325,306,403); Not detected: Green; For subsequent grasping/removal, please use grasp_color(color) / remove_color(color) directly.

4.2. check_remove(color) Function

Source path: src/llm/largemodel/largemodel/action_service.py

xxxxxxxxxx
def check_remove(self, color):
    check_color = color.strip("'\"")
    # self.cur_rm_pose is the removal list dictionary, key is the color name, value is the servo angle list during temporary placement
    if check_color in self.cur_rm_pose:
        self.action_status_pub("color is in the rm_list")
    else:
        self.action_status_pub("color is not in the rm_list")

Corresponding meanings of the two feedback types:

xxxxxxxxxx
"color is in the rm_list"     → "The block exists in the removal list, grasp it from the removal list"
"color is not in the rm_list" → "The block does not exist in the removal list, determine if there are other objects on top of this block"

4.3. grasp_from_rm_list(color) Function

Source path: src/llm/largemodel/largemodel/action_service.py

xxxxxxxxxx
def grasp_from_rm_list(self, color):
    tar_color = color.strip("'\"")
    # Get the servo angles recorded during temporary placement from the removal list
    tar_joints = self.cur_rm_pose.get(tar_color)
    # Move the arm to the temporary position
    self.pub_arm([tar_joints[0], tar_joints[1], tar_joints[2],
                  tar_joints[3], tar_joints[4], tar_joints[5]], runtime=2000)
    time.sleep(2.0)
    # Close gripper to grasp the block
    self.pub_arm([], id=6, angle=140, runtime=1000)
    time.sleep(2.0)
    # Lift the arm and retract
    self.pub_arm([90, 120, 0, 0, 90, 140], runtime=1000)
    time.sleep(2.0)
    self.action_status_pub("grasp_from_rm_list_done")

4.4. grasp_color(color) Function

Source path: src/llm/largemodel/largemodel/action_service.py

xxxxxxxxxx
def grasp_color(self, color):
    color = str(color).strip().strip("'\"").lower()
    # Get bbox from the cache built by scan_blocks_hsv(), no need for the large model to provide coordinates
    box = self._HSV_BOXES_CACHE.get(color)
    if not box:
        self.action_status_pub("grasp_color_no_bbox", color=color)
        return
    x1, y1, x2, y2 = box
    # Call grasp_obj for actual grasping, use _suppress_status=True to suppress its own feedback
    succeeded = self.grasp_obj(x1, y1, x2, y2, _suppress_status=True)
    if not self.interrupt_flag:
        if succeeded:
            # Publish semantically clearer grasp_color_done, letting the LLM know which color was successfully grasped
            self.action_status_pub("grasp_color_done", color=color)
        else:
            self.action_status_pub("grasp_color_failed", color=color)

Feedback content example:

xxxxxxxxxx
Robot feedback: grasp_color(yellow) execution completed.

4.5. remove_color(color) Function

Source path: src/llm/largemodel/largemodel/action_service.py

xxxxxxxxxx
def remove_color(self, color):
    color = str(color).strip().strip("'\"").lower()
    # Get the obstruction's bbox from HSV cache
    box = self._HSV_BOXES_CACHE.get(color)
    if not box:
        self.action_status_pub("remove_color_no_bbox", color=color)
        return
    x1, y1, x2, y2 = box
    # Remove the color from cache after removing to prevent stale coordinates being used; need re-scan_blocks_hsv() to update
    self._HSV_BOXES_CACHE.pop(color, None)
    # Call the underlying remove_obj to execute the grasp-temporary placement flow
    self.remove_obj(x1, y1, x2, y2, color)

The execution logic of remove_obj: Start the visual positioning node (Detect_Obj) and the grasping node (grasp_obj), grasp the obstruction and place it at a temporary side position, then record the color and temporary placement servo angles in self.cur_rm_pose (removal list) for subsequent retrieval by grasp_from_rm_list.

4.6. arm_stack(color) Function

Source path: src/llm/largemodel/largemodel/action_service.py

def arm_stack(self, color):
    tar_color = color.strip("'\"")
    # Each color corresponds to a preset hardcoded placement position (servo angles)
    if tar_color == 'red':
        self.pub_arm([117, 19, 66, 56, 90, self.grasp_joint], runtime=2000)
        time.sleep(2.0)
    elif tar_color == 'green':
        self.pub_arm([136, 66, 20, 29, 90, self.grasp_joint], runtime=2000)
        time.sleep(2.0)
    elif tar_color == 'blue':
        self.pub_arm([44, 66, 20, 28, 90, self.grasp_joint], runtime=2000)
        time.sleep(2.0)
    elif tar_color == 'yellow':
        self.pub_arm([65, 22, 64, 56, 90, self.grasp_joint], runtime=2000)
        time.sleep(2.0)
    # Release gripper, drop the block
    self.pub_arm([], id=6, angle=30, runtime=1500)
    time.sleep(1.5)
    # Retract arm to initial position
    self.pub_arm([90, 120, 0, 0, 90, 30], runtime=1000)
    time.sleep(2.0)
    self.action_status_pub("arm_stack_done", color=color)