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:
