def test_inv_kin(): TESSERACT_SUPPORT_DIR = os.environ["TESSERACT_SUPPORT_DIR"] with open(os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "abb_irb2400.urdf"), 'r') as f: abb_irb2400_urdf = f.read() with open(os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "abb_irb2400.srdf"), 'r') as f: abb_irb2400_srdf = f.read() t = tesseract.Tesseract() t.init(abb_irb2400_urdf, abb_irb2400_srdf, TesseractSupportResourceLocator()) tcp_pose = np.array([[0, 0, 1, 0.8], [0, 1, 0, 0], [-1, 0, 0, 1.2], [0, 0, 0, 1]]) inv_kin_manager = t.getInvKinematicsManager() inv_kin = inv_kin_manager.getInvKinematicSolver("manipulator") joint_angles = inv_kin.calcInvKin(tcp_pose, np.ones(6) * 0.25) # Now check fwd_kin fwd_kin_manager = t.getFwdKinematicsManager() fwd_kin = fwd_kin_manager.getFwdKinematicSolver("manipulator") tcp_pose2 = fwd_kin.calcFwdKin(joint_angles) np.testing.assert_allclose(tcp_pose[0:3, 3], tcp_pose2[0:3, 3], atol=1e-5)
def test_environment(): TESSERACT_SUPPORT_DIR = os.environ["TESSERACT_SUPPORT_DIR"] with open(os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "abb_irb2400.urdf"), "r") as f: abb_irb2400_urdf = f.read() with open(os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "abb_irb2400.srdf"), "r") as f: abb_irb2400_srdf = f.read() t = tesseract.Tesseract() t.init(abb_irb2400_urdf, abb_irb2400_srdf, TesseractSupportResourceLocator()) t_srdf = t.getSRDFModel() print("robot name: " + str(t_srdf.getName())) assert str(t_srdf.getName()) == "abb_irb2400" chain_groups = t_srdf.getChainGroups() assert len(chain_groups) == 1 group = chain_groups["manipulator"] assert (len(group) == 1) assert group[0] == ("base_link", "tool0") # Group state now group_states = t_srdf.getGroupStates() assert len(group_states) == 1 group_state = group_states["manipulator"] state = group_state["all-zeros"] for i in range(1, 7): assert state["joint_{}".format(i)] == 0.0
def test_fwd_kin(): TESSERACT_SUPPORT_DIR = os.environ["TESSERACT_SUPPORT_DIR"] with open(os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "abb_irb2400.urdf"), 'r') as f: abb_irb2400_urdf = f.read() with open(os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "abb_irb2400.srdf"), 'r') as f: abb_irb2400_srdf = f.read() t = tesseract.Tesseract() t.init(abb_irb2400_urdf, abb_irb2400_srdf, TesseractSupportResourceLocator()) fwd_kin_manager = t.getFwdKinematicsManager() fwd_kin = fwd_kin_manager.getFwdKinematicSolver("manipulator") tcp_pose = fwd_kin.calcFwdKin(np.deg2rad([15, -30, -20, 5, 10, -90])) np.testing.assert_allclose( tcp_pose, [[0.32232436, 0.59589736, 0.73553609, 0.1875004], [-0.94497009, 0.24852968, 0.21275462, 0.05157239], [-0.05602263, -0.7636356, 0.64321235, 1.94536084], [0., 0., 0., 1.]])
def test_environment(): TESSERACT_SUPPORT_DIR = os.environ["TESSERACT_SUPPORT_DIR"] with open(os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "abb_irb2400.urdf"), 'r') as f: abb_irb2400_urdf = f.read() with open(os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "abb_irb2400.srdf"), 'r') as f: abb_irb2400_srdf = f.read() t = tesseract.Tesseract() t.init(abb_irb2400_urdf, abb_irb2400_srdf, TesseractSupportResourceLocator()) t_env = t.getEnvironment() assert sorted(t_env.getJointNames()) == sorted( ('base_link-base', 'joint_6-tool0', 'joint_2', 'joint_1', 'joint_5', 'joint_4', 'joint_3', 'joint_6')) assert sorted(t_env.getLinkNames()) == sorted( ('base', 'tool0', 'link_1', 'base_link', 'link_2', 'link_4', 'link_6', 'link_3', 'link_5')) assert t_env.getRootLinkName() == 'base_link' l = t_env.getLink('link_4') assert l.getName() == 'link_4' assert len(l.visual) == 1 assert len(l.collision) == 1 visual = l.visual[0] collision = l.collision[0] np.testing.assert_allclose(visual.origin, np.eye(4)) assert len(visual.geometry.getVertices()) == 631 assert len(visual.geometry.getTriangles()) == 5032 np.testing.assert_allclose(visual.material.color.flatten(), [0.7372549, 0.3490196, 0.1607843, 1], rtol=1e-4) np.testing.assert_allclose(collision.origin, np.eye(4)) j = t_env.getJoint('joint_3') assert j.type == 1 assert j.getName() == 'joint_3' assert j.parent_link_name == 'link_2' assert j.child_link_name == 'link_3' np.testing.assert_allclose(j.axis.flatten(), [0, 1, 0]) j_origin = np.eye(4) j_origin[2, 3] = 0.705 np.testing.assert_allclose(j.parent_to_joint_origin_transform, j_origin) assert j.limits.lower == -1.0472 assert j.limits.upper == 1.1345 assert j.limits.velocity == 2.618 assert j.limits.effort == 0
def _load_tesseract(): TESSERACT_SUPPORT_DIR = os.environ["TESSERACT_SUPPORT_DIR"] with open( os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "lbr_iiwa_14_r820.urdf"), 'r') as f: robot_urdf = f.read() with open( os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "lbr_iiwa_14_r820.srdf"), 'r') as f: robot_srdf = f.read() t = tesseract.Tesseract() t.init(robot_urdf, robot_srdf, TesseractSupportResourceLocator()) return t
def test_environment(): TESSERACT_SUPPORT_DIR = os.environ["TESSERACT_SUPPORT_DIR"] with open(os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "abb_irb2400.urdf"), "r") as f: abb_irb2400_urdf = f.read() with open(os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "abb_irb2400.srdf"), "r") as f: abb_irb2400_srdf = f.read() t = tesseract.Tesseract() t.init(abb_irb2400_urdf, abb_irb2400_srdf, TesseractSupportResourceLocator()) t_srdf = t.getSRDFModel() assert t_srdf.getName() == "abb_irb2400" groups = t_srdf.getGroups() assert len(groups) == 1 group = groups[0] assert group.name_ == "manipulator" # Group definition contains no joints, no links, no subgroups, only a chain assert not group.joints_ assert not group.links_ assert not group.subgroups_ assert len(group.chains_) == 1 assert group.chains_[0] == ("base_link", "tool0") # Group state now group_states = t_srdf.getGroupStates() assert len(group_states) == 1 group_state = group_states[0] assert group_state.name_ == "all-zeros" assert group_state.group_ == "manipulator" for i in range(1, 7): assert group_state.joint_values_["joint_{}".format(i)] == (0.0, )
def test_environment(): TESSERACT_SUPPORT_DIR = os.environ["TESSERACT_SUPPORT_DIR"] with open(os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "abb_irb2400.urdf"), 'r') as f: abb_irb2400_urdf = f.read() with open(os.path.join(TESSERACT_SUPPORT_DIR, "urdf", "abb_irb2400.srdf"), 'r') as f: abb_irb2400_srdf = f.read() t = tesseract.Tesseract() t.init(abb_irb2400_urdf, abb_irb2400_srdf, TesseractSupportResourceLocator()) t_env = t.getEnvironment() t_env.setState(["joint_3"], np.array([1.6])) contact_distance = 0.2 monitored_link_names = t_env.getLinkNames() manager = t_env.getDiscreteContactManager() manager.setActiveCollisionObjects(monitored_link_names) manager.setContactDistanceThreshold(contact_distance) env_state = t_env.getCurrentState() manager.setCollisionObjectsTransform(env_state.link_transforms) contacts = manager.contactTest(2) contact_vector = tesseract.flattenResults(contacts) assert len(contact_vector) == 3 assert contact_vector[0].link_names == ('link_1', 'link_4') assert contact_vector[0].shape_id == (0, 0) assert contact_vector[0].nearest_points[0].shape == (3, 1) assert contact_vector[0].nearest_points[1].shape == (3, 1)
import numpy as np import time import sys import yaml sys.path.append('../../') from gazebo_model_resource_locator import GazeboModelResourceLocator with open("combined.urdf", 'r') as f: combined_urdf = f.read() with open("combined.srdf", 'r') as f: combined_srdf = f.read() pose = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], dtype=np.float64) t = tesseract.Tesseract() t.init(combined_urdf, combined_srdf, GazeboModelResourceLocator()) t_env = t.getEnvironment() viewer = TesseractViewer() viewer.update_environment(t_env, [0, 0, 0]) #link and joint names in urdf Sawyer_joint_names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] Sawyer_link_names = [
def __init__(self): self._lock = threading.RLock() self._running = False self.num_robot = 8 #load calibration parameters with open("calibration/UR1.yaml") as file: H_UR1 = np.array(yaml.load(file)["H"], dtype=np.float64) with open("calibration/UR2.yaml") as file: H_UR2 = np.array(yaml.load(file)["H"], dtype=np.float64) with open("calibration/UR3.yaml") as file: H_UR3 = np.array(yaml.load(file)["H"], dtype=np.float64) with open("calibration/UR4.yaml") as file: H_UR4 = np.array(yaml.load(file)["H"], dtype=np.float64) with open("calibration/ABB1.yaml") as file: H_ABB1 = np.array(yaml.load(file)["H"], dtype=np.float64) with open("calibration/ABB2.yaml") as file: H_ABB2 = np.array(yaml.load(file)["H"], dtype=np.float64) with open("calibration/ABB3.yaml") as file: H_ABB3 = np.array(yaml.load(file)["H"], dtype=np.float64) with open("calibration/ABB4.yaml") as file: H_ABB4 = np.array(yaml.load(file)["H"], dtype=np.float64) H_list = [ H42H3(H_UR1), H42H3(H_UR2), H42H3(H_UR3), H42H3(H_UR4), H42H3(H_ABB1), H42H3(H_ABB2), H42H3(H_ABB3), H42H3(H_ABB4) ] transformation = RRN.GetStructureType( "edu.rpi.robotics.distance.transformation") self.transformations = [] self.robot_link_list = [] self.robot_joint_list = [] for i in range(self.num_robot): #form H into RR transformation struct transformation1 = transformation() transformation1.name = "robot" + str(i) transformation1.row = len(H_list[i]) transformation1.column = len(H_list[i][0]) transformation1.H = np.float16(H_list[i]).flatten().tolist() self.transformations.append(transformation1) if i < 4: UR_joint_names = [ "UR" + str(i + 1) + "_shoulder_pan_joint", "UR" + str(i + 1) + "_shoulder_lift_joint", "UR" + str(i + 1) + "_elbow_joint", "UR" + str(i + 1) + "_wrist_1_joint", "UR" + str(i + 1) + "_wrist_2_joint", "UR" + str(i + 1) + "_wrist_3_joint" ] UR_link_names = [ "UR" + str(i + 1) + "_base_link", "UR" + str(i + 1) + "_shoulder_link", "UR" + str(i + 1) + "_upper_arm_link", "UR" + str(i + 1) + "_forearm_link", "UR" + str(i + 1) + "_wrist_1_link", "UR" + str(i + 1) + "_wrist_2_link", "UR" + str(i + 1) + "_wrist_3_link" ] self.robot_link_list.append(UR_link_names) self.robot_joint_list.append(UR_joint_names) else: ABB_joint_names = [ "ABB" + str(i - 3) + "_joint_1", "ABB" + str(i - 3) + "_joint_2", "ABB" + str(i - 3) + "_joint_3", "ABB" + str(i - 3) + "_joint_4", "ABB" + str(i - 3) + "_joint_5", "ABB" + str(i - 3) + "_joint_6" ] ABB_link_names = [ "ABB" + str(i - 3) + "_base_link", "ABB" + str(i - 3) + "_link_1", "ABB" + str(i - 3) + "_link_2", "ABB" + str(i - 3) + "_link_3", "ABB" + str(i - 3) + "_link_4", "ABB" + str(i - 3) + "_link_5", "ABB" + str(i - 3) + "_link_6" ] self.robot_link_list.append(ABB_link_names) self.robot_joint_list.append(ABB_joint_names) #Connect to robot service UR1 = RRN.ConnectService("rr+tcp://localhost:55555?service=robot") UR2 = RRN.ConnectService("rr+tcp://localhost:55556?service=robot") UR3 = RRN.ConnectService("rr+tcp://localhost:55557?service=robot") UR4 = RRN.ConnectService("rr+tcp://localhost:55558?service=robot") ABB1 = RRN.ConnectService("rr+tcp://localhost:11111?service=robot") ABB2 = RRN.ConnectService("rr+tcp://localhost:11112?service=robot") ABB3 = RRN.ConnectService("rr+tcp://localhost:11113?service=robot") ABB4 = RRN.ConnectService("rr+tcp://localhost:11114?service=robot") UR1_state = UR1.robot_state.Connect() UR2_state = UR2.robot_state.Connect() UR3_state = UR3.robot_state.Connect() UR4_state = UR4.robot_state.Connect() ABB1_state = ABB1.robot_state.Connect() ABB2_state = ABB2.robot_state.Connect() ABB3_state = ABB3.robot_state.Connect() ABB4_state = ABB4.robot_state.Connect() self.robot_state_list = [ UR1_state, UR2_state, UR3_state, UR4_state, ABB1_state, ABB2_state, ABB3_state, ABB4_state ] self.L2C = [''] * self.num_robot self.distance_matrix = -np.ones(self.num_robot * self.num_robot) ######tesseract environment setup: with open("urdf/combined.urdf", "r") as f: combined_urdf = f.read() with open("urdf/combined.srdf", "r") as f: combined_srdf = f.read() t = tesseract.Tesseract() t.init(combined_urdf, combined_srdf, GazeboModelResourceLocator()) self.t_env = t.getEnvironment() #update robot poses based on calibration file self.t_env.changeJointOrigin("UR1_pose", H_UR1) self.t_env.changeJointOrigin("UR2_pose", H_UR2) self.t_env.changeJointOrigin("UR3_pose", H_UR3) self.t_env.changeJointOrigin("UR4_pose", H_UR4) self.t_env.changeJointOrigin("ABB1_pose", H_ABB1) self.t_env.changeJointOrigin("ABB2_pose", H_ABB2) self.t_env.changeJointOrigin("ABB3_pose", H_ABB3) self.t_env.changeJointOrigin("ABB4_pose", H_ABB4) contact_distance = 1.5 monitored_link_names = self.t_env.getLinkNames() self.manager = self.t_env.getDiscreteContactManager() self.manager.setActiveCollisionObjects(monitored_link_names) self.manager.setContactDistanceThreshold(contact_distance)
def __init__(self): self._lock = threading.RLock() self._running = False #load calibration parameters with open('calibration/Sawyer.yaml') as file: H_Sawyer = np.array(yaml.load(file)['H'], dtype=np.float64) with open('calibration/ABB.yaml') as file: H_ABB = np.array(yaml.load(file)['H'], dtype=np.float64) self.H_Sawyer = H42H3(H_Sawyer) self.H_ABB = H42H3(H_ABB) self.L2C = ['', ''] #form H into RR transformation struct transformation = RRN.GetStructureType( "edu.rpi.robotics.distance.transformation") transformation1 = transformation() transformation1.name = "Sawyer" transformation1.row = len(self.H_Sawyer) transformation1.column = len(self.H_Sawyer[0]) transformation1.H = np.float16(self.H_Sawyer).flatten().tolist() transformation2 = transformation() transformation2.name = "ABB" transformation2.row = len(self.H_ABB) transformation2.column = len(self.H_ABB[0]) transformation2.H = np.float16(self.H_ABB).flatten().tolist() self.transformations = [transformation1, transformation2] #Connect to robot service Sawyer = RRN.ConnectService('rr+tcp://localhost:58654?service=robot') ABB = RRN.ConnectService('rr+tcp://localhost:58655?service=robot') Sawyer_state = Sawyer.robot_state.Connect() ABB_state = ABB.robot_state.Connect() #link and joint names in urdf Sawyer_joint_names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] Sawyer_link_names = [ "right_l0", "right_l1", "right_l2", "right_l3", "right_l4", "right_l5", "right_l6", "right_l1_2", "right_l2_2", "right_l4_2", "right_hand" ] ABB_joint_names = [ 'ABB1200_joint_1', 'ABB1200_joint_2', 'ABB1200_joint_3', 'ABB1200_joint_4', 'ABB1200_joint_5', 'ABB1200_joint_6' ] ABB_link_names = [ 'ABB1200_base_link', 'ABB1200_link_1', 'ABB1200_link_2', 'ABB1200_link_3', 'ABB1200_link_4', 'ABB1200_link_5', 'ABB1200_link_6' ] self.robot_state_list = [Sawyer_state, ABB_state] self.robot_link_list = [Sawyer_link_names, ABB_link_names] self.robot_joint_list = [Sawyer_joint_names, ABB_joint_names] self.num_robot = len(self.robot_state_list) self.distance_matrix = -np.ones(self.num_robot * self.num_robot) ######tesseract environment setup: with open("urdf/combined.urdf", 'r') as f: combined_urdf = f.read() with open("urdf/combined.srdf", 'r') as f: combined_srdf = f.read() t = tesseract.Tesseract() t.init(combined_urdf, combined_srdf, GazeboModelResourceLocator()) self.t_env = t.getEnvironment() #update robot poses based on calibration file self.t_env.changeJointOrigin("sawyer_pose", H_Sawyer) self.t_env.changeJointOrigin("abb_pose", H_ABB) contact_distance = 0.1 monitored_link_names = self.t_env.getLinkNames() self.manager = self.t_env.getDiscreteContactManager() self.manager.setActiveCollisionObjects(monitored_link_names) self.manager.setContactDistanceThreshold(contact_distance)
def __init__(self): self._lock = threading.RLock() self._running = False #load calibration parameters with open('calibration/Sawyer.yaml') as file: H_Sawyer = np.array(yaml.load(file)['H'], dtype=np.float64) with open('calibration/ur.yaml') as file: H_UR = np.array(yaml.load(file)['H'], dtype=np.float64) with open('calibration/abb.yaml') as file: H_ABB = np.array(yaml.load(file)['H'], dtype=np.float64) self.H_UR = H42H3(H_UR) self.H_Sawyer = H42H3(H_Sawyer) self.H_ABB = H42H3(H_ABB) self.distance_report = RRN.GetStructureType( "edu.rpi.robotics.distance.distance_report") self.dict = {'ur': 0, 'sawyer': 1, 'abb': 2} self.distance_report_dict = {} for robot_name, robot_idx in self.dict.items(): self.distance_report_dict[robot_name] = self.distance_report() #form H into RR transformation struct self.transformations = {} self.transformation = RRN.GetStructureType( "edu.rpi.robotics.distance.transformation") transformation1 = self.transformation() transformation1.name = "UR" transformation1.row = len(self.H_UR) transformation1.column = len(self.H_UR[0]) transformation1.H = np.float16(self.H_UR).flatten().tolist() self.transformations['ur'] = transformation1 transformation2 = self.transformation() transformation2.name = "Sawyer" transformation2.row = len(self.H_Sawyer) transformation2.column = len(self.H_Sawyer[0]) transformation2.H = np.float16(self.H_Sawyer).flatten().tolist() self.transformations['sawyer'] = transformation2 transformation3 = self.transformation() transformation3.name = "ABB" transformation3.row = len(self.H_ABB) transformation3.column = len(self.H_ABB[0]) transformation3.H = np.float16(self.H_ABB).flatten().tolist() self.transformations['abb'] = transformation3 #Connect to robot service with open('client_yaml/client_ur.yaml') as file: self.url_ur = yaml.load(file)['url'] with open('client_yaml/client_sawyer.yaml') as file: self.url_sawyer = yaml.load(file)['url'] with open('client_yaml/client_abb.yaml') as file: self.url_abb = yaml.load(file)['url'] self.ur_sub = RRN.SubscribeService(self.url_ur) self.sawyer_sub = RRN.SubscribeService(self.url_sawyer) self.abb_sub = RRN.SubscribeService(self.url_abb) self.ur_sub.ClientConnectFailed += self.connect_failed self.sawyer_sub.ClientConnectFailed += self.connect_failed self.abb_sub.ClientConnectFailed += self.connect_failed UR_state = self.ur_sub.SubscribeWire("robot_state") Sawyer_state = self.sawyer_sub.SubscribeWire("robot_state") ABB_state = self.abb_sub.SubscribeWire("robot_state") #link and joint names in urdf Sawyer_joint_names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] UR_joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] Sawyer_link_names = [ "right_l0", "right_l1", "right_l2", "right_l3", "right_l4", "right_l5", "right_l6", "right_l1_2", "right_l2_2", "right_l4_2", "right_hand" ] UR_link_names = [ 'UR_base_link', "shoulder_link", "upper_arm_link", "forearm_link", "wrist_1_link", "wrist_2_link", "wrist_3_link", "UR_link_7" ] ABB_joint_names = [ 'ABB1200_joint_1', 'ABB1200_joint_2', 'ABB1200_joint_3', 'ABB1200_joint_4', 'ABB1200_joint_5', 'ABB1200_joint_6' ] ABB_link_names = [ 'ABB1200_base_link', 'ABB1200_link_1', 'ABB1200_link_2', 'ABB1200_link_3', 'ABB1200_link_4', 'ABB1200_link_5', 'ABB1200_link_6', 'ABB1200_link_7' ] self.robot_state_list = [UR_state, Sawyer_state, ABB_state] self.robot_link_list = [ UR_link_names, Sawyer_link_names, ABB_link_names ] self.robot_joint_list = [ UR_joint_names, Sawyer_joint_names, ABB_joint_names ] self.num_robot = len(self.robot_state_list) ######tesseract environment setup: with open("urdf/combined.urdf", 'r') as f: combined_urdf = f.read() with open("urdf/combined.srdf", 'r') as f: combined_srdf = f.read() t = tesseract.Tesseract() t.init(combined_urdf, combined_srdf, GazeboModelResourceLocator()) self.t_env = t.getEnvironment() #update robot poses based on calibration file self.t_env.changeJointOrigin("ur_pose", H_UR) self.t_env.changeJointOrigin("sawyer_pose", H_Sawyer) self.t_env.changeJointOrigin("abb_pose", H_ABB) contact_distance = 0.2 monitored_link_names = self.t_env.getLinkNames() self.manager = self.t_env.getDiscreteContactManager() self.manager.setActiveCollisionObjects(monitored_link_names) self.manager.setContactDistanceThreshold(contact_distance) # viewer update self.viewer = TesseractViewer() self.viewer.update_environment(self.t_env, [0, 0, 0]) self.viewer.start_serve_background()
import cv2 import numpy as np # import matplotlib.pyplot as plt # import tesserocr # import PIL.Image import tesseract import os TESSERACT_DIR = 'D:\Program Files\Tesseract-OCR' temp = os.environ['Path'] os.environ['Path'] = f'{TESSERACT_DIR};' + os.environ['Path'] tess = tesseract.Tesseract(language=b'eng', datapath=bytes(f'{TESSERACT_DIR}\\tessdata', 'utf-8'), lib_path=f'{TESSERACT_DIR}\libtesseract-5.dll') os.environ['Path'] = temp def number_recog(roi): """ Image size of 720p is required. """ number_area = roi[85:115, 45:115, :] number_area_gray = cv2.cvtColor(number_area, cv2.COLOR_BGR2GRAY) threshed = cv2.adaptiveThreshold(number_area_gray, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 23, -45) contours, _ = cv2.findContours(threshed, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) valid_box = [] for ct in contours: if cv2.contourArea(ct) > 10: x, y, w, h = cv2.boundingRect(ct) if 10 / 45 < w / h and w / h < 35 / 45 and 12 < h and h < 18 and abs(y + h // 2 - 15) < 5: valid_box.append((x, y, w, h)) valid_box = sorted(valid_box, key=lambda x: x[0]) valid_box_2 = [] for box in valid_box: