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