def distance_check_robot(self):
while self._running:
with self._lock:
try:
self.L2C = [''] * self.num_robot
#reset distance matrix
distance_matrix = -np.ones(
(self.num_robot, self.num_robot))
#update all robot joints
for i in range(self.num_robot):
robot_joints = self.robot_state_list[
i].InValue.joint_position
self.t_env.setState(self.robot_joint_list[i],
robot_joints)
#get distance check
env_state = self.t_env.getCurrentState()
self.manager.setCollisionObjectsTransform(
env_state.link_transforms)
contacts = self.manager.contactTest(2)
contact_vector = tesseract.flattenResults(contacts)
distances = np.array([c.distance for c in contact_vector])
nearest_points = np.array(
[c.nearest_points for c in contact_vector])
names = np.array([c.link_names for c in contact_vector])
for i in range(self.num_robot):
for j in range(i + 1, self.num_robot):
min_distance = 1
# min_distance_index=0
for m in range(len(distances)):
if (names[m][0] in self.robot_link_list[i] and
names[m][1] in self.robot_link_list[j]
) and distances[m] < min_distance:
min_distance = distances[m]
self.L2C[i] = names[m][0]
elif (names[m][0] in self.robot_link_list[j]
and names[m][1]
in self.robot_link_list[i]
) and distances[m] < min_distance:
min_distance = distances[m]
self.L2C[i] = names[m][1]
#update distance matrix
if min_distance != 1:
distance_matrix[i][j] = min_distance
distance_matrix[j][i] = min_distance
self.distance_matrix = distance_matrix.flatten()
except:
traceback.print_exc()
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)
def distance_check(self, robot_idx):
with self._lock:
distance_report = RRN.GetStructureType(
"edu.rpi.robotics.distance.distance_report")
distance_report1 = distance_report()
for i in range(self.num_robot):
robot_joints = self.robot_state_list[i].InValue.joint_position
self.t_env.setState(self.robot_joint_list[i], robot_joints)
env_state = self.t_env.getCurrentState()
self.manager.setCollisionObjectsTransform(
env_state.link_transforms)
contacts = self.manager.contactTest(2)
contact_vector = tesseract.flattenResults(contacts)
distances = np.array([c.distance for c in contact_vector])
nearest_points = np.array(
[c.nearest_points for c in contact_vector])
names = np.array([c.link_names for c in contact_vector])
# nearest_index=np.argmin(distances)
# print(names)
min_distance = 9
min_index = 0
Closest_Pt = [0., 0., 0.]
Closest_Pt_env = [0., 0., 0.]
#initialize
distance_report1.Closest_Pt = Closest_Pt
distance_report1.Closest_Pt_env = Closest_Pt_env
for i in range(len(distances)):
#only 1 in 2 collision "objects"
if (names[i][0] in self.robot_link_list[robot_idx]
or names[i][1] in self.robot_link_list[robot_idx]
) and distances[i] < min_distance and not (
names[i][0] in self.robot_link_list[robot_idx]
and names[i][1] in self.robot_link_list[robot_idx]):
min_distance = distances[i]
min_index = i
J2C = 0
if (len(distances) > 0):
if names[min_index][0] in self.robot_link_list[
robot_idx] and names[min_index][
1] in self.robot_link_list[robot_idx]:
stop = 1
elif names[min_index][0] in self.robot_link_list[robot_idx]:
J2C = self.robot_link_list[robot_idx].index(
names[min_index][0])
Closest_Pt = nearest_points[min_index][0]
Closest_Pt_env = nearest_points[min_index][1]
print(names[min_index])
print(distances[min_index])
elif names[min_index][1] in self.robot_link_list[robot_idx]:
J2C = self.robot_link_list[robot_idx].index(
names[min_index][1])
Closest_Pt = nearest_points[min_index][1]
Closest_Pt_env = nearest_points[min_index][0]
print(names[min_index])
print(distances[min_index])
distance_report1.Closest_Pt = np.float16(
Closest_Pt).flatten().tolist()
distance_report1.Closest_Pt_env = np.float16(
Closest_Pt_env).flatten().tolist()
distance_report1.min_distance = np.float16(
distances[min_index])
distance_report1.J2C = J2C
return distance_report1
return distance_report1
def distance_check(self, robot_idx, joints, dt):
with self._lock:
distance_report = RRN.GetStructureType(
"edu.rpi.robotics.distance.distance_report")
distance_report1 = distance_report()
#update sawyer's joints
robot_joints = self.robot_state_list[0].InValue.joint_position
# robot joints prediction
robot_joints += 0.2 * np.cos(
time.time() * np.ones(len(robot_joints), )) - 0.2 * np.cos(
(time.time() + dt) * np.ones(len(robot_joints), ))
self.t_env.setState(self.robot_joint_list[0], robot_joints)
#update this robot joints
self.t_env.setState(self.robot_joint_list[robot_idx], joints)
env_state = self.t_env.getCurrentState()
self.manager.setCollisionObjectsTransform(
env_state.link_transforms)
contacts = self.manager.contactTest(2)
contact_vector = tesseract.flattenResults(contacts)
distances = np.array([c.distance for c in contact_vector])
nearest_points = np.array(
[c.nearest_points for c in contact_vector])
names = np.array([c.link_names for c in contact_vector])
# nearest_index=np.argmin(distances)
min_distance = 9
min_index = -1
Closest_Pt = [0., 0., 0.]
Closest_Pt_env = [0., 0., 0.]
#initialize
distance_report1.Closest_Pt = Closest_Pt
distance_report1.Closest_Pt_env = Closest_Pt_env
for i in range(len(distances)):
#only 1 in 2 collision "objects"
if (names[i][0] in self.robot_link_list[robot_idx]
or names[i][1] in self.robot_link_list[robot_idx]
) and distances[i] < min_distance and not (
names[i][0] in self.robot_link_list[robot_idx]
and names[i][1] in self.robot_link_list[robot_idx]):
min_distance = distances[i]
min_index = i
J2C = 0
if (min_index != -1):
if names[min_index][0] in self.robot_link_list[
robot_idx] and names[min_index][
1] in self.robot_link_list[robot_idx]:
stop = 1
print("stop")
elif names[min_index][0] in self.robot_link_list[robot_idx]:
J2C = self.robot_link_list[robot_idx].index(
names[min_index][0])
Closest_Pt = nearest_points[min_index][0]
Closest_Pt_env = nearest_points[min_index][1]
print(names[min_index])
print(distances[min_index])
elif names[min_index][1] in self.robot_link_list[robot_idx]:
J2C = self.robot_link_list[robot_idx].index(
names[min_index][1])
Closest_Pt = nearest_points[min_index][1]
Closest_Pt_env = nearest_points[min_index][0]
print(names[min_index])
print(distances[min_index])
if robot_idx == 0:
J2C = self.Sawyer_link(J2C)
distance_report1.Closest_Pt = np.float16(
Closest_Pt).flatten().tolist()
distance_report1.Closest_Pt_env = np.float16(
Closest_Pt_env).flatten().tolist()
distance_report1.min_distance = np.float16(
distances[min_index])
distance_report1.J2C = J2C
return distance_report1
return distance_report1
def distance_check(self, robot_name):
with self._lock:
robot_idx = self.dict[robot_name]
distance_report = RRN.GetStructureType(
"edu.rpi.robotics.distance.distance_report")
distance_report1 = distance_report()
for i in range(self.num_robot):
wire_packet = self.robot_state_list[i].TryGetInValue()
#only update the ones online
if wire_packet[0]:
robot_joints = wire_packet[1].joint_position
if i == 0:
robot_joints[0] += np.pi #UR configuration
self.t_env.setState(self.robot_joint_list[i], robot_joints)
env_state = self.t_env.getCurrentState()
self.manager.setCollisionObjectsTransform(
env_state.link_transforms)
contacts = self.manager.contactTest(2)
contact_vector = tesseract.flattenResults(contacts)
distances = np.array([c.distance for c in contact_vector])
nearest_points = np.array(
[c.nearest_points for c in contact_vector])
names = np.array([c.link_names for c in contact_vector])
# nearest_index=np.argmin(distances)
min_distance = 9
min_index = -1
Closest_Pt = [0., 0., 0.]
Closest_Pt_env = [0., 0., 0.]
#initialize
distance_report1.Closest_Pt = Closest_Pt
distance_report1.Closest_Pt_env = Closest_Pt_env
for i in range(len(distances)):
#only 1 in 2 collision "objects"
if (names[i][0] in self.robot_link_list[robot_idx]
or names[i][1] in self.robot_link_list[robot_idx]
) and distances[i] < min_distance and not (
names[i][0] in self.robot_link_list[robot_idx]
and names[i][1] in self.robot_link_list[robot_idx]):
min_distance = distances[i]
min_index = i
J2C = 0
if (min_index != -1):
if names[min_index][0] in self.robot_link_list[
robot_idx] and names[min_index][
1] in self.robot_link_list[robot_idx]:
stop = 1
print("stop")
elif names[min_index][0] in self.robot_link_list[robot_idx]:
J2C = self.robot_link_list[robot_idx].index(
names[min_index][0])
Closest_Pt = nearest_points[min_index][0]
Closest_Pt_env = nearest_points[min_index][1]
print(names[min_index])
print(distances[min_index])
elif names[min_index][1] in self.robot_link_list[robot_idx]:
J2C = self.robot_link_list[robot_idx].index(
names[min_index][1])
Closest_Pt = nearest_points[min_index][1]
Closest_Pt_env = nearest_points[min_index][0]
print(names[min_index])
print(distances[min_index])
if robot_idx == 1:
J2C = self.Sawyer_link(J2C)
distance_report1.Closest_Pt = np.float16(
Closest_Pt).flatten().tolist()
distance_report1.Closest_Pt_env = np.float16(
Closest_Pt_env).flatten().tolist()
distance_report1.min_distance = np.float16(
distances[min_index])
distance_report1.J2C = J2C
return distance_report1
return distance_report1
def distance_check_robot(self):
while self._running:
with self._lock:
try:
#update robot joints
for i in range(self.num_robot):
wire_packet = self.robot_state_list[i].TryGetInValue()
#only update the ones online
if wire_packet[0]:
robot_joints = wire_packet[1].joint_position
if i == 0:
robot_joints[0] += np.pi #UR configuration
self.t_env.setState(self.robot_joint_list[i],
robot_joints)
env_state = self.t_env.getCurrentState()
self.manager.setCollisionObjectsTransform(
env_state.link_transforms)
contacts = self.manager.contactTest(2)
contact_vector = tesseract.flattenResults(contacts)
distances = np.array([c.distance for c in contact_vector])
nearest_points = np.array(
[c.nearest_points for c in contact_vector])
names = np.array([c.link_names for c in contact_vector])
# nearest_index=np.argmin(distances)
for robot_name, robot_idx in self.dict.items():
min_distance = 9
min_index = -1
Closest_Pt = [0., 0., 0.]
Closest_Pt_env = [0., 0., 0.]
J2C = 0
#initialize
self.distance_report_dict[
robot_name].Closest_Pt = Closest_Pt
self.distance_report_dict[
robot_name].Closest_Pt_env = Closest_Pt_env
for i in range(len(distances)):
#only 1 in 2 collision "objects"
if (names[i][0] in self.robot_link_list[robot_idx]
or names[i][1]
in self.robot_link_list[robot_idx]
) and distances[i] < min_distance and not (
names[i][0]
in self.robot_link_list[robot_idx]
and names[i][1]
in self.robot_link_list[robot_idx]):
min_distance = distances[i]
min_index = i
if (min_index != -1):
if names[min_index][0] in self.robot_link_list[
robot_idx] and names[min_index][
1] in self.robot_link_list[robot_idx]:
stop = 1
elif names[min_index][0] in self.robot_link_list[
robot_idx]:
J2C = self.robot_link_list[robot_idx].index(
names[min_index][0]) - 1
Closest_Pt = nearest_points[min_index][0]
Closest_Pt_env = nearest_points[min_index][1]
elif names[min_index][1] in self.robot_link_list[
robot_idx]:
J2C = self.robot_link_list[robot_idx].index(
names[min_index][1]) - 1
Closest_Pt = nearest_points[min_index][1]
Closest_Pt_env = nearest_points[min_index][0]
if robot_idx == 1:
J2C = self.Sawyer_link(J2C) - 1
self.distance_report_dict[
robot_name].Closest_Pt = np.float16(
Closest_Pt).flatten().tolist()
self.distance_report_dict[
robot_name].Closest_Pt_env = np.float16(
Closest_Pt_env).flatten().tolist()
self.distance_report_dict[
robot_name].min_distance = np.float16(
distances[min_index])
self.distance_report_dict[robot_name].J2C = (
J2C if J2C >= 0 else 0)
self.distance_report_wire.OutValue = self.distance_report_dict
except:
traceback.print_exc()