def get_preferred_direction(self,beam):
# Obtain the moment vector
u1,u2,u3 = beam.global_default_axes()
m11,m22,m33 = self.get_moment_magnitudes(beam.name)
# Quick debugging - making sure the torsion doesn't get too high
if not helpers.compare(m11,0,4):
#pdb.set_trace()
pass
# Sum m22 and m33 (m11 is torsion, which doesn't play a role in the direction)
moment_vector = helpers.sum_vectors(helpers.scale(m22,u2),helpers.scale(m33,u3))
'''
Cross axis-1 with the moment vector to obtain the positive clockwise direction
This keeps the overall magnitude of moment_vector since u1 is a unit vector
We are always attempting to repair further up the broken beam (closer to
the j-end). The cross will always give us the vector in the right direction
if we do it this way.
'''
twist_direction = helpers.cross(moment_vector,u1)
# Project the twist direction onto the xy-plane and normalize to have a
# maximum of 45 degree approach (or, as specified in the variables.py)
# depending on the ratio of moment magnitude to max_magnitude
xy_change = (twist_direction[0],twist_direction[1],0)
# The rotation is parallel to the z-axis, so we don't add disturbance
if helpers.compare(helpers.length(xy_change),0):
# Return the normal direction - which way is the beam leaning???
return super(MomentAwareBuilder,self).get_preferred_direction(beam)
# We know the direction in which the beam is turning
else:
# Get direction of travel (this is a unit vector)
travel = super(MomentAwareBuilder,self).get_preferred_direction(beam)
# Normalize twist to the maximum moment of force -- structure_check
normal = helpers.normalize(xy_change,construction.beam['structure_check'])
# The beam is vertical - check to see how large the normalized moment is
if travel is None:
# The change is relatively small, so ignore it
if helpers.length(normal) <= helpers.ratio(construction.beam['verticality_angle']):
return travel
else:
return helpers.make_unit(normal)
else:
scalar = 1 / helpers.ratio(construction.beam['moment_angle_max'])
scaled_travel = helpers.scale(scalar,travel)
return helpesr.make_unit(helpers.sum_vectors(normal,scaled_travel))
def wander(self):
'''
When a robot is not on a structure, it wanders. The wandering in the working
class works as follows. The robot moves around randomly with the following
restrictions:
The robot moves towards the home location if it has no beams and
the home location is detected nearby.
Otherwise, if it has beams for construction, it moves toward the base
specified construction site. If it finds another beam nearby, it has a
tendency to climb that beam instead.
'''
# Check to see if robot is at home location and has no beams
if self.at_home() and self.num_beams == 0:
self.pickup_beams()
# If we have no beams, set the ground direction to home (TEMP CODE)
if self.num_beams == 0:
vector = helpers.make_vector(self.location,construction.home_center)
self.ground_direction = (vector if not helpers.compare(helpers.length(
vector),0) else self.non_zero_xydirection())
# Find nearby beams to climb on
result = self.ground()
# Either there are no nearby beams, we are on repair_mode/search_mode, our beams are 0, or
# we are constructing a support - so don't mess with direction
if (result == None or self.repair_mode or self.search_mode or
self.num_beams == 0 or self.memory['construct_support']):
direction = self.get_ground_direction()
new_location = helpers.sum_vectors(self.location,helpers.scale(self.step,
helpers.make_unit(direction)))
self.change_location_local(new_location)
# Nearby beam, jump on it
else:
dist, close_beam, direction = (result['distance'], result['beam'],
result['direction'])
# If the beam is within steping distance, just jump on it
if self.num_beams > 0 and dist <= self.step:
# Set the ground direction to None (so we walk randomly if we do get off
# the beam again)
self.ground_direction = None
# Then move on the beam
self.move(direction, close_beam)
# If we can "detect" a beam, change the ground direction to approach it
elif self.num_beams > 0 and dist <= variables.local_radius:
self.ground_direction = direction
new_location = helpers.sum_vectors(self.location, helpers.scale(
self.step,helpers.make_unit(direction)))
self.change_location_local(new_location)
# Local beams, but could not detect (this is redundant)
else:
direction = self.get_ground_direction()
new_location = helpers.sum_vectors(self.location,helpers.scale(
self.step,helpers.make_unit(direction)))
self.change_location_local(new_location)
def ground(self):
'''
This function finds the nearest beam to the robot that is connected
to the xy-plane (ground). It returns that beam and its direction from the
robot.
'''
# Get local boxes
boxes = self.structure.get_boxes(self.location)
# Initializations
distances = {}
vectors = {}
# Cycle through boxes
for box in boxes:
# Cycle through beams in each box
for name in box:
# So e1 is in the form (x,y,z)
e1, e2 = box[name].endpoints
# beam is lying on the ground (THIS IS NOT FUNCTIONAL)
if helpers.compare(e1[2],0) and helpers.compare(e2[0],0):
# pdb.set_trace()
vectors[name] = helpers.vector_to_line(e1,e2,self.location)
distances[name] = helpers.length(vectors[name])
# Only one point is on the ground
elif helpers.compare(e1[2],0):
vectors[name] = helpers.make_vector(self.location, e1)
distances[name] = helpers.distance(e1, self.location)
elif helpers.compare(e2[2],0):
vectors[name] = helpers.make_vector(self.location, e2)
distances[name] = helpers.distances(e2, self.location)
# No points on the ground
else:
pass
# get name of beam at the minimum distance if one exists
if distances == {}:
return None
else:
# This returns the key (ie, name) of the minimum value in distances
name = min(distances, key=distances.get)
# So far away that we can't "see it"
if distances[name] > variables.local_radius:
return None
else:
# All the same beans
beams = [box[name] for box in boxes if name in box]
return { 'beam' : beams[0],
'distance' : distances[name],
'direction' : vectors[name]}
def move(self, direction, beam):
'''
Moves the robot in direction passed in and onto the beam specified
'''
length = helpers.length(direction)
# The direction is smaller than the determined step, so move exactly by
# direction
if length < self.step:
new_location = helpers.sum_vectors(self.location, direction)
self.change_location(new_location, beam)
# call do_action again since we still have some distance left, and update
# step to reflect how much distance is left to cover
self.step = self.step - length
# Reset step in preparation for next timestep
if helpers.compare(self.step,0):
self.step == variables.step_length
# We still have steps to go, so run an analysis if necessary
elif self.beam is not None:
# Run analysis before deciding to get the next direction
if not self.model.GetModelIsLocked() and self.need_data():
errors = helpers.run_analysis(self.model)
if errors != '':
# pdb.set_trace()
pass
# Get next direction
self.next_direction_info = self.get_direction()
# Unlock the results so that we can actually move
if self.model.GetModelIsLocked():
self.model.SetModelIsLocked(False)
# Move
self.do_action()
# We climbed off
else:
assert not self.model.GetModelIsLocked()
self.do_action()
# The direction is larger than the usual step, so move only the step in the
# specified direction
else:
movement = helpers.scale(self.step, helpers.make_unit(direction))
new_location = helpers.sum_vectors(self.location, movement)
self.change_location(new_location, beam)
def global_default_axes(self):
'''
Returns the default local axes. Later on we might incorporate the ability to return
rotated axes.
'''
axis_1 = helpers.make_unit(helpers.make_vector(self.endpoints.i,
self.endpoints.j))
vertical = (math.sin(math.radians(helpers.smallest_angle(axis_1,(0,0,1))))
<= 0.001)
# Break up axis_1 into unit component vectors on 1-2 plane, along with
# their maginitudes
u1, u2 = (axis_1[0],axis_1[1],0),(0,0,axis_1[2])
l1,l2 = helpers.length(u1), helpers.length(u2)
u1 = helpers.make_unit(u1) if not helpers.compare(l1,0) else (1,1,0)
u2 = helpers.make_unit(u2) if not helpers.compare(l2,0) else (0,0,1)
# Calculate axis_2 by negating and flipping componenet vectors of axis_1
axis_2 = (1,0,0) if vertical else helpers.make_unit(helpers.sum_vectors(
helpers.scale(-1 * l2,u1),helpers.scale(l1,u2)))
# Make it have a positive z-component
axis_2 = axis_2 if axis_2[2] > 0 else helpers.scale(-1,axis_2)
# Calculate axis_3 by crossing axis 1 with axis 2 (according to right hand
# rule)
axis_3 = helpers.cross(axis_1,axis_2)
axis_3 = helpers.make_unit((axis_3[0],axis_3[1],0)) if vertical else axis_3
# Sanity checks
# Unit length
assert helpers.compare(helpers.length(axis_3),1)
# On the x-y plane
assert helpers.compare(axis_3[2],0)
return axis_1,axis_2,axis_3
def get_preferred_direction(self,beam):
'''
Returns the preferred direction - this is the direction towards which the
robot wants to move when looking for an already set support tube.
The direction is a unit vector
'''
# Calculate direction of repair (check 0 dist, which means it is perfectly
# vertical!)
i, j = beam.endpoints.i, beam.endpoints.j
v1 = helpers.make_vector(self.location,j)
v2 = helpers.make_vector(i,self.location)
l1,l2 = helpers.length(v1), helpers.length(v2)
# v1 is non-zero and it is not vertical
if not (helpers.compare(l1,0) or helpers.is_vertical(v1)):
return helpers.make_unit(v1)
# v2 is non-zero and it is not vertical
elif not (helpers.compare(l2,0) or helpers.is_vertical(v2)):
return helpers.make_unit(v2)
# No preferred direction because the beam is perfectly vertical
else:
return None
def random_direction():
'''
Returns a random, new location (direction)
'''
# obtain a random direction
direction = (random.uniform(-1 * self.step, self.step), random.uniform(
-1 * self.step, self.step), 0)
# The they can't all be zero!
if helpers.compare(helpers.length(direction),0):
return random_direction()
else:
step = helpers.scale(self.step,helpers.make_unit(direction))
predicted_location = helpers.sum_vectors(step, self.location)
# Check the location
if helpers.check_location(predicted_location):
return direction
else:
return random_direction()
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 add_angles(box,dictionary):
for name, beam in box.items():
# Ignore the beam you're on.
if self.beam == None or self.beam.name != name:
# Base vector (from which angles are measured)
base_vector = helpers.make_vector(pivot,endpoint)
# Get the closest points between the beam we want to construct and the
# current beam
points = helpers.closest_points(beam.endpoints,(pivot,endpoint))
if points != None:
# Endpoints (e1 is on a vertical beam, e2 is on the tilted one)
e1,e2 = points
# If we can actually reach the second point from vertical
if (not helpers.compare(helpers.distance(pivot,e2),0) and
helpers.distance(pivot,e2) <= variables.beam_length):
# Distance between the two endpoints
dist = helpers.distance(e1,e2)
# Vector of beam we want to construct and angle from base_vector
construction_vector = helpers.make_vector(pivot,e2)
angle = helpers.smallest_angle(base_vector,construction_vector)
# Add to dictionary
if e2 in dictionary:
assert helpers.compare(dictionary[e2],angle)
else:
dictionary[e2] = angle
# Get the points at which the beam intersects the sphere created by
# the vertical beam
sphere_points = helpers.sphere_intersection(beam.endpoints,pivot,
variables.beam_length)
if sphere_points != None:
# Cycle through intersection points (really, should be two, though
# it is possible for it to be one, in
# which case, we would have already taken care of this). Either way,
# we just cycle
for point in sphere_points:
# Vector to the beam we want to construct
construction_vector = helpers.make_vector(pivot,point)
angle = helpers.smallest_angle(base_vector,construction_vector)
# Add to dictionary
if point in dictionary:
assert helpers.compare(dictionary[point],angle)
else:
dictionary[point] = angle
# Endpoints are also included
for e in beam.endpoints:
v = helpers.make_vector(pivot,e)
l = helpers.length(v)
if (e not in dictionary and not helpers.compare(l,0) and (
helpers.compare(l,variables.beam_length) or l < variables.beam_length)):
angle = helpers.smallest_angle(base_vector,v)
dictionary[e] = angle
return dictionary
