def main():
locator = GazeboModelResourceLocator()
env = Environment()
urdf_path = FilesystemPath("../urdf/combined.urdf")
srdf_path = FilesystemPath("../urdf/combined.srdf")
assert env.init(urdf_path, srdf_path, locator)
checker = env.getDiscreteContactManager()
contact_distance = 0.2
monitored_link_names = env.getLinkNames()
checker.setActiveCollisionObjects(monitored_link_names)
checker.setContactDistanceThreshold(contact_distance)
#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)
env.changeJointOrigin("ur_pose", Isometry3d(H_UR))
env.changeJointOrigin("sawyer_pose", Isometry3d(H_Sawyer))
env.changeJointOrigin("abb_pose", Isometry3d(H_ABB))
joint_angles = np.ones((6, ), dtype=np.float64) * 0.1
joint_angles[1] = 2
joint_angles[2] = 2
env.setState(ABB_joint_names, joint_angles)
env_state = env.getCurrentState()
checker.setCollisionObjectsTransform(env_state.link_transforms)
result = ContactResultMap()
checker.contactTest(result, ContactRequest(ContactTestType_ALL))
result_vector = ContactResultVector()
collisionFlattenResults(result, result_vector)
for r in result_vector:
print(r.link_names[0] + "," + r.link_names[1] + " " + str(r.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.H_robot = {
'ur': self.H_UR,
'sawyer': self.H_Sawyer,
'abb': self.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:
self.t_env = Environment()
urdf_path = FilesystemPath("urdf/combined.urdf")
srdf_path = FilesystemPath("urdf/combined.srdf")
assert self.t_env.init(urdf_path, srdf_path,
GazeboModelResourceLocator())
#update robot poses based on calibration file
# self.t_env.changeJointOrigin("ur_pose", Isometry3d(H_UR))
# self.t_env.changeJointOrigin("sawyer_pose", Isometry3d(H_Sawyer))
# self.t_env.changeJointOrigin("abb_pose", Isometry3d(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()
self.robot_def_dict = {}
try:
UR = self.ur_sub.GetDefaultClientWait(1)
self.robot_def_dict['ur'] = Robot(
np.transpose(np.array(UR.robot_info.chains[0].H.tolist())),
np.transpose(np.array(UR.robot_info.chains[0].P.tolist())),
np.zeros(len(UR.robot_info.joint_info)))
except:
pass
try:
Sawyer = self.sawyer_sub.GetDefaultClientWait(1)
self.robot_def_dict['sawyer'] = Robot(
np.transpose(np.array(Sawyer.robot_info.chains[0].H.tolist())),
np.transpose(np.array(Sawyer.robot_info.chains[0].P.tolist())),
np.zeros(len(Sawyer.robot_info.joint_info)))
except:
pass
try:
ABB = self.abb_sub.GetDefaultClientWait(1)
self.robot_def_dict['abb'] = Robot(
np.transpose(np.array(ABB.robot_info.chains[0].H.tolist())),
np.transpose(np.array(ABB.robot_info.chains[0].P.tolist())),
np.zeros(len(ABB.robot_info.joint_info)))
except:
pass
#trajectories
self.traj_change = False
self.traj_change_name = None
self.steps = 300
self.plan_time = 0.15
self.execution_delay = 0.03
self.trajectory = {
'ur': np.zeros((self.steps, 7)),
'sawyer': np.zeros((self.steps, 8)),
'abb': np.zeros((self.steps, 7))
}
self.traj_joint_names = {
'ur': [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
],
'sawyer': [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
],
'abb':
['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
}
self.time_step = 0.03
#initialize static trajectories
for key, value in self.trajectory.items():
for i in range(self.steps):
try:
value[i] = np.append([0], self.robot_state_list[
self.dict[key]].InValue.joint_position)
except: #incase robot not on
value[i] = np.append(
[0], [0] * len(self.robot_joint_list[self.dict[key]]))
self.inv = {'ur': inv_ur, 'sawyer': inv_sawyer, 'abb': inv_abb}
self.joint_names_traj = {
'ur': inv_ur,
'sawyer': inv_sawyer,
'abb': inv_abb
}
#register service constant
self.JointTrajectoryWaypoint = RRN.GetStructureType(
"com.robotraconteur.robotics.trajectory.JointTrajectoryWaypoint")
self.JointTrajectory = RRN.GetStructureType(
"com.robotraconteur.robotics.trajectory.JointTrajectory")
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 = [
"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"
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()
#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']
robot_state_list=[UR_state,Sawyer_state,ABB_state]
robot_link_list=[UR_link_names,Sawyer_link_names,ABB_link_names]
robot_joint_list=[UR_joint_names,Sawyer_joint_names,ABB_joint_names]
num_robot=len(robot_state_list)
robot_dict={'ur':0,'sawyer':1,'abb':2}
locator = GazeboModelResourceLocator()
env = Environment()
urdf_path = FilesystemPath("../urdf/combined.urdf")
srdf_path = FilesystemPath("../urdf/combined.srdf")
assert env.init(urdf_path, srdf_path, locator)
checker = env.getDiscreteContactManager()
contact_distance=0.2
monitored_link_names = env.getLinkNames()
checker.setActiveCollisionObjects(monitored_link_names)
checker.setContactDistanceThreshold(contact_distance)
#load calibration parameters
with open(workspace_path+'calibration/Sawyer2.yaml') as file:
def __init__(self):
self._lock=threading.RLock()
self._running=False
#load calibration parameters
with open('calibration/Sawyer2.yaml') as file:
H_Sawyer = np.array(yaml.load(file)['H'],dtype=np.float64)
with open('calibration/UR2.yaml') as file:
H_UR = np.array(yaml.load(file)['H'],dtype=np.float64)
with open('calibration/ABB2.yaml') as file:
H_ABB = np.array(yaml.load(file)['H'],dtype=np.float64)
with open('calibration/tx60.yaml') as file:
H_tx60 = 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.H_tx60=H42H3(H_tx60)
self.H_robot={'ur':self.H_UR,'sawyer':self.H_Sawyer,'abb':self.H_ABB,'staubli':self.H_tx60}
self.distance_report=RRN.GetStructureType("edu.rpi.robotics.distance.distance_report")
self.dict={'ur':0,'sawyer':1,'abb':2,'staubli':3}
self.distance_report_dict={}
for robot_name,robot_idx in self.dict.items():
self.distance_report_dict[robot_name]=self.distance_report()
#connect to RR gazebo plugin service
server=RRN.ConnectService('rr+tcp://localhost:11346/?service=GazeboServer')
self.w=server.get_worlds(str(server.world_names[0]))
#create RR pose type
pose_dtype = RRN.GetNamedArrayDType("com.robotraconteur.geometry.Pose", server)
self.model_pose = np.zeros((1,), dtype = pose_dtype)
#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
transformation4=self.transformation()
transformation4.name="Staubli"
transformation4.row=len(self.H_tx60)
transformation4.column=len(self.H_tx60[0])
transformation4.H=np.float16(self.H_tx60).flatten().tolist()
self.transformations['staubli']=transformation4
#Connect to robot service
UR = RRN.ConnectService('rr+tcp://localhost:58653?service=robot')
Sawyer= RRN.ConnectService('rr+tcp://localhost:58654?service=robot')
ABB= RRN.ConnectService('rr+tcp://localhost:58655?service=robot')
tx60= RRN.ConnectService('rr+tcp://localhost:58656?service=robot')
UR_state=UR.robot_state.Connect()
Sawyer_state=Sawyer.robot_state.Connect()
ABB_state=ABB.robot_state.Connect()
tx60_state=tx60.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"]
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"]
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']
tx60_joint_names=['tx60_joint_1','tx60_joint_2','tx60_joint_3','tx60_joint_4','tx60_joint_5','tx60_joint_6']
tx60_link_names=['tx60_base_link','tx60_link_1','tx60_link_2','tx60_link_3','tx60_link_4','tx60_link_5','tx60_link_6']
self.robot_list=[UR,Sawyer,ABB,tx60]
self.robot_state_list=[UR_state,Sawyer_state,ABB_state,tx60_state]
self.robot_link_list=[UR_link_names,Sawyer_link_names,ABB_link_names,tx60_link_names]
self.robot_joint_list=[UR_joint_names,Sawyer_joint_names,ABB_joint_names,tx60_joint_names]
self.num_robot=len(self.robot_state_list)
######tesseract environment setup:
self.t_env = Environment()
urdf_path = FilesystemPath("urdf/combined.urdf")
srdf_path = FilesystemPath("urdf/combined.srdf")
assert self.t_env.init(urdf_path, srdf_path, GazeboModelResourceLocator())
#update robot poses based on calibration file
self.t_env.changeJointOrigin("ur_pose", Isometry3d(H_UR))
self.t_env.changeJointOrigin("sawyer_pose", Isometry3d(H_Sawyer))
self.t_env.changeJointOrigin("abb_pose", Isometry3d(H_ABB))
self.t_env.changeJointOrigin("staubli_pose", Isometry3d(H_tx60))
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()
self.robot_def_dict={'ur':Robot(np.transpose(np.array(UR.robot_info.chains[0].H.tolist())),np.transpose(np.array(UR.robot_info.chains[0].P.tolist())),np.zeros(len(UR.robot_info.joint_info))),
'sawyer':Robot(np.transpose(np.array(Sawyer.robot_info.chains[0].H.tolist())),np.transpose(np.array(Sawyer.robot_info.chains[0].P.tolist())),np.zeros(len(Sawyer.robot_info.joint_info))),
'abb':Robot(np.transpose(np.array(ABB.robot_info.chains[0].H.tolist())),np.transpose(np.array(ABB.robot_info.chains[0].P.tolist())),np.zeros(len(ABB.robot_info.joint_info))),
'staubli':Robot(np.transpose(np.array(tx60.robot_info.chains[0].H.tolist())),np.transpose(np.array(tx60.robot_info.chains[0].P.tolist())),np.zeros(len(tx60.robot_info.joint_info)))
}
#trajectories
self.traj_change=False
self.traj_change_name=None
self.steps=300
self.plan_time=0.15
self.execution_delay=0.03
self.trajectory={'ur':np.zeros((self.steps,7)),'sawyer':np.zeros((self.steps,8)),'abb':np.zeros((self.steps,7)),'staubli':np.zeros((self.steps,7))}
self.traj_joint_names={'ur':['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'],
'sawyer':['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6'],
'abb':['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'],
'staubli':['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
}
self.time_step=0.03
#initialize static trajectories
for key, value in self.trajectory.items():
for i in range(self.steps):
try:
value[i]=np.append([0],self.robot_state_list[self.dict[key]].InValue.joint_position)
except:
traceback.print_exc()
value[i]=np.append([0],[0,0,0,0,0,0])
self.inv={'ur':inv_ur,'sawyer':inv_sawyer,'abb':inv_abb,'staubli':inv_staubli}
self.joint_names_traj={'ur':inv_ur,'sawyer':inv_sawyer,'abb':inv_abb,'staubli':inv_staubli}
#register service constant
self.JointTrajectoryWaypoint = RRN.GetStructureType("com.robotraconteur.robotics.trajectory.JointTrajectoryWaypoint")
self.JointTrajectory = RRN.GetStructureType("com.robotraconteur.robotics.trajectory.JointTrajectory")