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

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

1. Function Description

After the program starts, input commands for the robotic arm to track objects via voice. 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

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

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

After startup completes, wake up the module and say the tracking command.

There are two types of objects for robotic arm tracking:

• Track AprilTag: Track AprilTag x, where x is the AprilTag ID, ranging from 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

After waking up the module, say:

x
Track machine code 2

First, the robotic arm will adjust to the tracking pose, then capture the current image and start recognizing and tracking the target AprilTag. If the recognized AprilTag is not 2, the robotic arm will not track and will print "Not target apriltag" in the terminal. If the recognized AprilTag is 2, the robotic arm will start tracking. Slowly move the AprilTag and the robotic arm will follow. To exit tracking, wake up the module and say: Cancel tracking.

2.1.2. Task Plan

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

2. Call the apriltag_follow(2) function, pass parameter 2 indicating the ID of the AprilTag to track.

2.1.3. Core Code Analysis

The track_pose() function source is 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 the values of 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")

The apriltag_follow function source is 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 via command line parameter
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 currently 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 pixels 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 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

After waking up the module, say:

x
Track the red block in my hand

First, the robotic arm will adjust to the tracking pose, then capture a frame of the 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. To exit tracking, wake up the module and say: Cancel tracking.

2.2.2. Task Plan

1. Call the function track_pose() 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 function track(x1, y1, x2, y2), 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 previously and will not be repeated here. Mainly look at the track function. The source of this function is 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 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 of this program:

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
#Raspberry Pi: import library, path is /home/yahboom/dofbot_ultra_ws/src/llm/largemodel_arm/largemodel_arm/Dofbot_Track.py
#RDK: import library, path is /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 center point coordinates are not within 8 pixels of the image center
    if abs(center_x-320) >8 or abs(center_y-240)>8 :
        #Call XY_track function for tracking, pass current center point xy coordinates
        self.dofbot_tracker.XY_track(center_x,center_y) 

​x
#View the XY_track function in Dofbot_Track.py, calculate servo 1-4 values based on center point coordinates combined with PID parameters
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