def CheckCollisionConfig(self, config, activeIndices, vnear=None):
"""Check for both virtual and actual collisions.
"""
inCollision = False
positions = [
self.robots[i].Locate(config[i])[0] for i in activeIndices
]
# Check for virtual collisions (trapped in deadlocks).
a = self._a
b = self._b
for (iconflict, conflict) in enumerate(self.conflicts):
if conflict.type != ConflictType.DEADLOCK:
continue
if not set(conflict.indices).issubset(set(activeIndices)):
continue
indices = conflict.indices
svect = Config([config[rindex] for rindex in indices])
# svect_start = Config([conflict.intervals[rindex][0] - a
# for rindex in indices])
# svect_end = Config([conflict.intervals[rindex][1] + b
# for rindex in indices])
svect_start = Config(
[conflict.intervals[rindex][0] for rindex in indices])
svect_end = Config(
[conflict.intervals[rindex][1] for rindex in indices])
if svect.Dominates(svect_start) and svect_end.Dominates(svect):
msg = " Config in deadlock {0}: Robot {1}".format\
(iconflict, indices[0])
for index in indices[1:]:
msg += " & Robot {0}".format(index)
log.debug(msg)
inCollision = True
if vnear is not None:
log.debug(" vnear.index = {0}".format(vnear.index))
return inCollision
# Check for actual collisions
for i in xrange(len(activeIndices)):
index1 = activeIndices[i]
for j in xrange(i + 1, len(activeIndices)):
index2 = activeIndices[j]
diff = positions[i] - positions[j]
dist = np.sqrt(np.dot(diff, diff))
if dist <= 2 * self.params.collisionRadius:
log.debug(" Config in collision: Robot {0} & Robot {1}".
format(index1, index2))
if vnear is not None:
log.debug(" vnear.index = {0}".format(vnear.index))
inCollision = True
if vnear is not None:
if index1 in vnear.waitingIndices or index2 in vnear.waitingIndices:
# If one of the colliding robot is a waiting
# robot, the other robot should also wait.
if index1 in vnear.waitingIndices:
vnear.waitingIndices.append(index2)
elif index2 in vnear.waitingIndices:
vnear.waitingIndices.append(index1)
else:
for conflict in self.conflicts:
# if conflict.type != ConflictType.SHARED:
# continue
# Consider both shared segments and junctions.
if conflict.type == ConflictType.DEADLOCK:
continue
if (not (index1 in conflict.indices
and index2 in conflict.indices)):
continue
s1_start, s1_end = conflict.intervals[index1]
s2_start, s2_end = conflict.intervals[index2]
s1 = config[index1]
s2 = config[index2]
rem1 = s1_end - s1
rem2 = s2_end - s2
# The robot that is behind the other shall wait.
if rem1 > rem2:
vnear.waitingIndices.append(index1)
else:
vnear.waitingIndices.append(index2)
vnear.waitingIndices = sorted(
list(set(vnear.waitingIndices)))
return inCollision
return inCollision
def CheckAndPreventConflicts(self):
"""Check if the newly added config is near any conflict. If so, apply the
conflict prevention policy.
"""
v = self.treeStart[-1] # the newly added vertex
c = v.config
waitingIndices = []
constrainedIndexSets = []
a = self.cd._a
b = self.cd._b
d = self.params.preventionDistance
# When there is (seems to be) freedom to choose which robot to
# wait, we need to be more careful since the choice we make
# can affect the problem. The order in which we iterate
# through the conflict also affects the problem.
# decisionSets is a list of sublists. Each sublist contains
# robot indices among which we can choose them to
# wait. Suppose a sublist is [rindex1, rindex2]. There might
# be some future conflict that forces, e.g., rindex1 to
# wait. Then we remove the sublist.
decisionSets = []
# movingIndices is a list of sublists. Each sublist contains robot
# indices among which at least one robot MUST BE MOVING. (Otherwise, a
# configuration will be stuck in a deadlock.)
movingIndices = []
for conflict in self.cd.conflicts:
if conflict.type == ConflictType.DEADLOCK:
# indices store active indices that are related to this deadlock.
if not set(conflict.indices).issubset(
set(self.currentActiveIndices)):
continue
indices = sorted(conflict.indices)
# indices = sorted(list(set(conflict.indices) & set(self.currentActiveIndices)))
# if len(indices) < 2:
# continue
svect = Config([c[rindex] for rindex in indices])
# svect_inner = Config([conflict.intervals[rindex][0] - a
# for rindex in indices])
svect_inner = Config(
[conflict.intervals[rindex][0] for rindex in indices])
svect_outer = Config([s - d for s in svect_inner])
# svect_max = Config([conflict.intervals[rindex][1] + b
# for rindex in indices])
svect_max = Config(
[conflict.intervals[rindex][1] for rindex in indices])
if not (svect.Dominates(svect_outer)
and svect_max.Dominates(svect)):
# This config is not near this deadlock obstacle
continue
if svect_inner.Dominates(svect):
# All robots that are related to this deadlock have not
# entered the deadlock yet.
intersect = list(set(indices) & set(waitingIndices))
if len(intersect) > 0:
# At least one robot is waiting so that's fine.
continue
else:
# In this case, we will decide later which one to wait.
decisionSets.append(indices)
else:
# Some robots have already entered the deadlock. See first
# which robots have already entered or not entered.
diff = svect - svect_inner
decisionSet = [] # this list keeps the indices of robots
# outside the deadlock
for (ds, index) in zip(diff, indices):
if ds < 0:
# This robot has not entered the deadlock.
decisionSet.append(index)
intersect = list(set(decisionSet) & set(waitingIndices))
if len(intersect) > 0:
# At least one robot outside the deadlock is already
# waiting. Do nothing here.
continue
else:
if len(decisionSet) == 1:
# # If we reach this case, the robot with index
# # decisionSet[0] is forced to wait since it is THE
# # ONLY ONE that has not entered the deadlock yet. We
# # also need to make sure that at least one of the
# # other robots MUST BE moving.
# waitingIndices.append(decisionSet[0])
# if len(indices) > 2:
# # We may need to examine this case more closely.
# # IPython.embed(header="len(conflict.indices) > 2")
# moving = list(set(indices) - set(decisionSet))
# movingIndices.append(moving)
# else:
# # index is the index of the robot that must be moving.
# index = list(set(indices) - set(decisionSet))[0]
# for decisionSet in decisionSets:
# # Remove any appearance of index in decision sets.
# try:
# decisionSet.remove(index)
# except:
# pass
# newDecisionSets = []
# for decisionSet in decisionSets:
# if len(decisionSet) == 0:
# # This should not be the case
# raise ValueError
# elif len(decisionSet) == 1:
# # The decision has been made.
# waitingIndices.append(decisionSet[0])
# else:
# intersect = list(set(waitingIndices) & set(decisionSet))
# if len(intersect) > 0:
# continue
# else:
# newDecisionSets.append(decisionSet)
# decisionSets = newDecisionSets
if decisionSet[0] not in waitingIndices:
waitingIndices.append(decisionSet[0])
else:
# In this case, we decide later
decisionSets.append(decisionSet)
else:
# Elementary conflict (shared segment or junction)
rindex1, rindex2 = conflict.indices
if ((rindex1 not in self.currentActiveIndices)
or (rindex2 not in self.currentActiveIndices)):
# At least one robot is inactive: this conflict is not relevant
continue
s1 = c[rindex1]
s2 = c[rindex2]
s1_start, s1_end = conflict.intervals[rindex1]
s2_start, s2_end = conflict.intervals[rindex2]
if conflict.type == ConflictType.SHARED:
# SHARED SEGMENT
# if ((s1_start - a - d <= s1 <= s1_start) and
# (s2_start - a - d <= s2 <= s2_start - a)):
# # Both robots have not entered the shared segment
# # yet. So we keep them as candidates.
# decisionSet = [rindex1, rindex2]
# decisionSets.append(decisionSet)
# elif ((s1_start - a - d <= s1 <= s1_start - a) and
# (s2_start - a <= s2 <= s2_start)):
# # Both robots have not entered the shared segment
# # yet. So we keep them as candidates.
# decisionSet = [rindex1, rindex2]
# decisionSets.append(decisionSet)
if ((s1_start - d <= s1 <= s1_start)
and (s2_start - d <= s2 <= s2_start)):
# Both robots have not entered the shared segment
# yet. So we keep them as candidates.
decisionSet = [rindex1, rindex2]
decisionSets.append(decisionSet)
elif ((s1_start - d <= s1 <= s1_start + a)
and (s2_start - d <= s2 <= s2_start + a)):
if s1 >= s1_start:
waitingIndices.append(rindex2)
else:
waitingIndices.append(rindex1)
# elif ((s1_start - a - d <= s1 <= s1_end + a) and
# (s2_start - a - d <= s2 <= s2_end + a)):
elif ((s1_start - d <= s1 <= s1_end)
and (s2_start - d <= s2 <= s2_end)):
# This configuration is inside a velocity constrained
# region. (Both robots are inside a shared segment so
# they should move at the same speed.)
indices = [rindex1, rindex2]
# Now we need to check if robot1/robot2 is already part
# of any other shared segment conflict.
appeared = False
for i in xrange(len(constrainedIndexSets)):
if ((rindex1 in constrainedIndexSets[i])
or (rindex2 in constrainedIndexSets[i])):
newIndices = list(
set(constrainedIndexSets[i] + indices))
constrainedIndexSets[i] = newIndices
appeared = True
break
if not appeared:
constrainedIndexSets.append(indices)
else:
# JUNCTION
# if ((s1_start - d <= s1 <= s1_end) and
# (s2_start - d <= s2 <= s2_end)):
# # Both robots almost reach the junction.
# decisionSet = [rindex1, rindex2]
# decisionSets.append(decisionSet)
if ((s1_start - d <= s1 <= s1_start)
and (s2_start - d <= s2 <= s2_start)):
# Both robots are about to cross this junction.
decisionSet = [rindex1, rindex2]
decisionSets.append(decisionSet)
elif ((s1_start - d <= s1 <= s1_end)
and (s2_start - d <= s2 <= s2_end)):
rem1 = s1_end - s1
rem2 = s2_end - s2
# The robot that is behind the other shall wait.
if rem1 > rem2:
waitingIndices.append(rindex1)
else:
waitingIndices.append(rindex2)
# Now we have already iterated through all conflicts.
# Manage the pending decisions.
newDecisionSets = []
for decisionSet in decisionSets:
intersect = list(set(waitingIndices) & set(decisionSet))
if len(intersect) > 0:
continue
else:
newDecisionSets.append(decisionSet)
decisionSets = newDecisionSets
if len(decisionSets) > 0:
appearance = dict()
for decisionSet in decisionSets:
for el in decisionSet:
if el not in appearance:
appearance[el] = 1
else:
appearance[el] += 1
# Sort the dictionary `appearance` by its values (descending order).
sorted_appearance = sorted(appearance.items(), key=itemgetter(1))
# Idea: indices that appear most often should be waiting so as to
# minimize the waiting robots.
for item in sorted_appearance:
index, _ = item
newDecisionSets = []
for decisionSet in decisionSets:
try:
decisionSet.remove(index)
waitingIndices.append(index)
except:
pass
if len(decisionSet) > 1:
newDecisionSets.append(decisionSet)
decisionSets = newDecisionSets
if len(decisionSets) == 0:
break
if len(decisionSets) > 0:
# All pending decisions should have already been made.
raise ValueError
for movingSet in movingIndices:
if len(list(set(movingSet) - set(waitingIndices))) == 0:
import IPython
IPython.embed(header="error with moving indices")
# Manage constrained index sets
# Idea: if there are three robot in the shared segment but the one
# behind is waiting, that robot should not affect the other two. If the
# one waiting is not the one behind, the moving one will eventually
# collide with it and be made waiting anyway.
waitingSet = set(waitingIndices)
newconstrainedset = []
for constrainedset in constrainedIndexSets:
constrainedset = list(set(constrainedset) - waitingSet)
if len(constrainedset) > 1:
newconstrainedset.append(constrainedset)
v.waitingIndices = sorted(list(set(waitingIndices)))
v.constrainedIndexSets = newconstrainedset # constrainedIndexSets
return