Welcome to DOFBOT-Ultra repository

Multimodal Large Model + Color Blocks Back In Place

1. Function Description

After the program starts, first say a command like "remember the current positions of the color blocks" via voice. After the program records them, scramble the positions of the color blocks, then say a command like "put the color blocks on the table back to their original positions" via voice. The large model will think and plan a list of actions. The program will control the robotic arm to complete the task of returning the color blocks to their original positions based on the action functions in the action list.

[Note: The content in this section is experimental functionality, greatly affected by the environment. In complex scenarios, the return may fail. This section is only intended to provide a reference approach.]

2. Startup

Enter the Docker container and input the following command:

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

After startup completes, wake up the module and say the command to remember the current positions of the color blocks. Refer to the following example:

x
Remember the current positions of the color blocks

After recording the color block positions, say the command to end the task via voice:

xxxxxxxxxx
End the position recording task

Then you can scramble the positions of the color blocks. After scrambling, say the command to return the color blocks to their original positions via voice. For example:

x
Return the blocks on the desktop to their original positions

After the buzzer beeps, the program starts gripping the color blocks in the order recorded in the program and placing them back to their original positions. If there are other color blocks on top of the color block that needs to be returned, it will first remove the blocks on top, then grip and place it back to its original position.

3. Task Plan

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 color blocks on the table and cache the bboxes. Subsequent gripping/removal will uniformly use color parameters, no need to pass coordinates manually.

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

3. For each current target color block X, follow these steps (only one action function output at a time):
   (1) Call check_remove('X') to check the removal list;
   (2) If feedback is "the color block exists in the removal list": call grasp_from_rm_list('X'), then call return_to_orin('X');
   (3) If feedback is "the color block does not exist in the removal list": first call scan_blocks_hsv() to update the cache, then call seewhat() to check if there is anything blocking the top of X;
   (4) If there is no blocking on top of X: call grasp_color('X') to grip, then call return_to_orin('X') to return;
   (5) If there is blocking on top of X (blocking color is top_color): call remove_color('top_color') to remove the blocking, then call scan_blocks_hsv() to update the cache, then call seewhat() to recheck, until no blocking exists, then execute (4).

4. When all returnable color blocks have been processed, the system feeds back return_to_orin_done and ends the current task.

Explanation: In the new workflow, seewhat() is only used for the large model to visually check blocking relationships, no longer responsible for providing gripping coordinates; in the return phase, using grasp_obj(...) and remove_obj(...) instead of 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: The top outer bounding box coordinates of the top block, x1,y1 indicate...
#name: The color of the top block
#order: Indicates the order list of stacked blocks from top to bottom. For example, if the top-to-bottom stacked colors are red and yellow, then the passed order is 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 top-to-bottom stacking order of the blocks
    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 position calculation program
    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 consider the xy direction to be consistent with the top block, only the height changes, so only z value needs to be modified, 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}")
            #Feedback to 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 Color Block Bboxes

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 col
 Multimodal Large Model + Robotic Arm Handling
Multimodal Large Model + Intent Prediction + Robotic Arm Gripping