def mid_point(config_1, config_2): """Return the mid config for """ angles_new = [] length_new = [] if config_1.ee1_grappled == True: angles_1 = copy.deepcopy(config_1.ee1_angles) elif config_1.ee2_grappled == True: angles_1 = copy.deepcopy(config_1.ee2_angles) if config_2.ee1_grappled == True: angles_2 = copy.deepcopy(config_2.ee1_angles) elif config_2.ee2_grappled == True: angles_2 = copy.deepcopy(config_2.ee2_angles) for i in range(len(angles_1)): an = (angles_1[i]) / 2 + (angles_2[i]) / 2 angles_new.append(an) for i in range(len(config_1.lengths)): length_new.append((config_1.lengths[i] + config_2.lengths[i]) / 2) if config_1.ee1_grappled == True: config = make_robot_config_from_ee1(config_1.points[0][0], config_1.points[0][1], angles_new, length_new, ee1_grappled=True) elif config_1.ee2_grappled == True: config = make_robot_config_from_ee2(config_1.points[len(config_1.points) - 1][0], config_1.points[len(config_1.points) - 1][1], angles_new, length_new, ee2_grappled=True) return config
def uniformly_sampling(spec, obstacles, stage): while True: sampling_angle = [] sampling_length = [] for i in range(spec.num_segments): angle = random.uniform(-165, 165) sampling_angle.append(Angle(degrees=float(angle))) length = random.uniform(spec.min_lengths[i], spec.max_lengths[i]) sampling_length.append(length) if stage % 2 == 0: new_config = make_robot_config_from_ee1( spec.grapple_points[stage][0], spec.grapple_points[stage][1], sampling_angle, sampling_length, ee1_grappled=True) else: new_config = make_robot_config_from_ee2( spec.grapple_points[stage][0], spec.grapple_points[stage][1], sampling_angle, sampling_length, ee2_grappled=True) if test_obstacle_collision(new_config, spec, obstacles) and test_self_collision(new_config, spec) and \ test_environment_bounds(new_config): return new_config
def check_path(config1, config2, spec, obstacles): angles1 = config1.ee1_angles angles2 = config2.ee1_angles angle_distance = [] if config1.ee2_grappled is True: angles1 = config1.ee2_angles angles2 = config2.ee2_angles lengths1 = config1.lengths lengths2 = config2.lengths length_distance = [] for j in range(len(angles1)): angle_distance.append(angles2[j].__sub__(angles1[j])) length_distance.append(lengths2[j] - lengths1[j]) for j in range(20): new_angle_list = [] new_length_list = [] for k in range(len(angles1)): if config1.ee1_grappled is True: new_angle_list.append(config1.ee1_angles[k].__add__( angle_distance[k].__mul__(0.05 * j))) elif config1.ee2_grappled is True: new_angle_list.append(config1.ee2_angles[k].__add__( angle_distance[k].__mul__(0.05 * j))) new_length_list.append(config1.lengths[k] + length_distance[k] * 0.05 * j) x, y = config1.points[0] new_config = make_robot_config_from_ee1(x, y, new_angle_list, new_length_list, ee1_grappled=True, ee2_grappled=False) if config1.ee2_grappled is True: x, y = config1.points[-1] new_config = make_robot_config_from_ee2( x, y, new_angle_list, list(reversed(new_length_list)), ee1_grappled=False, ee2_grappled=True) if not (test_obstacle_collision(new_config, spec, obstacles) and test_self_collision(new_config, spec) and test_environment_bounds(new_config)): return False return True
def tracking_robot(coord: tuple): new_angles = [] for idx, angObj in enumerate(init_robot.ee1_angles): new_angle = angle.Angle(radians=0) new_angles.append(new_angle) new_len = problem.min_lengths new_robot = robot_config.make_robot_config_from_ee2( coord[0], coord[1], new_angles, new_len, False, False) return new_robot
def __init__(self, input_file): # parse input file f = open(input_file, 'r') # parse arm constraints try: self.num_segments = int(next_valid_line(f)) except Exception: print("Invalid value for number of segments") sys.exit(1) self.min_lengths = [float(i) for i in next_valid_line(f).split(' ')] assert len(self.min_lengths) == self.num_segments, \ "Number of minimum lengths does not match number of segments" self.max_lengths = [float(i) for i in next_valid_line(f).split(' ')] assert len(self.max_lengths) == self.num_segments, \ "Number of maximum lengths does not match number of segments" # parse initial configuration initial_grappled = int(next_valid_line(f)) assert initial_grappled == 1 or initial_grappled == 2, "Initial end effector number is not 1 or 2" try: initial_eex, initial_eey = [ float(i) for i in next_valid_line(f).split(' ') ] except Exception: print("Invalid value(s) for initial end effector position") sys.exit(1) initial_angles = [ Angle(degrees=float(i)) for i in next_valid_line(f).split(' ') ] assert len(initial_angles) == self.num_segments, \ "Number of initial angles does not match number of segments" initial_lengths = [float(i) for i in next_valid_line(f).split(' ')] assert len(initial_lengths) == self.num_segments, \ "Number of initial lengths does not match number of segments" if initial_grappled == 1: self.initial = make_robot_config_from_ee1(initial_eex, initial_eey, initial_angles, initial_lengths, ee1_grappled=True) else: self.initial = make_robot_config_from_ee2(initial_eex, initial_eey, initial_angles, initial_lengths, ee2_grappled=True) # parse goal configuration goal_grappled = int(next_valid_line(f)) assert goal_grappled == 1 or goal_grappled == 2, "Goal end effector number is not 1 or 2" try: goal_eex, goal_eey = [ float(i) for i in next_valid_line(f).split(' ') ] except Exception: print("Invalid value(s) for goal end effector 1 position") sys.exit(1) goal_angles = [ Angle(degrees=float(i)) for i in next_valid_line(f).split(' ') ] assert len(goal_angles) == self.num_segments, \ "Number of goal ee1 angles does not match number of segments" goal_lengths = [float(i) for i in next_valid_line(f).split(' ')] assert len(goal_lengths) == self.num_segments, \ "Number of goal lengths does not match number of segments" if goal_grappled == 1: self.goal = make_robot_config_from_ee1(goal_eex, goal_eey, goal_angles, goal_lengths, ee1_grappled=True) else: self.goal = make_robot_config_from_ee2(goal_eex, goal_eey, goal_angles, goal_lengths, ee2_grappled=True) # parse grapple points try: self.num_grapple_points = int(next_valid_line(f)) except Exception: print("Invalid value for number of grapple points") sys.exit(1) grapple_points = [] for i in range(self.num_grapple_points): try: grapple_points.append( tuple([float(i) for i in next_valid_line(f).split(' ')])) except Exception: print("Invalid value(s) for grapple point " + str(i) + " position") sys.exit(1) self.grapple_points = grapple_points # parse obstacles try: self.num_obstacles = int(next_valid_line(f)) except Exception: print("Invalid value for number of obstacles") sys.exit(1) obstacles = [] for i in range(self.num_obstacles): try: x1, y1, x2, y2 = [ float(i) for i in next_valid_line(f).split(' ') ] obstacles.append(Obstacle(x1, y1, x2, y2)) except Exception: print("Invalid value(s) for obstacle " + str(i)) sys.exit(1) self.obstacles = obstacles
def create_state_graph(spec, obstacles): init_node = GraphNode(spec, spec.initial) goal_node = GraphNode(spec, spec.goal) nodes = [init_node, goal_node] for stage in range(spec.num_grapple_points): for i in range(500): sample_point = uniformly_sampling(spec, obstacles, stage) new_node = GraphNode(spec, sample_point) nodes.append(new_node) for node in nodes: if check_distance( spec, sample_point, node.config) and check_path( sample_point, node.config, spec, obstacles): node.neighbors.append(new_node) new_node.neighbors.append(node) init_container = [init_node] init_visited = {init_node: [init_node.config]} while len(init_container) > 0: current = init_container.pop(0) if test_config_equality(current.config, spec.goal, spec): result = init_visited[current] # return result new_list = [] for i in range(len(result) - 1): angles1 = result[i].ee1_angles angles2 = result[i + 1].ee1_angles if result[i].ee2_grappled is True: angles1 = result[i].ee2_angles angles2 = result[i + 1].ee2_angles angle_distance = [] lengths1 = result[i].lengths lengths2 = result[i + 1].lengths length_distance = [] for j in range(len(angles1)): angle_distance.append(angles2[j].__sub__(angles1[j])) length_distance.append(lengths2[j] - lengths1[j]) for j in range(501): new_angle_list = [] new_length_list = [] for k in range(len(angles1)): if result[i].ee1_grappled is True: new_angle_list.append( result[i].ee1_angles[k].__add__( angle_distance[k].__mul__(0.002 * j))) elif result[i].ee2_grappled is True: new_angle_list.append( result[i].ee2_angles[k].__add__( angle_distance[k].__mul__(0.002 * j))) new_length_list.append(result[i].lengths[k] + length_distance[k] * 0.002 * j) if result[i].ee1_grappled is True: x, y = result[i].points[0] new_config = make_robot_config_from_ee1( x, y, new_angle_list, new_length_list, ee1_grappled=True, ee2_grappled=False) if test_obstacle_collision( new_config, spec, obstacles) and test_self_collision( new_config, spec ) and test_environment_bounds(new_config): new_list.append(new_config) elif result[i].ee2_grappled is True: x, y = result[i].points[-1] new_config = make_robot_config_from_ee2( x, y, new_angle_list, list(reversed(new_length_list)), ee1_grappled=False, ee2_grappled=True) if test_obstacle_collision( new_config, spec, obstacles) and test_self_collision( new_config, spec ) and test_environment_bounds(new_config): new_list.append(new_config) return new_list successors = current.get_successors() for suc in successors: if suc not in init_visited: init_container.append(suc) init_visited[suc] = init_visited[current] + [suc.config]
def generate_sub_goals(spec, stage, obstacles): start_point = spec.grapple_points[stage] goal_point = spec.grapple_points[stage+1] sub_goals = [] for i in range(10): find_goal = False while find_goal is False: sampling_angle = [] sampling_length = [] for j in range(spec.num_segments - 1): angle = random.uniform(-165, 165) sampling_angle.append(Angle(degrees=float(angle))) length = random.uniform(spec.min_lengths[j], spec.max_lengths[j]) sampling_length.append(length) if stage % 2 == 0: x1, y1 = start_point x2, y2 = goal_point partial_config = make_robot_config_from_ee1(x1, y1, sampling_angle, sampling_length, ee1_grappled=True) if (partial_config.points[-1][0] - x2) ** 2 + (partial_config.points[-1][1] - y2) ** 2 < spec.max_lengths[spec.num_segments-1] ** 2: sampling_length.append(math.sqrt((partial_config.points[-1][0] - x2) ** 2 + (partial_config.points[-1][1] - y2) ** 2)) required_net_angle = math.atan((partial_config.points[-1][1] - y2) / (partial_config.points[-1][0] - x2)) if y2 > partial_config.points[-1][1] and x2 < partial_config.points[-1][0]: required_net_angle += math.pi elif y2 < partial_config.points[-1][1] and x2 < partial_config.points[-1][0]: required_net_angle -= math.pi current_net_angle = 0 for angle in sampling_angle: current_net_angle += angle.in_radians() sampling_angle.append(Angle(radians=float(required_net_angle-current_net_angle))) sub_goal_config = make_robot_config_from_ee1(x1, y1, sampling_angle, sampling_length, ee1_grappled=True, ee2_grappled=True) if test_obstacle_collision(sub_goal_config, spec, obstacles) and test_self_collision(sub_goal_config, spec)\ and test_environment_bounds(sub_goal_config): sub_goals.append(sub_goal_config) find_goal = True else: x1, y1 = start_point x2, y2 = goal_point partial_config = make_robot_config_from_ee2(x1, y1, sampling_angle, sampling_length, ee2_grappled=True) if (partial_config.points[0][0] - x2) ** 2 + (partial_config.points[0][1] - y2) ** 2 < spec.max_lengths[0] ** 2: sampling_length.insert(0, math.sqrt((partial_config.points[0][0] - x2) ** 2 + (partial_config.points[0][1] - y2) ** 2)) required_net_angle = math.atan((partial_config.points[0][1] - y2) / (partial_config.points[0][0] - x2)) if required_net_angle < 0: required_net_angle += math.pi current_net_angle = 0 for angle in sampling_angle: current_net_angle += angle.in_radians() sampling_angle.append(Angle(radians=float(required_net_angle-current_net_angle))) sub_goal_config = make_robot_config_from_ee2(x1, y1, sampling_angle, sampling_length, ee1_grappled=True, ee2_grappled=True) if test_obstacle_collision(sub_goal_config, spec, obstacles) and test_self_collision(sub_goal_config, spec)\ and test_environment_bounds(sub_goal_config): sub_goals.append(sub_goal_config) find_goal = True return sub_goals
def create_state_graph(spec, obstacles, stage, sub_init=None, sub_goals=None): if sub_init is None: init_node = GraphNode(spec, spec.initial) else: init_node = GraphNode(spec, sub_init) nodes = [init_node] if sub_goals is None: goal_node = GraphNode(spec, spec.goal) nodes.append(goal_node) else: for sub_goal in sub_goals: nodes.append(GraphNode(spec, sub_goal)) for i in range(2000): sample_point = uniformly_sampling(spec, obstacles, stage) new_node = GraphNode(spec, sample_point) nodes.append(new_node) for node in nodes: if check_distance(spec, sample_point, node.config, stage): if check_path(sample_point, node.config, spec, obstacles, stage): node.neighbors.append(new_node) new_node.neighbors.append(node) init_container = [init_node] init_visited = {init_node: [init_node.config]} while len(init_container) > 0: current = init_container.pop(0) find_goal = False if sub_goals is None: if test_config_equality(current.config, spec.goal, spec): find_goal = True else: for sub_goal in sub_goals: if test_config_equality(current.config, sub_goal, spec): find_goal = True if find_goal is True: result = init_visited[current] # return result new_list = [] for i in range(len(result) - 1): angles1 = result[i].ee1_angles angles2 = result[i + 1].ee1_angles if stage % 2 == 1: angles1 = result[i].ee2_angles angles2 = result[i + 1].ee2_angles angle_distance = [] lengths1 = result[i].lengths lengths2 = result[i + 1].lengths length_distance = [] for j in range(len(angles1)): angle_distance.append(angles2[j].__sub__(angles1[j])) length_distance.append(lengths2[j] - lengths1[j]) for j in range(501): new_angle_list = [] new_length_list = [] for k in range(len(angles1)): if stage % 2 == 0: new_angle_list.append(result[i].ee1_angles[k].__add__(angle_distance[k].__mul__(0.002 * j))) elif stage % 2 == 1: new_angle_list.append(result[i].ee2_angles[k].__add__(angle_distance[k].__mul__(0.002 * j))) new_length_list.append(result[i].lengths[k] + length_distance[k] * 0.002 * j) if stage % 2 == 0: x, y = result[i].points[0] new_config = make_robot_config_from_ee1(x, y, new_angle_list, new_length_list, ee1_grappled=True) new_list.append(new_config) elif stage % 2 == 1: x, y = result[i].points[-1] new_config = make_robot_config_from_ee2(x, y, new_angle_list, new_length_list, ee2_grappled=True) new_list.append(new_config) return new_list successors = current.get_successors() for suc in successors: if suc not in init_visited: init_container.append(suc) init_visited[suc] = init_visited[current] + [suc.config]
def randomsample(config_initial, number_samples, spec, graphnodes,a_count): """Created sample robot configs and for valid configs creates nodes New nodes are checked with existing nodes for neighbours and appended to the neighbours list""" configs = [] # Check if graphnodes is empty(1st iteration) and append source configs to it if len(graphnodes) != 0: for config in config_initial: graphnodes.append(config) i = 0 while i < number_samples: # choose a random config from base configs random_number = random.randint(0, len(config_initial) - 1) config = config_initial[random_number].config # choose a random angle and add +value to it if config.ee1_grappled == True: angles = copy.deepcopy(config.ee1_angles) elif config.ee2_grappled == True: angles = copy.deepcopy(config.ee2_angles) lengths = copy.deepcopy(config.lengths) angle_add = random.randint(-50, 50) random_number = random.randint(0, len(angles) - 1) angles[random_number] = angles[random_number] + angle_add random_number = random.randint(0, len(angles) - 1) lengths[random_number] = random.uniform(spec.min_lengths[random_number], spec.max_lengths[random_number]) # Make new config with new angle and length if config.ee1_grappled == True: config_new = make_robot_config_from_ee1(config.points[0][0], config.points[0][1], angles, config.lengths, ee1_grappled=True) elif config.ee2_grappled == True: config_new = make_robot_config_from_ee2(config.points[len(config.points) - 1][0], config.points[len(config.points) - 1][1], angles, config.lengths, ee2_grappled=True) if (valid_config(config_new, spec) == False): continue i = i + 1 configs.append(config_new) # Creating a new node with the config and updating other nodes if it is a neighbour current_node = copy.deepcopy(GraphNode(spec, config_new)) for node in graphnodes: if (neighbour_check(node, current_node, spec)) != False: node.neighbors.append(current_node) current_node.neighbors.append(node) graphnodes.append(current_node) dummy_print=[] for i in graphnodes: dummy_print.append(i.config) write_robot_config_list_to_file("test_output.txt", dummy_print) #return graphnodes sampling_specific_points = pointsBetweenPassage(spec) sampling_specific_points.extend(pointsNearObstruction(spec)) sampling_specific_points = list(set(sampling_specific_points)) sampling_specific_points = removeUnwantedPoints(sampling_specific_points, spec) if a_count==3: if config.ee1_grappled == True: config_for_every_point = config3Obstacle(sampling_specific_points, spec, spec.grapple_points, spec.min_lengths, spec.max_lengths ) elif config.ee2_grappled == True: grapple_points = copy.deepcopy(spec.grapple_points) grapple_points.reverse() min_lengths = copy.deepcopy(spec.min_lengths) min_lengths.reverse() max_lengths = copy.deepcopy(spec.max_lengths) max_lengths.reverse() config_for_every_point = config3Obstacle(sampling_specific_points, spec, grapple_points, min_lengths, max_lengths ) elif a_count==4: if config.ee1_grappled == True: config_for_every_point = config4Obstacle(sampling_specific_points, spec,spec.grapple_points, spec.min_lengths, spec.max_lengths) elif config.ee2_grappled == True: grapple_points = copy.deepcopy(spec.grapple_points) grapple_points.reverse() min_lengths = copy.deepcopy(spec.min_lengths) min_lengths.reverse() max_lengths = copy.deepcopy(spec.max_lengths) max_lengths.reverse() config_for_every_point = config4Obstacle(sampling_specific_points, spec, grapple_points, min_lengths, max_lengths ) else: return graphnodes for random_config in config_for_every_point: if random_config== []: continue else: random_config=random_config[0] configs.append(random_config) current_node = copy.deepcopy(GraphNode(spec, random_config)) for node in graphnodes: if (neighbour_check(node, current_node, spec)) != False: node.neighbors.append(current_node) current_node.neighbors.append(node) graphnodes.append(current_node) return graphnodes