Welcome to DOFBOT-Ultra repository

Multimodal Large Model + Color Blocks Back In Place

[Note: The content of this section is an experimental feature, greatly affected by the environment. In complex scenarios, returning to the original position may fail. This tutorial is only intended to provide a reference approach.]

1. Feature Description

After the program runs, first enter the instruction to remember the current positions of the color blocks in the terminal. After the program completes recording, scramble the positions of the color blocks. Then enter the instruction to put the color blocks on the table back to their original positions. After the large model thinks, it will plan a list of actions. The program will execute the action functions in the list to control the robotic arm to complete the task of returning the color blocks to their original 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

Enter the Docker container and run the following command:

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

Now enter the instruction to remember the current positions of the color blocks in the terminal. Refer to the following example:

x
Remember the current positions of the color blocks

Then you can scramble the positions of the color blocks. After scrambling, enter the instruction to return the color blocks to their original positions in the terminal. For example:

x
Return the blocks on the desktop to their original positions

After the buzzer beeps, the program will start grasping the color blocks and putting them back to their original positions according to the order recorded in the program. If there are other blocks on top of the block that needs to be returned, the program will first remove the block on top, then grasp and return it to its original position.

3. Task Planning

3.1. Record Current Positions of Color Blocks

1. Call seewhat() to observe the environment;

2. Call seewhat() again to observe the environment, then determine which color block is the top block;

3. If there are two or more piles of color blocks in the environment, select and execute the following content based on whether a color block is a single one. Calculate for each pile of color blocks. Only one action function can be output at a time here:
   (1) If it is a single color block, pass the outer frame coordinates of the color block into the function compute_pose(x1, y1, x2, y2, 'name'), where name represents the color of the color block, and its value can be red, blue, green, or yellow;
   (2) If it is not a single color block, pass the outer frame coordinates of the top color block into the function compute_pose_order(x1, y1, x2, y2, 'name', 'order'), where name represents the color of the color block (with values red, blue, green, or yellow), and order represents the top-to-bottom arrangement order of the color blocks. Its value is a string composed of the initial letters of the colors of each layer of color blocks from top to bottom. For example, if the top-to-bottom arrangement order of the color blocks is red, blue, green, yellow, then the value of order is rbgy, and so on.

4. If there is only one pile of color blocks, pass the outer frame coordinates of the top color block into the function compute_pose_order(x1, y1, x2, y2, 'name', 'order'), where name represents the color of the color block (with values red, blue, green, or yellow), and order represents the top-to-bottom arrangement order of the color blocks. Its value is a string composed of the initial letters of the colors of each layer of color blocks from top to bottom. For example, if the top-to-bottom arrangement order of the color blocks is red, blue, green, yellow, then the value of order is rbgy, and so on.

3.2. Return Color Blocks to Original Positions

1. First call scan_blocks_hsv() to scan the blocks on the table and cache the bbox. For subsequent grasping/removal, uniformly use color parameters instead of manually passing coordinates.

2. Call color_back_to_orin() to start the return process. The program will publish "Next, return the X color block to its original position" one by one.

3. For each current target color block X, execute the following steps (only one action function at a time): (1) Call check_remove('X') to check the removal list; (2) If feedback is "The block exists in the removal list": call grasp_from_rm_list('X'), then call return_to_orin('X'); (3) If feedback is "The block does not exist in the removal list": first call scan_blocks_hsv() to update the cache, then call seewhat() to determine if there is an obstruction on top of X; (4) If there is no obstruction on top of X: call grasp_color('X') to grasp, then call return_to_orin('X') to return to position; (5) If there is an obstruction on top of X (obstructing color is top_color): call remove_color('top_color') to remove the obstruction, then call scan_blocks_hsv() to update the cache, then call seewhat() to re-check, until no obstruction remains, then execute (4).

4. When all returnable color blocks have been processed, the system sends feedback return_to_orin_done and ends the current round of tasks.

Note: In the new workflow, seewhat() is only used for the large model to examine images and determine obstruction relationships, no longer responsible for providing grasping coordinates. During the return phase, using grasp_obj(...) and remove_obj(...) to replace the color interface is prohibited.

4. Core Code Analysis

4.1. compute_pose_order - Record Stacked Color Block Positions

Source path:

Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/llm/largemodel/largemodel/action_service.py

RDK:/home/sunrise/dofbot_ultra_ws/src/llm/largemodel/largemodel/action_service.py

