Welcome to DOFBOT-Ultra repository

Multimodal Large Model + Robotic Arm Handling

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.

1. Feature Description

After the program runs, enter color block handling instructions through the terminal. The large model will plan the actions to complete the handling task. Then the program will control the robotic arm to handle the color blocks in sequence.

2. Startup

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

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 open a second terminal and enter the following command:

xxxxxxxxxx
ros2 run text_chat text_chat

Then enter the color block handling instruction in the terminal. Refer to the following example:

x
Place the green block on the desktop above the red block

The available placement directions are: front, back, left, right, and top.

The program will first take a photo to get the positions of the green block and the red block. Then it will control the robotic arm to grasp the green block and place it on top of the red block.

3. Task Planning

1. Call seewhat() to observe the environment and check the positions of the green block and the red block.

2. Call grasp_obj(x1, y1, x2, y2) to grasp the green block, where (x1, y1, x2, y2) are the outer frame coordinates of the green block.

3. Call change_pose(x1, y1, x2, y2, "green", "red", 5, tar_size) to place the green block on top of the red block, where (x1, y1, x2, y2) are the outer frame coordinates of the red block, tar_size indicates the height of the red block.

4. Core Code Analysis

4.1. change_pose Function

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-left and bottom-right coordinates of the outer bounding box of the block to grasp, in the above case, the green block
#x3,y3,x4,y4: Top-left and bottom-right coordinates of the outer bounding box of the target block for placement, in the above case, the red block
#src: Color of the block to grasp, in the above case, green
#tar: Color of the target block for placement, in the above case, red
#side: Placement direction: front, back, left, right, top correspond to 1, 2, 3, 4, 5. In the above case, it is 5, indicating top
def change_pose(self, x1, y1, x2, y2,x3,y3,x4,y4,src,tar,side):
    #Extract the color of the block to grasp
    src_color = src.strip("'\"")  
    self.cur_down_name = src_color
    #Create a key in the dictionary
    self.cur_down_pose[self.cur_down_name] = []
    #Start the handling program
    cmd1 = "ros2 run largemodel_arm Get_Target_Pose_KCF"
    subprocess.Popen(
        [
            "gnome-terminal",
            "--title=grasp_desktop",
            "--",
            "bash",
            "-c",
            f"{cmd1}; exec bash",
        ]
    )
    time.sleep(3.0)
    #Publish outer bounding box coordinate information and placement direction topic data, Get_Target_Pose_KCF node will subscribe to this topic
    x1 = int(x1)
    y1 = int(y1)
    x2 = int(x2)
    y2 = int(y2)
    x3 = int(x3)
    y3 = int(y3)
    x4 = int(x4)
    y4 = int(y4)
    side = int(side)
    time.sleep(5.0)       
    self.object_position_pub.publish(Int16MultiArray(data=[x1, y1, x2, y2,x3,y3,x4,y4,side])

4.2. Get_Target_Pose_KCF Handling Program

Source path:

Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/llm/largemodel_arm/largemodel_arm/Get_Target_Pose_KCF.py

RDK:/home/sunrise/dofbot_ultra_ws/src/llm/largemodel_arm/largemodel_arm/Get_Target_Pose_KCF.py

xxxxxxxxxx
#Create subscriber, subscribe to corner_xy topic content
self.xy_subscription = self.create_subscription(Int16MultiArray,'corner_xy',self.GetXYCallback,qos_profile=1)
#Create publisher, publish the angle value topic of the arm's placement pose
self.down_joints_pub = self.create_publisher(CurJoints,'/down_joints',1)
#GetXYCallback callback function, process received topic message data
def GetXYCallback(self,msg):
    print("msg: ",msg.data)
    #Get the outer bounding box coordinates of the block to grasp
    self.Roi_init_src = (msg.data[0],msg.data[1],msg.data[2],msg.data[3])
    #Get the outer bounding box coordinates of the target block for placement
    self.Roi_init_tar = (msg.data[4],msg.data[5],msg.data[6],msg.data[7])
    #Get the placement direction data
    self.side = msg.data[8]
    self.Track_state = 'identify'
    self.get_xy = True
    time.sleep(2.5)
    
#Image callback function get_Src_Tar
def get_Src_Tar(self,color_frame,depth_frame):
    #If the outer bounding box coordinates of both blocks are not empty
    if len(self.Roi_init_src)!=0 and len(self.Roi_init_tar)!=0:
        cv2.rectangle(result_image, (self.Roi_init_src[0], self.Roi_init_src[1]), (self.Roi_init_src[2], self.Roi_init_src[3]), (255,0,0), thickness=2)
        cv2.rectangle(result_image, (self.Roi_init_tar[0], self.Roi_init_tar[1]), (self.Roi_init_tar[2], self.Roi_init_tar[3]), (0,255,0), thickness=2)
        
        #Get the center point coordinates of the block to grasp and calculate its depth information
        grasp_cx = (self.Roi_init_src[0] + self.Roi_init_src[2])/2
        grasp_cy = (self.Roi_init_src[1] + self.Roi_init_src[3])/2
        grasp_depth = depth_image_info[int(grasp_cy),int(grasp_cx)]/1000
        
        #Get the center point coordinates of the target block for placement and calculate its depth information
        tar_cx = (self.Roi_init_tar[0] + self.Roi_init_tar[2])/2
        tar_cy = (self.Roi_init_tar[1] + self.Roi_init_tar[3])/2
        tar_depth = depth_image_info[int(tar_cy),int(tar_cx)]/1000    
        #If both depth values are not 0, the distance is valid
        if tar_depth!=0 and grasp_depth!=0:
            #Calculate the position information of the block to grasp
            pose_grasp = self.compute_heigh(grasp_cx,grasp_cy,grasp_depth)
            #Calculate the position information of the target block for placement
            pose_tar = self.compute_heigh(tar_cx,tar_cy,tar_depth)
            self.tar_x = pose_tar[0]
            self.tar_y = pose_tar[1]
            self.tar_z = pose_tar[2]
            #print("pose: ",pose)
            if self.done ==True:
                #Get the current end pose of the arm
                self.get_current_end_pos()
                self.done = False
                #Call the grasping function to grasp the block, pass in the position information of the block to grasp
                self.grasp(pose_grasp[0],pose_grasp[1],pose_grasp[2])
#Grasp function grasp, pass in the position information of the block to grasp, call inverse kinematics service
def grasp(self,x,y,z):
    while not self.client.wait_for_service(timeout_sec=1.0):
        self.get_logger().info('Service not available, waiting again...')       
    self.get_current_end_pos()        
    request = Kinemarics.Request()        
    request.tar_y = y  + self.y_offset + 0.01
    request.tar_z = z  + self.z_offset + 0.01
    if x>0:
        request.tar_x = x +0.01 + self.x_offset
    else:
        request.tar_x = x - 0.01 + self.x_offset
    request.kin_name = "ik"
    request.roll = -0.785
    future = self.client.call_async(request)
    future.add_done_callback(self.grasp_get_ik_respone_callback)   
#Inverse kinematics service callback function grasp_get_ik_respone_callback, get servo values, control servos to target pose and grasp
def grasp_get_ik_respone_callback(self, future):    
    try: 
        response = future.result()
        print("calcutelate_response: ",response)
        joints = [0.0, 0.0, 0.0, 0.0, 0.0,0.0]
        joints[0] = response.joint1 #response.joint1
        joints[1] = response.joint2 
        joints[2] = response.joint3 
        if response.joint4>90:
            joints[3] = 90 
        else:
            joints[3] = response.joint4 
        joints[4] = 90
        joints[5] = 30
        self.Arm.Arm_serial_servo_write6(joints[0],joints[1],joints[2],joints[3],joints[4],joints[5],2000)
        time.sleep(3.5)
        self.Arm.Arm_serial_servo_write(6, 135, 2000)
        time.sleep(2.5)
        self.init_joints[5] = self.grasp_joint
        #After grasping, return to initial pose, ready to place on top of the target block
        self.Arm.Arm_serial_servo_write6_array(self.init_joints,2000)    
        time.sleep(2.5)
        #Execute the function to place on top of the target block
        self.move()
    except Exception:
       pass 
#Place block function move
def move(self):
    request = Kinemarics.Request()
    px = self.tar_x + self.x_offset
    py = self.tar_y + self.y_offset 
    pz = self.tar_z + self.z_offset 
    #Based on the placement direction value, modify the position of the target block. px: left/right, py: front/back, pz: up
    #Front
    if self.side == 1:
        py = py - 0.05
    #Back
    elif self.side == 2:
        py = py + 0.05
    #Left
    elif self.side == 3:
        px = px - 0.07
    #Right
    elif self.side == 4:
        px = px + 0.07
    #Top
    elif self.side == 5:
        pz = pz + 0.04
    #After determining the direction data, call the inverse kinematics service to get the arm angles to reach the target block position    
    request.tar_x =  px 
    request.tar_y = py 
    request.tar_z = pz 
    request.kin_name = "ik"
    request.roll = -1.0
    future = self.client.call_async(request)
    future.add_done_callback(self.move_get_ik_respone_callback)
#Inverse kinematics service callback function move_get_ik_respone_callback, get servo values, control servos to target pose and place
def move_get_ik_respone_callback(self, future):    
    try: 
        response = future.result()
        print("calcutelate_response: ",response)
        joints = [0.0, 0.0, 0.0, 0.0, 0.0,0.0]
        joints[0] = response.joint1 #response.joint1
        joints[1] = response.joint2 
        joints[2] = response.joint3 
        if response.joint4>90:
            joints[3] = 90 
        else:
            joints[3] = response.joint4 
        joints[4] = 90
        joints[5] = self.grasp_joint
        self.Arm.Arm_serial_servo_write6(joints[0],joints[1],joints[2],joints[3],joints[4],joints[5],2000)
        time.sleep(2.5)
        self.Arm.Arm_serial_servo_write(6, 0, 2000)
        time.sleep(2.5)
        cur_joints = CurJoints()
        cur_joints.joints = [int(x) for x in joints]
        #Publish the angle value topic of the arm's placement pose, action_service node will subscribe to this topic
        self.down_joints_pub.publish(cur_joints)
        self.init_joints[5] = 30
        self.Arm.Arm_serial_servo_write6_array(self.init_joints,2000)
        time.sleep(2.5)
        #Provide feedback to the large model that handling is complete
        self.largemodel_arm_done_pub.publish(String(data='change_pose_done'))
    except Exception:
       pass    

5. Advanced Instructions

We can have the robotic arm complete a more complex task: remember the current position of a color block, then place it on top of another block, and finally put it back to its original position. Enter the following in the text_chat terminal:

x
Remember the current position of the red square, then place the red square on top of the yellow square, and finally return the red square to its original position.

The program will first take a photo, find the red block and calculate its current position, then take another photo to find the positions of the red block and yelloww block, then control the robotic arm to place the red block on top of the yellow block. Then grip the red block and finally return the red block to its original position.

5.1. Task Planning

1. Call seewhat() to observe the environment;

2. Call the compute_pose(x1, y1, x2, y2) function to record the current position of the yellow block;

3. Call grasp_obj(x1, y1, x2, y2) to grasp the yellow block, where x1, y1, x2, y2 are the outer frame coordinates of the yellow block;

4. Call seewhat() to observe the environment and check the position of the red block;

5. Call the change_pose(x1, y1, x2, y2, "yellow", "red", side, tar_size) function to place the yellow block on top of the red block, where x1, y1, x2, y2 are the outer frame coordinates of the red block, side indicates the placement position, and tar_size indicates the height of the red block;

6. Call grasp_from_down_list("yellow") to find the current position of the yellow block in the placement list;

7. Call the return_to_orin("color") function, where "color" is the color of the target block to be reset, valid values are "red", "blue", "green", or "yellow".

5.2. Core Code Analysis

The change_pose function was analyzed above. Let's mainly analyze the three functions: compute_pose, grasp_from_down_list, and return_to_orin.

5.2.1. compute_pose - Position Recording Function

Source located at:

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 compute_pose(self,x1,y1,x2,y2,name):
    
    cur_name = name.strip("'\"")  # Remove single and double quotes
    self.cur_name = cur_name
    self.cur_pose[self.cur_name] = []
    #self.check_close_compute_pose()
    #Start the position recording program
    cmd1 = "ros2 run largemodel_arm Record_pose"
    # cmd3 = "ros2 run --prefix 'gdb -ex run --args' M3Pro_KCF ALM_KCF_Tracker_Node"
    subprocess.Popen(
        [
            "gnome-terminal",  
            "--title=compute_pose",
            "--",
            "bash",
            "-c",
            f"{cmd1}; exec bash",
        ]
    )                 
    time.sleep(5.0)
    #Publish outer bounding box coordinate information topic data, Record_pose node will subscribe to this topic
    x1 = int(x1)
    y1 = int(y1)
    x2 = int(x2)
    y2 = int(y2)
    self.object_position_pub.publish(Int16MultiArray(data=[x1, y1, x2, y2]))

Record_pose position recording program, source located at:

Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/llm/largemodel_arm/largemodel_arm/Record_pose.py

RDK:/home/sunrise/dofbot_ultra_ws/src/llm/largemodel_arm/largemodel_arm/Record_pose.py

​x
#Create subscriber, subscribe to object outer bounding box information
self.xy_subscription = self.create_subscription(Int16MultiArray,'corner_xy',self.GetXYCallback,qos_profile=1)   #Create publisher, publish current position topic message   
self.current_pose_position_pub = self.create_publisher(Float32MultiArray, "current_pose", 1)

#Callback function GetXYCallback, process received message, calculate the center point coordinates of the object
def GetXYCallback(self,msg):
    print("msg: ",msg.data)
    print(msg.data[0])
    print(msg.data[1])        
    print(msg.data[2])
    print(msg.data[3])
    self.cx = (msg.data[0] + msg.data[2])/2
    self.cy = (msg.data[1] + msg.data[3])/2
    
#In the TagDetect callback function,
if self.cx!=0 and self.cy!=0:
    cx = self.cx
    cy = self.cy
    #Get the depth value of the object's center point
    cz = depth_image_info[int(cy),int(cx)]/1000
    if cz!=0 and self.pub_pose == True:
        #Calculate the position of the current center point
        pose = self.compute_heigh(cx,cy,cz)
        print("pose: ",pose) 
        #Publish the object's current position topic message
        self.current_pose_position_pub.publish(Float32MultiArray(data=[pose[0], pose[1], pose[2]]))
        self.pub_pose = False
        #Provide feedback to the large model that recording the current position is complete
        self.largemodel_arm_done_pub.publish(String(data="compute_pose_done"))

5.2.2. grasp_from_down_list - Grasp Object from Placement List Function

Source located at:

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 grasp_from_down_list(self,color):
    tar_color = color.strip("'\"")
    #Based on the key, find the target pose value of the arm in the placement list
    tar_joints = self.cur_down_pose.get(tar_color)
    #Control the arm to move to the target pose
    Arm.Arm_serial_servo_write6(tar_joints[0], tar_joints[1], tar_joints[2], tar_joints[3], tar_joints[4], 0,2000)
    time.sleep(2.0)
    Arm.Arm_serial_servo_write(6, 140, 1000)
    time.sleep(2.0)
    Arm.Arm_serial_servo_write6(90,120,10,10,90,140,2000)
    time.sleep(2.0)
    #Provide feedback to the large model that grasping from the placement list is complete
    self.action_status_pub("grasp_from_down_list_done")   

5.2.3. return_to_orin - Color Block Return to Origin Function

Source located at:

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

def return_to_orin(self,name):
    #Get the current color parameter
    tar_name = name.strip("'\"")
    self.get_logger().info(f'"tar_name": {tar_name}')
    self.return_flag = True
    self.get_logger().info('Start to put it to the orin position.')
    self.get_current_end_pos()
    while not self.client.wait_for_service(timeout_sec=1.0):
        self.get_logger().info('Service not available, waiting again...')     
    #If the current color block can be found in the dictionary storing current positions, call the inverse kinematics service with the block's position as a parameter, get the arm servo angle values, then control the arm to move.
    if tar_name in self.cur_pose:
        self.get_logger().info('Get the orin position.') 
        request = Kinemarics.Request()
        request.tar_x = self.cur_pose[tar_name][0]
        request.tar_y = self.cur_pose[tar_name][1] + 0.01
        request.tar_z = self.cur_pose[tar_name][2] + 0.01
        request.kin_name = "ik"
        request.roll = -1.0
        #request.pitch = self.CurEndPos[4]
        #request.yaw = self.CurEndPos[5]
        self.get_logger().info(f"request: {request}")
        future = self.client.call_async(request)
        future.add_done_callback(self.get_ik_respone_callback)