Welcome to DOFBOT-Ultra repository

Multimodal Large Model + Robotic Arm Tracking

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 instructions for the robotic arm to track objects through the terminal. Based on the type of object to track, the large model will start an external program to control the robotic arm to track the target object.

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

Then open a second terminal and enter the following command:

xxxxxxxxxx
ros2 run text_chat text_chat

Robotic arm tracking has two types of objects:

• Track AprilTag: Track AprilTag x, x is the AprilTag ID, range 1-4. For example, track AprilTag 1.

• Track other objects: Track any object. For example, track the little yellow duck on the green block.

2.1. Track AprilTag

2.1.1. Startup

Enter the following in the text_chat terminal:

x
Track machine code 2

First, the robotic arm will adjust to the tracking posture, then acquire the current image and start recognizing and tracking the target machine code. If the recognized machine code is not 2, the robotic arm will not track and will print "Not target apriltag" in the terminal; if the recognized machine code is 2, the robotic arm will start tracking, moving the machine code slowly, and the robotic arm will follow the movement. If you want to exit tracking, input in the text_chat terminal: Cancel tracking.

2.1.2. Task Planning

1. Call the track_pose() function to adjust the robotic arm to the tracking pose;

2. Call the apriltag_follow(2) function, where the parameter 2 indicates the ID of the AprilTag to track.

2.1.3. Core Code Analysis

track_pose() 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 track_pose(self):  # Robotic arm tracking pose
    #Directly communicate with the bottom control board, send values for the 6 servos
    Arm.Arm_serial_servo_write6(90,150,12,20,90,30,1000)
    time.sleep(3.0)
    if not self.combination_mode and not self.interrupt_flag:
        self.action_status_pub("track_pose_done")

apriltag_follow 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 apriltag_follow(self,target_id):
    target_idf = float(target_id)
    #Open a terminal, run the AprilTag tracking program, pass parameter target_id indicating the AprilTag ID to track
    cmd1 = f"ros2 run largemodel_arm apriltag_follow_2D --ros-args -p target_id:={target_idf:.1f}"
    subprocess.Popen(
        [
            "gnome-terminal",
            "--title=apriltag_follow",
            "--",
            "bash",
            "-c",
            f"{cmd1}; exec bash",
        ]
    )    

The AprilTag tracking program apriltag_follow_2D, source path 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
#Get the target tracking id from command line parameters
self.declare_parameter('target_id', 0.0)
self.TargetID = int(self.get_parameter('target_id').get_parameter_value().double_value)
#In the image callback function callback
cur_id = tags[0].tag_id
#If the current recognized id is the target tracking id
if cur_id == self.TargetID:
    #Get the center point coordinates of the AprilTag
    center_x, center_y = tags[0].center
    #Check if the center point coordinates are within 10 pixel error of the image center
    if (abs(center_x-320) >10 or abs(center_y-240)>10) and self.XY_Track_flag==True:
        #Call XY_track function for tracking, pass in the current center point coordinates
        self.XY_track(center_x,center_y)  
        print("Tracking")
        print("-------------------------------------")
    else:
        print("Not target apriltag.")

2.2. Track Other Objects

2.2.1. Startup

Enter the following in the text_chat terminal:

x
Track the red block in my hand

First, the robotic arm will adjust to the tracking posture, then acquire the current frame image to find the red block in hand, then control the robotic arm to track the red block. Slowly move the position of the red block, and the robotic arm will follow the movement. If you want to exit tracking, input in the text_chat terminal: Cancel tracking.

2.2.2. Task Planning

1. Call the track_pose() function to adjust the robotic arm to the tracking pose;

2. Call seewhat() to observe the environment and find the red block in hand;

3. Call the track(x1, y1, x2, y2) function, where x1, y1, x2, y2 are the outer bounding box coordinates of the red block.

2.2.3. Core Code Analysis

