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

The web control program backend process must be closed before starting this function.

1. Function Description

After the program starts, speak the color block sorting order via voice. 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 set positions.

2. Startup

Raspberry Pi version needs to enter docker to run the command.

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

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

Then give the sorting order of color blocks via voice. 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 a yellow color block, first check if it is in the removal list. If it is, directly take it from the removal list and place it at the yellow color block placement point. If it is not, then determine whether there are other color blocks on top of the yellow block. If there are, first remove the other blocks on top of the yellow block. If not, directly grip it and place it at the yellow color block placement point. Other color blocks follow the same execution order until the task ends.

3. Task Plan

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

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

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

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

3. If the target color block is not in the removal list: (1) Call the seewhat() function to get the current desktop image, and let the large model determine whether there are other objects blocking the top of the target color block to be sorted (the large model analyzes the image, no additional function call); (2) If there is no blocking object on top of the target color block: (1) Call the grasp_color(color) function to grip the target color block (the function internally reads the bbox of that color from the HSV cache, no need to pass coordinates manually); (2) Call arm_stack(color) to place the target color block at the sorting target position corresponding to its color. (3) If there is a blocking object on top of the target color block (blocking color is top_color): (1) Call the remove_color(top_color) function to remove the blocking object (the function internally reads the bbox of that color from the HSV cache); (2) Call scan_blocks_hsv() again to update the color block cache; (3) Call seewhat() again to recheck until the top of the target color block is no longer blocked; (4) Call grasp_color(color) to grip the target color block, then call arm_stack(color) to place it at the target position.

4. After processing all colors in the sorting order, the task ends.

Main differences from the old version:

• Added a scan_blocks_hsv() step at the beginning of the task; color block bboxes are uniformly provided by HSV scanning;

• Changed color block gripping from grasp_obj(x1,y1,x2,y2) to grasp_color(color), no need to manually pass coordinates;

• Changed blocking object removal from remove_obj(x1,y1,x2,y2,color) to remove_color(color), no need to manually pass coordinates;

• seewhat() is only used for the large model to visually check blocking relationships, no longer responsible for providing gripping coordinates.

4. Core Code Analysis

The core code is identical to the text version (17.AI Large Model - Text Version) of [Multimodal Large Model + Robotic Arm Sorting], only the startup method differs (voice version does not need a second terminal). The following is the complete code description.

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, results written 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 use
    self._HSV_BOXES_CACHE = found

    if not found:
        # No color blocks found at all, 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, execution layer LLM proceeds to the next step
    self.action_status_pub("scan_blocks_hsv_done", summary=summary)

Feedback content example:

xxxxxxxxxx
机器人反馈:HSV色块扫描完成，检测到：红色(bbox=412,176,484,232)；蓝色(bbox=422,312,506,389)；黄色(bbox=226,325,306,403)；未检测到：绿色；后续夹取/移除请直接使用 grasp_color(color) / remove_color(color)

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 at 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")

Semantic meaning of the two feedback responses:

xxxxxxxxxx
"color is in the rm_list"     → "The color block exists in the removal list, grip the color block from the removal list"
"color is not in the rm_list" → "The color block does not exist in the removal list, check if there are other objects on top of this color 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 robotic 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 grip color block
    self.pub_arm([], id=6, angle=140, runtime=1000)
    time.sleep(2.0)
    # Lift the robotic 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 established 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 to perform actual gripping, 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 gripped
            self.action_status_pub("grasp_color_done", color=color)
        else:
            self.action_status_pub("grasp_color_failed", color=color)

Feedback content example:

xxxxxxxxxx
机器人反馈:grasp_color(yellow)执行完成

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 blocking object's bbox from the 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
    # After removal, invalidate the color cache to avoid misusing old coordinates; need to call scan_blocks_hsv() again to update
    self._HSV_BOXES_CACHE.pop(color, None)
    # Call the underlying remove_obj to execute the grip-temporary placement process
    self.remove_obj(x1, y1, x2, y2, color)

The execution logic of remove_obj: start the visual positioning node (Detect_Obj) and the gripping node (grasp_obj), grip the blocking object and place it at a temporary side position, and record the color and the temporary placement servo angles into 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 pre-defined 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 color block
    self.pub_arm([], id=6, angle=30, runtime=1500)
    time.sleep(1.5)
    # Retract robotic 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)