def create_bridge_config(spec):
grapple_points = spec.grapple_points
arm = spec.initial
min_lengths = spec.min_lengths
max_lengths = spec.max_lengths
#print(min_lengths)
#print(max_lengths)
angles = []
lengths = []
for i in range(len(arm.lengths)):
if i == 0:
angles.append(Angle(math.radians(random.randint(-180, 180))))
else:
angles.append(Angle(math.radians(random.randint(-165, 165))))
lengths.append(random.uniform(min_lengths[i], max_lengths[i]))
#angles.append(Angle)
#print(angles)
#print(lengths)
#print(arm.points[0][0])
#print(arm.points[0][1])
#print(arm.points[1])
sample = make_robot_config_from_ee1(arm.points[0][0], arm.points[0][1],
angles, lengths, True)
#for points in sample.points:
#print(points)
sample_points = sample.points[-2]
#print("--------------")
#print(sample_points)
delta_y = grapple_points[1][1] - sample_points[1]
delta_x = grapple_points[1][0] - sample_points[0]
new_angle = delta_y / delta_x
last_angle = Angle.tan(new_angle)
sum_angles = 0
for angle in range(len(angles) - 1):
if angle == 0:
sum_angles = angles[angle].in_degrees()
else:
sum_angles = 180 + angles[angle].in_degrees()
second_last_angle = 360 - sum_angles - last_angle
angles[-1] = Angle(math.radians(360 + second_last_angle))
lengths[-1] = math.sqrt(delta_x**2 + delta_y**2)
bridge_2_config = make_robot_config_from_ee1(arm.points[0][0],
arm.points[0][1], angles,
lengths, True, False)
while (not individual_config_collision_checking(spec, bridge_2_config)):
bridge_2_config = create_bridge_config(spec)
return bridge_2_config
