def next_dict(item, dictionary):
"""
Returns whether or not the value (a direction vector) is found inside of
dictionary (ie, looks for parallel directions)
"""
key, value = item
temp = {}
for test_key, test_values in dictionary.items():
# Keys are the same, vectors are parallel (point in same dir too)
if key == test_key:
for test_value in test_values:
if helpers.parallel(value, test_value) and helpers.dot(value, test_value) > 0:
if test_key in temp:
temp[test_key].append(test_value)
else:
temp[test_key] = [test_value]
# No values are parallel, so return None
if temp == {}:
return None
# Pick a direction from those that are parallel (so we continue moving)
# in our preferred direction
else:
return self.pick_direction(temp)
def get_walkable_directions(self,box):
'''
Finds all of the beams in box which intersect the robots location or
to which the robot can walk onto. Returns delta x, delta y, and delta z
of the change necessary to arrive at either the joint or to move along
the current beam by current_step.
'''
# Get all joints within a time-step
# Remember that beams DOES NOT include the current beam, only others
crawlable = {}
for joint in self.beam.joints:
dist = helpers.distance(self.location,joint)
# If we are at the joint, return the possible directions of other beams
if helpers.compare(dist,0):
for beam in self.beam.joints[joint]:
# The index error should never happen, but this provides nice error
# support
try:
# Get endpoints of beam and find direction vector to those endpoints
e1, e2 = beam.endpoints
v1, v2 = helpers.make_vector(self.location,e1), helpers.make_vector(
self.location,e2)
# We don't want to include zero-vectors
bool_v1,bool_v2 = (not helpers.compare(helpers.length(v1),0),
not helpers.compare(helpers.length(v2),0))
# Checking for zero_vectors
if bool_v1 and bool_v2:
crawlable[beam.name] = ([helpers.make_vector(self.location,e1),
helpers.make_vector(self.location,e2)])
elif bool_v1:
crawlable[beam.name] = [helpers.make_vector(self.location,e1)]
elif bool_v2:
crawlable[beam.name] = [helpers.make_vector(self.location,e2)]
else:
raise Exception("All distances from beam were zero-length.")
# Include distances to nearby joints (on the beam moving out from our
# current joint)
for coord in beam.joints:
# Direction vecotrs
v = helpers.make_vector(self.location,coord)
length = helpers.length(v)
# If further than our step, or zero, pass
if ((length < self.step or helpers.compare(length, self.step))
and not helpers.compare(length,0)):
try:
# Only add if it is not already accounted for
if v not in crawlable[beam.name]:
crawlable[beam.name].append(v)
except IndexError:
raise Exception("Adding nearby joints failed because \
endpoints were ignored.")
except IndexError:
print ("The beam {} seems to have a joint with {}, but it is not in\
the box?".format(name,self.beam.name))
# For all joints within the timestep, return a direction that is exactly
# the change from current to that point.
elif dist <= self.step:
if self.beam.name in crawlable:
crawlable[self.beam.name].append(helpers.make_vector(self.location,
joint))
else:
crawlable[self.beam.name] = [helpers.make_vector(self.location,joint)]
# The joint is too far, so no point in considering it as a walkable direction
else:
pass
# The joints never include our own beam, so now add directions pertaining to
# our own beam
v1, v2 = (helpers.make_vector(self.location,self.beam.endpoints.i),
helpers.make_vector(self.location,self.beam.endpoints.j))
# Check to make sure directions are non-zero
b_v1 = not helpers.compare(helpers.length(v1),0)
b_v2 = not helpers.compare(helpers.length(v2),0)
# If we haven't already accounted for our beam
if self.beam.name not in crawlable:
# Add the non-zero directions
if b_v1 and b_v2:
crawlable[self.beam.name] = [v1,v2]
elif b_v1:
crawlable[self.beam.name] = [v1]
elif b_v2:
crawlable[self.beam.name] = [v2]
# Add directions that might not have been entered by joints
else:
bool_v1, bool_v2 = True, True
for direct in crawlable[self.beam.name]:
# Don't add directions which are basically the same.
if helpers.parallel(direct,v1) and helpers.dot(direct,v1) > 0:
bool_v1 = False
if helpers.parallel(direct,v2) and helpers.dot(direct,v2) > 0:
bool_v2 = False
# Add the non-zero non-parallel direction
if bool_v2 and b_v2:
crawlable[self.beam.name].append(v2)
if bool_v1 and b_v1:
crawlable[self.beam.name].append(v1)
return crawlable
def get_walkable_directions(self, box):
'''
Finds all of the beams in box which intersect the robots location or
to which the robot can walk onto. Returns delta x, delta y, and delta z
of the change necessary to arrive at either the joint or to move along
the current beam by current_step.
'''
# Get all joints within a time-step
# Remember that beams DOES NOT include the current beam, only others
crawlable = {}
for joint in self.beam.joints:
dist = helpers.distance(self.location, joint)
# If we are at the joint, return the possible directions of other beams
if helpers.compare(dist, 0):
for beam in self.beam.joints[joint]:
# The index error should never happen, but this provides nice error
# support
try:
# Get endpoints of beam and find direction vector to those endpoints
e1, e2 = beam.endpoints
v1, v2 = helpers.make_vector(self.location,
e1), helpers.make_vector(
self.location, e2)
# We don't want to include zero-vectors
bool_v1, bool_v2 = (
not helpers.compare(helpers.length(v1), 0),
not helpers.compare(helpers.length(v2), 0))
# Checking for zero_vectors
if bool_v1 and bool_v2:
crawlable[beam.name] = ([
helpers.make_vector(self.location, e1),
helpers.make_vector(self.location, e2)
])
elif bool_v1:
crawlable[beam.name] = [
helpers.make_vector(self.location, e1)
]
elif bool_v2:
crawlable[beam.name] = [
helpers.make_vector(self.location, e2)
]
else:
raise Exception(
"All distances from beam were zero-length.")
# Include distances to nearby joints (on the beam moving out from our
# current joint)
for coord in beam.joints:
# Direction vecotrs
v = helpers.make_vector(self.location, coord)
length = helpers.length(v)
# If further than our step, or zero, pass
if ((length < self.step
or helpers.compare(length, self.step))
and not helpers.compare(length, 0)):
try:
# Only add if it is not already accounted for
if v not in crawlable[beam.name]:
crawlable[beam.name].append(v)
except IndexError:
raise Exception(
"Adding nearby joints failed because \
endpoints were ignored.")
except IndexError:
print(
"The beam {} seems to have a joint with {}, but it is not in\
the box?".format(name, self.beam.name))
# For all joints within the timestep, return a direction that is exactly
# the change from current to that point.
elif dist <= self.step:
if self.beam.name in crawlable:
crawlable[self.beam.name].append(
helpers.make_vector(self.location, joint))
else:
crawlable[self.beam.name] = [
helpers.make_vector(self.location, joint)
]
# The joint is too far, so no point in considering it as a walkable direction
else:
pass
# The joints never include our own beam, so now add directions pertaining to
# our own beam
v1, v2 = (helpers.make_vector(self.location, self.beam.endpoints.i),
helpers.make_vector(self.location, self.beam.endpoints.j))
# Check to make sure directions are non-zero
b_v1 = not helpers.compare(helpers.length(v1), 0)
b_v2 = not helpers.compare(helpers.length(v2), 0)
# If we haven't already accounted for our beam
if self.beam.name not in crawlable:
# Add the non-zero directions
if b_v1 and b_v2:
crawlable[self.beam.name] = [v1, v2]
elif b_v1:
crawlable[self.beam.name] = [v1]
elif b_v2:
crawlable[self.beam.name] = [v2]
# Add directions that might not have been entered by joints
else:
bool_v1, bool_v2 = True, True
for direct in crawlable[self.beam.name]:
# Don't add directions which are basically the same.
if helpers.parallel(direct,
v1) and helpers.dot(direct, v1) > 0:
bool_v1 = False
if helpers.parallel(direct,
v2) and helpers.dot(direct, v2) > 0:
bool_v2 = False
# Add the non-zero non-parallel direction
if bool_v2 and b_v2:
crawlable[self.beam.name].append(v2)
if bool_v1 and b_v1:
crawlable[self.beam.name].append(v1)
return crawlable
def support_xy_direction(self):
'''
Improves the construction direction so that we take into account the angle
at which our current beam is located, and the verticality of the beam we
are attempting to reach. This returns a unit direction (always should!)
'''
# If we're on the ground, then continue doing as before
if self.beam is None:
return super(Repairer,self).support_xy_direction()
else:
# Get repair beam vector
b_i,b_j = self.structure.get_endpoints(self.memory['broken_beam_name'],
self.location)
repair_vector = helpers.make_vector(b_i,b_j)
# Get the correct vector for the current beam
# Remember - we travel in the opposite direction as normal when building
# the support beam, so that's why this seems opposite of normal
c_i,c_j = self.beam.endpoints
current_vector = (helpers.make_vector(c_j,c_i) if
self.memory['previous_direction'][1][2] > 0 else helpers.make_vector(
c_i,c_j))
#
angle = helpers.smallest_angle(repair_vector,current_vector)
# If below the specified angle, then place the beam directly upwards (no
# change in xy)
if angle < BConstants.beam['direct_repair_limit']:
return None
else:
vertical = (0,0,1)
v1,v2 = helpers.make_vector(b_i,c_i), helpers.make_vector(b_i,c_j)
# We can't get a direction based on beam locations
if helpers.parallel(vertical,v1) and helpers.parallel(vertical,v2):
return super(Repairer,self).support_xy_direction()
# We can use the current beam to decide the direction
elif not helpers.parallel(vertical,current_vector):
# pdb.set_trace()
# Project onto the xy-plane and negate
if current_vector[2] > 0:
projection = helpers.make_unit(helpers.scale(-1,(current_vector[0],
current_vector[1],0)))
else:
projection = helpers.make_unit((current_vector[0],current_vector[1],0))
# Add some small disturbance
disturbance = helpers.scale(random.uniform(-1,1),(-projection[1],
projection[0],projection[2]))
result = helpers.sum_vectors(projection,disturbance)
# TODO
return result
elif not helpers.parallel(vertical,repair_vector):
return super(Repairer,self).support_xy_direction()
else:
return super(Repairer,self).support_xy_direction()
def support_xy_direction(self):
'''
Improves the construction direction so that we take into account the angle
at which our current beam is located, and the verticality of the beam we
are attempting to reach. This returns a unit direction (always should!)
'''
# If we're on the ground, then continue doing as before
if self.beam is None:
return super(Repairer, self).support_xy_direction()
else:
# Get repair beam vector
b_i, b_j = self.structure.get_endpoints(
self.memory['broken_beam_name'], self.location)
repair_vector = helpers.make_vector(b_i, b_j)
# Get the correct vector for the current beam
# Remember - we travel in the opposite direction as normal when building
# the support beam, so that's why this seems opposite of normal
c_i, c_j = self.beam.endpoints
current_vector = (helpers.make_vector(c_j, c_i)
if self.memory['previous_direction'][1][2] > 0
else helpers.make_vector(c_i, c_j))
#
angle = helpers.smallest_angle(repair_vector, current_vector)
# If below the specified angle, then place the beam directly upwards (no
# change in xy)
if angle < BConstants.beam['direct_repair_limit']:
return None
else:
vertical = (0, 0, 1)
v1, v2 = helpers.make_vector(b_i, c_i), helpers.make_vector(
b_i, c_j)
# We can't get a direction based on beam locations
if helpers.parallel(vertical, v1) and helpers.parallel(
vertical, v2):
return super(Repairer, self).support_xy_direction()
# We can use the current beam to decide the direction
elif not helpers.parallel(vertical, current_vector):
# pdb.set_trace()
# Project onto the xy-plane and negate
if current_vector[2] > 0:
projection = helpers.make_unit(
helpers.scale(
-1, (current_vector[0], current_vector[1], 0)))
else:
projection = helpers.make_unit(
(current_vector[0], current_vector[1], 0))
# Add some small disturbance
disturbance = helpers.scale(random.uniform(
-1, 1), (-projection[1], projection[0], projection[2]))
result = helpers.sum_vectors(projection, disturbance)
# TODO
return result
elif not helpers.parallel(vertical, repair_vector):
return super(Repairer, self).support_xy_direction()
else:
return super(Repairer, self).support_xy_direction()
