Welcome to DOFBOT-Ultra repository
5. Multimodal Large Model + Robotic Arm Gripping
Multimodal Large Model + Robotic Arm Gripping
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.
Please use 30x30x30 wood blocks.
1. Function Description
After the program starts, input action commands for the robotic arm to grip objects via voice. Based on the type of object to grip, the large model will start an external program to control the robotic arm to grip the target object.
2. Startup
Raspberry Pi version needs to enter docker to run the command. RDK runs the command directly on the host.
xxxxxxxxxxros2 launch largemodel largemodel_control.launch.py text_chat_mode:=FalseThe robotic arm gripping has the following action commands:
Grab by machine code ID (For the machine code to be captured by the USB camera, use the wooden block supplied with the package and place it on the desktop): Sort machine codes No.1/2/3/4
Grip by AprilTag height [Depth camera only]: Remove AprilTags with height above x cm (x indicates height), for example: Remove AprilTags with height above 5 cm
Grip by color block height [Depth camera only]: Remove y color blocks with height above x cm (x indicates height, y indicates color, values are red, green, yellow, blue), for example: Remove red blocks with height above 5 cm
Grip other objects [Depth camera only]: Grip the little yellow duck on the table, grip the blue cuboid on the table
2.1. Grip by AprilTag ID
2.1.1. Startup
After waking up the module, say:
xSort machine code 2
Then two terminals will open, starting the machine code recognition program and gripping program respectively. After recognizing machine code 2, the gripping program will control the robotic arm to descend and grip machine code 2, then place it at the set position, and finally return to the machine code recognition posture to prepare for the next gripping. This continues until no more machine code 2 is recognized, then it reports back to the large model and the task ends.
2.1.2. Task Plan
Plan the action function apriltag_sort(2), where the parameter 2 indicates the ID of the AprilTag to be gripped.
2.1.3. Core Code Analysis
The apriltag_sort 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
xxxxxxxxxxdef apriltag_sort(self, target_id): # Grip AprilTag #self.check_apriltag_sort() target_idf = float(target_id) #Start the gripping program cmd1 = "ros2 run largemodel_arm grasp #Start the AprilTag recognition program, pass parameter target_id cmd2 = f"ros2 run largemodel_arm apriltag_sort --ros-args -p target_id:={target_idf:.1f}" subprocess.Popen( [ "gnome-terminal", "--title=grasp_desktop_apritag", "--", "bash", "-c", f"{cmd1}; exec bash", ] ) subprocess.Popen( [ "gnome-terminal", "--title=apriltag_sort", "--", "bash", "-c", f"{cmd2}; exec bash", ] )
The gripping program grasp, source path at:
Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/llm/largemodel_arm/largemodel_arm/grasp.py
RDK:/home/sunrise/dofbot_ultra_ws/src/llm/largemodel_arm/largemodel_arm/grasp.py
xxxxxxxxxx#Create subscriber, subscribe to position information topicself.sub = self.create_subscription(AprilTagInfo,'PosInfo',self.pos_callback,1)#Position information topic callback functiondef pos_callback(self,msg): pos_x = msg.x pos_y = msg.y pos_z = msg.z self.cur_tagId = msg.id if pos_z!=0.0: print("xyz id : ",pos_x,pos_y,pos_z,self.cur_tagId) #Start coordinate system transformation camera_location = self.pixel_to_camera_depth((pos_x,pos_y),pos_z) PoseEndMat = np.matmul(self.EndToCamMat, self.xyz_euler_to_mat(camera_location, (0, 0, 0))) EndPointMat = self.get_end_point_mat() WorldPose = np.matmul(EndPointMat, PoseEndMat) #Convert rotation transformation matrix to xyz and Euler angles, get target pose pose_T, pose_R = self.mat_to_xyz_euler(WorldPose) pose_T[0] = pose_T[0] + self.x_offset pose_T[1] = pose_T[1] + self.y_offset pose_T[2] = pose_T[2] + self.z_offset print("pose_T: ",pose_T) if self.grasp_flag == True : print("Take it now.") self.grasp_flag = False #Pass target position information to grasp function, perform inverse kinematics to get each servo angle threading.Thread(target=self.grasp, args=(pose_T,)).start()#Call inverse kinematics service, pass target position, get servo 1-5 values.def grasp(self,pose_T): print("------------------------------------------------") print("pose_T: ",pose_T)
request = Kinemarics.Request() request.tar_x = pose_T[0] request.tar_y = pose_T[1] + 0.01 request.tar_z = pose_T[2] + 0.01 request.kin_name = "ik" request.roll = -1.0 #((2.5*request.tar_y*100)-207.5)*(math.pi / 180)
# print("calcutelate_request: ",request) try: future = self.client.call_async(request) rclpy.spin_until_future_complete(self, future, timeout_sec=5.0) 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 The AprilTag recognition program apriltag_sort, source path at:
Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/llm/largemodel_arm/largemodel_arm/apriltag_sort.py
RDK:/home/sunrise/dofbot_ultra_ws/src/llm/largemodel_arm/largemodel_arm/apriltag_sort.py
xxxxxxxxxx#Get target sorting ID via command line parameterself.declare_parameter('target_id', 0.0)self.TargetID = int(self.get_parameter('target_id').get_parameter_value().double_value)
#Create publisher, publish position information topicself.pos_info_pub = self.create_publisher(AprilTagInfo, "PosInfo", qos_profile=10)
#Image callback functiondef TagDetect(self,color_frame,depth_frame): #rgb_image rgb_image = self.rgb_bridge.imgmsg_to_cv2(color_frame,'rgb8') result_image = np.copy(rgb_image) #depth_image depth_image = self.depth_bridge.imgmsg_to_cv2(depth_frame, encoding[1]) depth_to_color_image = cv.applyColorMap(cv.convertScaleAbs(depth_image, alpha=1.0), cv.COLORMAP_JET) frame = cv.resize(depth_image, (640, 480)) depth_image_info = frame.astype(np.float32) #AprilTag recognition tags = self.at_detector.detect(cv2.cvtColor(rgb_image, cv2.COLOR_RGB2GRAY), False, None, 0.025) tags = sorted(tags, key=lambda tag: tag.tag_id) draw_tags(result_image, tags, corners_color=(0, 0, 255), center_color=(0, 255, 0)) key = cv2.waitKey(10) if self.count==True: if (time.time() - self.start_time)>10: self.pubPos_flag = True self.count = False if key == 32: self.pubPos_flag = True if len(tags) > 0 : for i in range(len(tags)): center_x, center_y = tags[i].center cur_id = tags[i].tag_id cv.circle(depth_to_color_image,(int(center_x),int(center_y)),1,(255,255,255),10) #If the current ID is the target sorting ID, then found, set self.detect_flag to True and record the current array index for subsequent value retrieval if self.pubPos_flag == True and cur_id == self.TargetID: self.detect_flag = True print("Found the target.") self.index = i center_x, center_y = tags[self.index].center cv.circle(result_image, (int(center_x),int(center_y)), 10, (0,210,255), thickness=-1) pos = AprilTagInfo() #Get the xy coordinates of the target AprilTag, the depth value of the center point, and the ID pos.id = tags[self.index].tag_id pos.x = center_x pos.y = center_y pos.z = depth_image_info[int(center_y),int(center_x)]/1000 vx = int(tags[self.index].corners[0][0]) - int(tags[self.index].corners[1][0]) vy = int(tags[self.index].corners[0][1]) - int(tags[self.index].corners[1][1]) target_joint5 = compute_joint5(vx,vy) if pos.z>0: self.joint5.data = int(target_joint5) self.TargetJoint5_pub.publish(self.joint5) #Publish position information topic data message self.pos_info_pub.publish(pos) self.pubPos_flag = False else: print("Invalid distance.") #AprilTag not found if self.detect_flag == False and self.TargetID!=0 and self.pubPos_flag==True: self.pubPos_flag = False print("Did not find the target.") self.largemodel_arm_done_pub.publish( String(data='apriltag_sort_done')) self.TargetID = 02.2. Grip by AprilTag Height
2.2.1. Startup
After waking up the module, say:
xRemove machine codes with height higher than 3 cm
Then two terminals will open, starting the AprilTag height calculation program and the gripping program respectively. After recognizing an AprilTag with height above 3 cm, the gripping program will control the robotic arm to grip the abnormal height AprilTag, place it at the set position, and finally return to the AprilTag recognition pose to prepare for the next grip. This continues until no more abnormal height AprilTags are recognized, then it reports back to the large model and the task ends.
2.2.2. Task Plan
Plan the action function apriltag_remove_higher(30.0), where the parameter 30 indicates the height threshold. AprilTags with height above this value will be removed. Unit is mm.
2.2.3. Core Code Analysis
The apriltag_remove_higher 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
xxxxxxxxxxdef apriltag_remove_higher(self, target_high): # Remove AprilTags at specified height target_highf = float(target_high) / 100 #Robotic arm gripping program cmd1 = "ros2 run largemodel_arm grasp" #AprilTag height calculation program, pass parameter target_high (height threshold) cmd2 = f"ros2 run largemodel_arm apriltag_remove_higher --ros-args -p target_high:={target_highf:.2f}" subprocess.Popen( [ "gnome-terminal", "--title=grasp_desktop_remove", "--", "bash", "-c", f"{cmd1}; exec bash", ] ) subprocess.Popen( [ "gnome-terminal", "--title=apriltag_remove_higher", "--", "bash", "-c", f"{cmd2}; exec bash", ] )The grasp program was analyzed in the previous section and will not be repeated here. Now analyze the apriltag_remove_higher program. The source location of this program:
Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/llm/largemodel_arm/largemodel_arm/apriltag_remove_higher.py
RDK:/home/sunrise/dofbot_ultra_ws/src/llm/largemodel_arm/largemodel_arm/apriltag_remove_higher.py
xxxxxxxxxx#Get height threshold via command line parameterself.declare_parameter('target_high', 0.0)self.Target_height = self.get_parameter('target_high').get_parameter_value().double_value * 100#Height calculation function, three parameters xyz: xy is the center point coordinates of the AprilTag, z is the depth value of the center pointdef compute_heigh(self,x,y,z): #Start coordinate system transformation camera_location = self.pixel_to_camera_depth((x,y),z) PoseEndMat = np.matmul(self.EndToCamMat, self.xyz_euler_to_mat(camera_location, (0, 0, 0))) EndPointMat = self.get_end_point_mat() WorldPose = np.matmul(EndPointMat, PoseEndMat) #Convert rotation transformation matrix to xyz and Euler angles, get target pose, where pose_T[2] represents the height value pose_T, pose_R = self.mat_to_xyz_euler(WorldPose) pose_T[0] = pose_T[0] + self.x_offset pose_T[1] = pose_T[1] + self.y_offset pose_T[2] = pose_T[2] + self.z_offset return pose_T#Get the current AprilTag heightcompute_height = round(pose[2],4)*1000 heigh = 'heigh: ' + str(compute_height) + 'mm'cv.putText(result_image, heigh, (int(cx)+5, int(cy)-15), cv.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2)#If the current height value is greater than the height threshold, set self.detect_flag to true, then set self.compute_height to false to avoid recalculating height when the gripping action is completedif compute_height > self.Target_height and self.pubPos_flag == True and self.compute_height == True: print("Found the target.") self.found_cnt = self.found_cnt + 1 self.compute_height = False self.index = i self.detect_flag = True2.3. Grip by Color Block Height
2.3.1. Startup
After waking up the module, say:
xRemove green blocks with height higher than 3 cm
Then two terminals will open, starting the color block height calculation program and the gripping program respectively. After recognizing a green block with height above 3 cm, the gripping program will control the robotic arm to grip that block, place it at the set position, and finally return to the recognition pose to prepare for the next grip. This continues until no more abnormal height green blocks are recognized, then it reports back to the large model and the task ends.
2.2.2. Task Plan
Plan the action function color_remove_higher('green', 30.0), where 'green' indicates the color block to sort is green. The parameter 30 indicates the height threshold. Blocks with height above this value will be removed. Unit is mm.
2.2.3. Core Code Analysis
The color_remove_higher 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
xxxxxxxxxxdef color_remove_higher(self, color, target_high): arm_joints = [90, 110, 0, 0, 90, 0] self.pubSix_Arm(arm_joints) color = color.strip("'\"") # Remove single and double quotes target_highf = float(target_high) / 10 #Modify target_color value based on the passed color parameter, for passing to color_remove_higher if color == "red": target_color = float(1)