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,
BConstants.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(
BConstants.beam['verticality_angle']):
return travel
else:
return helpers.make_unit(normal)
else:
scalar = 1 / helpers.ratio(BConstants.beam['moment_angle_max'])
scaled_travel = helpers.scale(scalar, travel)
return helpesr.make_unit(
helpers.sum_vectors(normal, scaled_travel))
def support_vertical_change(self, angle=None):
"""
Returns the vertical change for the support endpoint locations
"""
# Add beam_directions plus vertical change based on angle ratio (tan)
if angle is None:
ratio = helpers.ratio(self.get_angle("support_angle"))
# We changed the angle from the default
else:
ratio = helpers.ratio(angle)
# Calculate vertical based on assumption that xy-dir is unit
vertical = helpers.scale(1 / ratio, (0, 0, 1)) if ratio != 0 else None
return vertical
def get_repair_beam_direction(self):
"""
Returns the xy direction at which the support beam should be set (if none is
found). Currently, we just add a bit of disturbace while remaining within
the range that the robot was set to search.
"""
direction = self.memory["preferred_direction"]
# No preferred direction, so beam was vertically above use
if direction is None:
return None
# Add a bit of disturbace
else:
# Project onto xy_plane and make_unit
xy = helpers.make_unit((direction[0], direction[1], 0))
xy_perp = (-1 * xy[1], xy[0], 0)
# Obtain disturbance based on "search_angle"
limit = helpers.ratio(BConstants.beam["direction_tolerance_angle"])
scale = random.uniform(-1 * limit, limit)
disturbance = helpers.scale(scale, xy_perp)
return helpers.sum_vectors(disturbance, xy)
def support_beam_endpoint(self):
"""
Returns the endpoint for construction of a support beam
"""
# Add beam_directions plus vertical change based on angle ratio (tan)
ratio = helpers.ratio(self.get_angle("support_angle"))
vertical = self.support_vertical_change()
xy_dir = self.support_xy_direction()
if xy_dir is None or vertical is None:
direction = (0, 0, 1)
else:
xy_dir = helpers.make_unit(xy_dir)
direction = helpers.make_unit(helpers.sum_vectors(xy_dir, vertical))
# Calculate endpoints
endpoint = helpers.sum_vectors(self.location, helpers.scale(BConstants.beam["length"], direction))
return endpoint
def get_default(self, ratio_coord, vertical_coord):
'''
Returns the coordinate onto which the j-point of the beam to construct
should lie
'''
# No vertical coordinate this time, since we will use a leaning one
coord = super(LeanRepairer, self).get_default(ratio_coord, None)
if coord is not None:
return coord
# We need to return one that leans
else:
xy_dir = self.non_zero_xydirection()
scale = 1 / helpers.ratio(BConstants.beam['construction_angle'])
vertical = helpers.scale(scale,
BConstants.beam['vertical_dir_set'])
direction = helpers.make_unit(helpers.sum_vectors(
xy_dir, vertical))
endpoint = helpers.sum_vectors(
self.location,
helpers.scale(BConstants.beam['length'], direction))
return endpoint