xxxxxxxxxx
#Parameter description
#x1,y1,x2,y2: Top outer bounding box coordinates of the top block, x1,y1 represent
#name: Color of the top block
#order: Indicates the order list of stacked blocks from top to bottom. For example, if the colors stacked from top to bottom are red and yellow, the passed order would be ry
def compute_pose_order(self,x1,y1,x2,y2,name,order):
    #Get current block parameters
    cur_name = name.strip("'\"")  
    color_order = order.strip("'\"")
    #Create a list to store the stacking order from top to bottom
    order_list = list(color_order[1:])
    self.get_logger().info(f"order_list:{order_list}")
    self.cur_name = cur_name
    self.cur_pose[self.cur_name] = []
    #Start calculating position information
    cmd1 = "ros2 run largemodel_arm Record_pose"
    proc = subprocess.Popen(
        [
            "gnome-terminal",
            "--title=compute_pose",
            "--",
            "bash",
            "-c",
            f"{cmd1}; exec bash",
        ]
    )
    self.pid_num = proc.pid   
    self.get_logger().info(f"self.pid_num:{self.pid_num}")
    time.sleep(5.0)
    x1 = int(x1)
    y1 = int(y1)
    x2 = int(x2)
    y2 = int(y2)
    self.object_position_pub.publish(Int16MultiArray(data=[x1, y1, x2, y2]))

    while not self.compute_pose_future.done():
        if self.interrupt_flag:
            self.check_close_compute_pose()
            Arm.Arm_serial_servo_write6(90,150,12,20,90,30,1000)
            self.stop()
            return
        time.sleep(0.1)

    result = self.compute_pose_future.result()
    self.check_close_compute_pose()
    if not self.interrupt_flag:
        if result.data == "compute_pose_done":
            for i in range(0,len(order_list)):
                #Here we assume that the xy direction is consistent with the top block, only the height changes. So we only need to change the z value, decreasing by 2.8 cm each time, approximately the height of one block
                x = self.cur_pose[cur_name][0]
                y = self.cur_pose[cur_name][1]
                z = self.cur_pose[cur_name][2] - (i+1)*0.028
                if order_list[i] == 'r' or  order_list[i] == 'R':
                    self.cur_pose['red'] = []
                    self.cur_pose['red'].append(x) 
                    self.cur_pose['red'].append(y) 
                    self.cur_pose['red'].append(z) 
                elif order_list[i] == 'b' or  order_list[i] == 'B':
                    self.cur_pose['blue'] = []
                    self.cur_pose['blue'].append(x) 
                    self.cur_pose['blue'].append(y) 
                    self.cur_pose['blue'].append(z)                
                elif order_list[i] == 'y' or  order_list[i] == 'Y':
                    self.cur_pose['yellow'] = []
                    self.cur_pose['yellow'].append(x) 
                    self.cur_pose['yellow'].append(y) 
                    self.cur_pose['yellow'].append(z)  
                elif order_list[i] == 'g' or  order_list[i] == 'G':
                    self.cur_pose['green'] = []
                    self.cur_pose['green'].append(x) 
                    self.cur_pose['green'].append(y) 
                    self.cur_pose['green'].append(z)  
                    
            self.get_logger().info(f"self.cur_pose:{self.cur_pose}")
            #Provide feedback to the large model that recording stacked block positions is complete
            self.action_status_pub("compute_pose_order_done", x1=x1, y1=y1, x2=x2, y2=y2,name=name,order=order)
        else:
            self.action_status_pub("compute_pose_failed", x1=x1, y1=y1, x2=x2, y2=y2)

4.2. scan_blocks_hsv - Scan and Cache Block Bbox

Source path:

Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/llm/largemodel/largemodel/action_service.py

RDK:/home/sunrise/dofbot_ultra_ws/src/llm/largemodel/largemodel/action_service.py

xxxxxxxxxx
def scan_blocks_hsv(self):
    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
        else:
            missing.append(color)
    self._HSV_BOXES_CACHE = found
    if not found:
        self.action_status_pub("scan_blocks_hsv_no_block")
        self.finishtask()
        return
    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)
    self.action_status_pub("scan_blocks_hsv_done", summary=summary)

4.3. color_back_to_orin - Publish Color Block Return Order

Source path:

Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/llm/largemodel/largemodel/action_service.py

RDK:/home/sunrise/dofbot_ultra_ws/src/llm/largemodel/largemodel/action_service.py

xxxxxxxxxx
def color_back_to_orin(self):
    self.back_list = True
    self.return_order = self._get_return_color_order()
    self.return_order_index = 0
    self.return_done = False
    if len(self.return_order) == 0:
        self.action_status_pub("return_to_orin_done")
        self.back_list = False
        return
    self._publish_next_return_target()

"send_red_back_to_orin": "机器人反馈:接下来把红色方块进行归位",
"send_green_back_to_orin": "机器人反馈:接下来把绿色方块进行归位",
"send_blue_back_to_orin": "机器人反馈:接下来把蓝色方块进行归位",
"send_yellow_back_to_orin": "机器人反馈:接下来把黄色方块进行归位"

4.4. check_remove(color) - Check If the Current Block to Return Exists in the Removal List

Source path:

Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/llm/largemodel/largemodel/action_service.py

RDK:/home/sunrise/dofbot_ultra_ws/src/llm/largemodel/largemodel/action_service.py

​x
def check_remove(self,color):
    check_color = color.strip("'\"")
    if check_color == "all":
        if len(self.cur_rm_pose) > 0:
            self.action_status_pub("color is in the rm_list")
        else:
            self.action_status_pub("color is not in the rm_list")
        return
    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")

"color is in the rm_list" : "移除列表中存在该色块，在移除列表中夹取该色块",
"color is not in the rm_list" : "移除列表中不存在该色块，判断该色块的顶部是否存在其它物体",