The track_pose() and seewhat() functions were analyzed earlier and will not be repeated here. Let's mainly look at the track 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
#x1,y1,x2,y2: Object outer bounding box coordinates
def track(self, x1, y1, x2, y2):
    #Start KCF tracking program
    cmd1 = "ros2 run largemodel_arm KCF_Track_Move"
    #Start KCF tracking and positioning program
    cmd2 = "ros2 run largemodel_arm ALM_KCF_Tracker"
    subprocess.Popen( 
        [
            "gnome-terminal",
            "--title=KCF_track",
            "--",
            "bash",
            "-c",
            f"{cmd1}; exec bash",
        ]
    )
    subprocess.Popen(
        [
            "gnome-terminal",
            "--title=ALM_KCF_Tracker_Node",
            "--",
            "bash",
            "-c",
            f"{cmd2}; exec bash",
        ]
    )
    #Publish outer bounding box coordinate information topic data, ALM_KCF_Tracker 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]))

KCF_Track_Move program, source location:

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

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

xxxxxxxxxx
#Import library, path is
#Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/llm/largemodel_arm/largemodel_arm/Dofbot_Track.py
#RDK:/home/sunrise/dofbot_ultra_ws/src/llm/largemodel_arm/largemodel_arm/Dofbot_Track.py
from largemodel_arm.Dofbot_Track import *

#Create robotic arm tracking and gripping object
self.dofbot_tracker = DofbotTrack()

#Create subscriber, subscribe to object position information topic (xy)
self.sub_grasp_status = self.create_subscription(Position,"/pos_xy",self.TrackAndGrap,100)
#Topic callback function, process received topic message data
def TrackAndGrap(self,position):  
    center_x, center_y = position.x,position.y
    #If the current object's center point coordinates are not within 8 pixel error of the image center
    if abs(center_x-320) >8 or abs(center_y-240)>8 :
        #Call XY_track function for tracking, pass in the current center point xy coordinates
        self.dofbot_tracker.XY_track(center_x,center_y) 

#In Dofbot_Track.py, view the XY_track function. Based on the center point coordinates, combined with PID parameters, calculate values for servos 1-4
def XY_track(self,center_x,center_y):
    self.px = center_x
    self.py = center_y
    if not (self.target_servox>=180 and center_x<=320 and self.a == 1 or self.target_servox<=0 and center_x>=320 and self.a == 1):
        if(self.a == 0):
            self.xservo_pid.SystemOutput = center_x
            if self.x_out_range == True:
                if self.target_servox<0:
                    self.target_servox = 0
                    self.xservo_pid.SetStepSignal(630)
                if self.target_servox>0:
                    self.target_servox = 180
                    self.xservo_pid.SetStepSignal(10)
                self.x_out_range = False
            else:
                self.xservo_pid.SetStepSignal(320)
                self.x_out_range = False
           
            self.xservo_pid.SetInertiaTime(0.01, 0.1)
            
            target_valuex = int(1500 + self.xservo_pid.SystemOutput)
            
            self.target_servox = int((target_valuex - 500) / 10) -10
    
            if self.target_servox > 180:
                self.x_out_range = True
                
            if self.target_servox < 0:
                self.x_out_range = True
                        
    if not (self.target_servoy>=180 and center_y<=240 and self.b == 1 or self.target_servoy<=0 and center_y>=240 and self.b == 1):
        if(self.b == 0):
            self.yservo_pid.SystemOutput = center_y

            if self.y_out_range == True:
                self.yservo_pid.SetStepSignal(450)
                self.y_out_range = False
            else:
                self.yservo_pid.SetStepSignal(240)

            self.yservo_pid.SetInertiaTime(0.01, 0.1)
           
            target_valuey = int(1500 + self.yservo_pid.SystemOutput)
            
            if target_valuey<=1000:
                target_valuey = 1000
                self.y_out_range = True
            self.target_servoy = int((target_valuey - 500) / 10) - 55
            if self.target_servoy > 180: self.target_servoy = 180 
            if self.target_servoy < 0: self.target_servoy = 0 

            joint2 = 120 + self.target_servoy
            joint3 =  self.target_servoy / 4.5
            joint4 =  self.target_servoy / 3
    joints_0 = [float(self.target_servox/1), float(joint2), float(joint3), float(joint4), 90.0, 30.0]
    self.Arm.Arm_serial_servo_write6_array(joints_0,1000)
    self.cur_joints = joints_0