def __init__(self,
lengths,
ee1x=None,
ee1y=None,
ee1_angles=None,
ee2x=None,
ee2y=None,
ee2_angles=None,
ee1_grappled=False,
ee2_grappled=False):
"""
Constructor for RobotConfig - we suggest using make_robot_config_from_ee1() or make_robot_config_from_ee2()
to construct new instances of RobotConfig rather than calling this function directly.
"""
self.lengths = lengths
self.ee1_grappled = ee1_grappled
self.ee2_grappled = ee2_grappled
if ee1x is not None and ee1y is not None and ee1_angles is not None:
points = [(ee1x, ee1y)]
net_angle = Angle(radians=0)
for i in range(len(ee1_angles)):
x, y = points[-1]
net_angle = net_angle + ee1_angles[i]
x_new = x + (lengths[i] * math.cos(net_angle.in_radians()))
y_new = y + (lengths[i] * math.sin(net_angle.in_radians()))
points.append((x_new, y_new))
self.ee1_angles = ee1_angles
# 1st angle is last angle of e1_angles + pi, others are all -1 * e1_angles (in reverse order)
self.ee2_angles = [math.pi + net_angle] + \
[-ee1_angles[i] for i in range(len(ee1_angles) - 1, 0, -1)]
self.points = points
elif ee2x is not None and ee2y is not None and ee2_angles is not None:
points = [(ee2x, ee2y)]
net_angle = Angle(radians=0)
for i in range(len(ee2_angles)):
x, y = points[0]
net_angle = net_angle + ee2_angles[i]
x_new = x + (lengths[-i - 1] *
math.cos(net_angle.in_radians()))
y_new = y + (lengths[-i - 1] *
math.sin(net_angle.in_radians()))
points.insert(0, (x_new, y_new))
# 1st angle is last angle of e2_angles + pi, others are all -1 * e2_angles (in reverse order)
self.ee1_angles = [math.pi + sum(ee2_angles)] + \
[-ee2_angles[i] for i in range(len(ee2_angles) - 1, 0, -1)]
self.ee2_angles = ee2_angles
self.points = points
else:
raise Exception(
"Could not create RobotConfig - Insufficient information given"
)
def generate_bridge_configs(spec, obstacles, phase):
"""
start point (x1, y1) is the current grapple point
goal point (x2, y2) is the next grapple point
create an empty sub goal list to keep the goal we find
for i := range[1, 10] do
for j := range[1, num of segments - 1] do
sample some lengths and angles
if grapple point is even order do
partial config = generate configuration (start point, length, and angle)
if the distance between ee2 and goal point is within the length limit do
calculate the angle and length of the last segment
use the parameters to generate a completed configuration
if the configuration is collision free do
add the configuration into bridge_configs list
else do the former steps but with ee2 grappled at the grapple point
"""
start_point = spec.grapple_points[phase]
end_point = spec.grapple_points[phase + 1]
bridge_configs = []
for i in range(10):
complete_find = False
# repeat until 10 bridges generated
while complete_find is False:
sampling_angles = []
sampling_lengths = []
# finding a set of angles and lengths which form a bridge is to generate the first n-1 links
for j in range(spec.num_segments - 1):
sample_angle = random.uniform(-165, 165)
sampling_angles.append(Angle(degrees=float(sample_angle)))
sample_length = random.uniform(spec.min_lengths[j],
spec.max_lengths[j])
sampling_lengths.append(sample_length)
if phase % 2 == 0:
x1, y1 = start_point
x2, y2 = end_point
partial_bridge = make_robot_config_from_ee1(x1,
y1,
sampling_angles,
sampling_lengths,
ee1_grappled=True)
if (partial_bridge.points[-1][0] - x2) ** 2 + (partial_bridge.points[-1][1] - y2) ** 2 < \
spec.max_lengths[spec.num_segments - 1] ** 2:
# using tan(delta_y / delta_x)
calculated_net_angle = math.atan(
(partial_bridge.points[-1][1] - y2) /
(partial_bridge.points[-1][0] - x2))
# length sqrt(delta_x^2 + delta_y^2)
sampling_lengths.append(
math.sqrt((partial_bridge.points[-1][0] - x2)**2 +
(partial_bridge.points[-1][1] - y2)**2))
if y2 > partial_bridge.points[-1][
1] and x2 < partial_bridge.points[-1][0]:
calculated_net_angle += math.pi
elif y2 < partial_bridge.points[-1][
1] and x2 < partial_bridge.points[-1][0]:
calculated_net_angle -= math.pi
partial_net_angle = Angle(radians=0)
for n in range(len(sampling_angles)):
partial_net_angle += sampling_angles[n]
sampling_angles.append(
Angle(radians=float(calculated_net_angle -
partial_net_angle.in_radians())))
bridge_config = make_robot_config_from_ee1(
x1,
y1,
sampling_angles,
sampling_lengths,
ee1_grappled=True,
ee2_grappled=True)
if collison_checking(bridge_config, spec, obstacles):
bridge_configs.append(bridge_config)
complete_find = True
else:
x1, y1 = start_point
x2, y2 = end_point
partial_bridge = make_robot_config_from_ee2(x1,
y1,
sampling_angles,
sampling_lengths,
ee2_grappled=True)
if (partial_bridge.points[0][0] - x2) ** 2 + (partial_bridge.points[0][1] - y2) ** 2 < \
spec.max_lengths[0] ** 2:
calculated_net_angle = math.atan(
(partial_bridge.points[0][1] - y2) /
(partial_bridge.points[0][0] - x2))
sampling_lengths.insert(
0,
math.sqrt((partial_bridge.points[0][0] - x2)**2 +
(partial_bridge.points[0][1] - y2)**2))
if y2 > partial_bridge.points[0][
1] and x2 < partial_bridge.points[0][0]:
calculated_net_angle += math.pi
elif y2 < partial_bridge.points[0][
1] and x2 < partial_bridge.points[0][0]:
calculated_net_angle -= math.pi
partial_net_angle = Angle(radians=0)
for n in range(len(sampling_angles)):
partial_net_angle += sampling_angles[n]
sampling_angles.append(
Angle(radians=float(calculated_net_angle -
partial_net_angle.in_radians())))
bridge_config = make_robot_config_from_ee2(
x1,
y1,
sampling_angles,
sampling_lengths,
ee1_grappled=True,
ee2_grappled=True)
if collison_checking(bridge_config, spec, obstacles):
bridge_configs.append(bridge_config)
complete_find = True
return bridge_configs
