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
def generate_sample(spec, config, index): angles = [] lengths = [] for i in range(spec.num_segments): angles.append(Angle(random.uniform(-165, 165))) if spec.min_lengths != spec.max_lengths: lengths.append( random.uniform(spec.min_lengths[i], spec.max_lengths[i])) else: lengths = config.lengths if index % 2 == 0: next_config = make_robot_config_from_ee1(spec.grapple_points[index][0], spec.grapple_points[index][1], angles, lengths, ee1_grappled=True, ee2_grappled=False) else: next_config = make_robot_config_from_ee2(spec.grapple_points[index][0], spec.grapple_points[index][1], angles, lengths, ee1_grappled=False, ee2_grappled=True) if detect_collision(spec, next_config): node = GraphNode(spec, next_config) return node else: return generate_sample(spec, config, index)
def interpolate_collision_check(self, node1, node2, num_links): """ :param node1: first node :param node2: second node :param num_links: number of links of robot arm :param spec: problem spec - used for obstacle detection :return: True for collision between interpolations, False for no collision """ ee1 = node1.configuration[-2:] node1_configuration = node1.configuration node2_configuration = node2.configuration configs = [node1_configuration, node2_configuration] robot_configuration = [] for i in range(len(configs)): # Remove coord points for now # print(type(robot_config)) robot_configuration.append(configs[i][2:-2]) # Convert the angles to radians for j in range(num_links): robot_configuration[i][j] = in_radians( robot_configuration[i][j]) # Get the configurations c1 = np.array(robot_configuration[0]) c2 = np.array(robot_configuration[1]) # Get the diff diff = c2 - c1 # Get the max diff max_diff = max(abs(diff)) # Get the number steps required n_steps = math.ceil(max_diff / 0.05) # Find delta for each step delta = diff / n_steps # Replace nans with 0 delta[np.isnan(delta)] = 0 # Deep copy first config ci = copy.deepcopy(c1) for step_number in range(n_steps): # Iterate over every angle and length ci = c1 + (step_number * delta) # Create robot config for new ci lengths = ci[-num_links:] angles = ci[:num_links] config = robot_config.make_robot_config_from_ee1(lengths=lengths, angles=angles, x=ee1[0], y=ee1[1], ee1_grappled=True) ee2 = list(config.get_ee2()) # If there is a collision return True if not test_obstacle_collision( config, self.spec, self.obstacle) or not self.test_self_collision( config, self.spec) or not self.inside_world_point(ee2): return True # There is no collision between the interpolation return False
def get_p_steps(c1, c2, spec, d): steps = [c1] step_amount = math.ceil(d / 0.001) length_step = [] angle1_step = [] angle2_step = [] for i in range(spec.num_segments): length_diff = c2.lengths[i] - c1.lengths[i] if length_diff != 0: length_step.append(length_diff / step_amount) else: length_step.append(0) angle1_diff = c2.ee1_angles[i] - c1.ee1_angles[i] if angle1_diff != 0: angle1_step.append(angle1_diff / step_amount) else: angle1_step.append(0) angle2_diff = c2.ee2_angles[i] - c1.ee2_angles[i] if angle2_diff != 0: angle2_step.append(angle2_diff / step_amount) else: angle2_step.append(0) for i in range(step_amount - 1): if c1.ee1_grappled and c2.ee1_grappled: new_lengths = list(map(add, steps[i].lengths, length_step)) new_angles = list(map(add, steps[i].ee1_angles, angle1_step)) new_config = make_robot_config_from_ee1(c1.points[0][0], c1.points[0][1], new_angles, new_lengths, True, False) else: new_lengths = list(map(add, steps[i].lengths, length_step)) new_angles = list(map(add, steps[i].ee2_angles, angle2_step)) new_config = make_robot_config_from_ee1(c1.points[-1][0], c1.points[-1][1], new_angles, new_lengths, False, True) if check_valid(new_config, spec): steps.append(new_config) else: return False steps += [c2] return steps
def generate_random_points(self): """ Generate N random sample points""" total = 0 while total < self.num_nodes: angles = [random.randrange(180)] # print('annnngles', angles, type(angles)) angles.extend( list(np.random.uniform(-179, 179, self.num_links - 1))) radian_angles = deepcopy(angles) for i in range(len(angles)): radian_angles[i] = in_radians(radian_angles[i]) # print(angles) lengths = np.random.uniform(self.min_lengths, self.max_lengths, self.num_links) # print('angles',angles) # print(self.initial_x) config = robot_config.make_robot_config_from_ee1( lengths=lengths, angles=radian_angles, x=self.initial_ee1_x, y=self.initial_ee1_y, ee1_grappled=True) # print(self.initial_ee1_x, self.initial_ee1_y) # print(self.initial_ee1_x, self.initial_ee1_y) point = list(config.get_ee2()) # print(point) # print(point) p = Node(point[0], point[1], total + self.num_grapples + 2, angles, self.current_grapple[0], self.current_grapple[1], lengths) ''' p = Node(np.random.uniform(self.x_min, self.x_max, 1)[0], np.random.uniform(self.y_min, self.y_max, 1)[0], total + self.num_grapples + 2, angles) ''' # print(p.point) if not in_obstacle(self.obstacle, p) and self.inside_world( p) and test_obstacle_collision( config, self.spec, self.obstacle) and self.test_self_collision( config, self.spec): self.nodes.append(p) print(point) print(angles) # print(p.configuration) # Indent line below to ensure N points generated total += 1 # print(total) print('succesful no. nodes: ', len(self.nodes))
def generate_sample(spec, grapple_point_index): """ Generates a sample robot config taking a problem spec as input. """ new_angles = generate_angles( spec.num_segments) # num_segments == num_angles new_lengths = generate_lengths(spec.min_lengths, spec.max_lengths) # for now only implementing grapple points == 1 if spec.num_grapple_points == 1: is_ee1_grappled = spec.initial.ee1_grappled robot_config = make_robot_config_from_ee1( spec.grapple_points[0][0], spec.grapple_points[0][1], new_angles, new_lengths, ee1_grappled=is_ee1_grappled, ee2_grappled=not is_ee1_grappled) else: is_ee1_grappled = True if grapple_point_index == 0 else False #This is essentially hardcoding, fix it if you have time new_x = spec.grapple_points[grapple_point_index][0] new_y = spec.grapple_points[grapple_point_index][1] if is_ee1_grappled: robot_config = make_robot_config_from_ee1( new_x, new_y, new_angles, new_lengths, ee1_grappled=is_ee1_grappled, ee2_grappled=not is_ee1_grappled) else: robot_config = make_robot_config_from_ee2( new_x, new_y, new_angles, new_lengths, ee1_grappled=is_ee1_grappled, ee2_grappled=not is_ee1_grappled) return robot_config
def interpolate_path(start_config, end_config, is_end, spec): """ produces a series of primitive steps between two RobotConfigs :param start_config: Beginning RobotConfig to check :param end_config: End RobotConfig to check :param is_end: Whether a path has been found :param spec: The ProblemSpec object defining problem :return: List of primitive steps if valid, None otherwise """ primitive_steps = [] angle_diffs = [] length_diffs = [] primitive_angles = [] primitive_lengths = [] for i in range(len(start_config.ee1_angles)): angle_diffs.append(end_config.ee1_angles[i].in_radians() - start_config.ee1_angles[i].in_radians()) length_diffs.append(end_config.lengths[i] - start_config.lengths[i]) abs_angles = [abs(angle) for angle in angle_diffs] abs_lengths = [abs(length) for length in length_diffs] segs = max(max(abs_angles), max(abs_lengths)) total_configs = int(math.ceil(segs/0.001)) for arm in range(len(start_config.ee1_angles)): primitive_angles.append(angle_diffs[arm] / total_configs) primitive_lengths.append(length_diffs[arm] / total_configs) for prim_step in range(total_configs): ee1_angles = [] lengths = [] for arm in range(len(start_config.ee1_angles)): angle = Angle(radians=start_config.ee1_angles[arm].in_radians() + (prim_step * primitive_angles[arm])) ee1_angles.append(angle) length = start_config.lengths[arm] + prim_step * primitive_lengths[arm] lengths.append(length) config = make_robot_config_from_ee1(start_config.get_ee1()[0], start_config.get_ee1()[1], ee1_angles, lengths, start_config.ee1_grappled, start_config.ee2_grappled) # If collision check happened, don't do it again if not is_end: if not test_obstacle_collision(config, spec, spec.obstacles) or not test_environment_bounds(config) \ or not test_self_collision(config, spec): return None primitive_steps.append(config) primitive_steps.append(end_config) return primitive_steps
def load_output(filename): # return a list of RobotConfig objects robot_configs = [] f = open(filename, 'r') for line in f: ee1_xy_str, ee1_angles_str, lengths_str = line.strip().split(';') ee1x, ee1y = tuple([float(i) for i in ee1_xy_str.split(' ')]) ee1_angles = [ Angle(degrees=float(i)) for i in ee1_angles_str.strip().split(' ') ] lengths = [float(i) for i in lengths_str.strip().split(' ')] robot_configs.append( make_robot_config_from_ee1(ee1x, ee1y, ee1_angles, lengths)) return robot_configs
def sample_double_grappled(spec): samples = [] for i in range(spec.num_grapple_points): for j in range(i + 1, spec.num_grapple_points): for k in range(10): c1 = make_robot_config_from_ee1(spec.grapple_points[i][0], spec.grapple_points[i][1], [], [], True, False) c2 = make_robot_config_from_ee2(spec.grapple_points[j][0], spec.grapple_points[j][1], [], [], False, True) s = extend(spec, c1, c2, [0], 0) if s is not False: samples.append(s) return samples
def generate_config(spec): """ Generates a random RobotConfig object :param spec: ProblemSpec object :return: RobotConfig object """ angles = [] lengths = [] for i in range(spec.num_segments): angles.append(Angle(degrees=random.randint(-165, 165))) lengths.append(random.uniform(spec.min_lengths[i], spec.max_lengths[i])) next_node = make_robot_config_from_ee1(spec.initial.get_ee1()[0], spec.initial.get_ee1()[1], angles, lengths, spec.initial.ee1_grappled, spec.initial.ee2_grappled) return next_node
def interpolation_path(c1, c2, spec): max_delta = dist(c1, c2, spec) n_steps = math.ceil(max_delta / 0.001) path = [c1] for i in range(1, n_steps): angles = [] lengths = [] if c1.ee1_grappled and c2.ee1_grappled: for j in range(spec.num_segments): angles.append((c2.ee1_angles[j] - c1.ee1_angles[j]).in_radians() * i / n_steps + c1.ee1_angles[j]) lengths.append((c2.lengths[j] - c1.lengths[j]) * i / n_steps + c1.lengths[j]) next_config = make_robot_config_from_ee1(c1.get_ee1()[0], c1.get_ee1()[1], angles, lengths, ee1_grappled=True, ee2_grappled=False) else: for j in range(spec.num_segments): angles.append((c2.ee2_angles[j] - c1.ee2_angles[j]).in_radians() * i / n_steps + c1.ee2_angles[j]) lengths.append((c2.lengths[j] - c1.lengths[j]) * i / n_steps + c1.lengths[j]) next_config = make_robot_config_from_ee2(c1.get_ee2()[0], c1.get_ee2()[1], angles, lengths, ee1_grappled=False, ee2_grappled=True) if i % PATH_CHECK_STEP == 0: if detect_collision(spec, next_config): path.append(next_config) else: return False else: path.append(next_config) path.append(c2) return path
def generate_random_points(self): """ Generate N random sample points""" total = 0 while total < self.num_nodes: angles = np.random.uniform(0, 180, self.num_links) lengths = np.random.uniform(self.min_lengths, self.max_lengths, self.num_links) # print(angles) # print(self.initial_x) config = robot_config.make_robot_config_from_ee1( lengths=lengths, angles=angles, x=self.initial_ee1_x, y=self.initial_ee1_y) # print(self.initial_ee1_x, self.initial_ee1_y) point = list(config.get_ee2()) # print(point) p = Node(point[0], point[1], total + self.num_grapples + 2, angles, lengths) ''' p = Node(np.random.uniform(self.x_min, self.x_max, 1)[0], np.random.uniform(self.y_min, self.y_max, 1)[0], total + self.num_grapples + 2, angles) ''' # print(p.point) if not in_obstacle(self.obstacle, p) and self.inside_world( p) and test_obstacle_collision(config, self.spec, self.obstacle): self.nodes.append(p) print(p.configuration) # Indent line below to ensure N points generated total += 1 # print(total) print('succesful no. nodes: ', len(self.nodes))
def uniformly_sampling(spec, obstacles, phase): """ Sample a configuration q by choosing a random arm posture. Check whether the random arm position is within the minimum and maximum conditions, whether it is within the obstacle, whether it collides with the obstacle or itself (if q -> F, then add to G), repeat this operation until N samples are created . Each configuration becomes a node to be added to the state diagram. """ while True: sampling_angles = [] sampling_lengths = [] for i in range(spec.num_segments): # The angle between adjacent arm segments cannot be tighter than 15 degrees # (i.e. angles 2, 3, 4... must be between -165 and +165). sample_angle = random.uniform(-165, 165) sampling_angles.append(Angle(degrees=float(sample_angle))) # The segment lengths must be within the bounds specified in the input file # (i.e. within min and max lengths) sample_length = random.uniform(spec.min_lengths[i], spec.max_lengths[i]) sampling_lengths.append(sample_length) if phase % 2 == 0: # if grapple points is 1 or odd new_config = make_robot_config_from_ee1( spec.grapple_points[phase][0], spec.grapple_points[phase][1], sampling_angles, sampling_lengths, ee1_grappled=True) else: new_config = make_robot_config_from_ee2( spec.grapple_points[phase][0], spec.grapple_points[phase][1], sampling_angles, sampling_lengths, ee2_grappled=True) # Verification inspection if collison_checking(new_config, spec, obstacles): return new_config
def interpolate_path_segment(spec, start, end, grapple_point_index): primitive_step = spec.PRIMITIVE_STEP interpolated_path = [start] is_in_tolerance = test_config_distance(start, end, spec) is_ee1_grappled = start.ee1_grappled while not is_in_tolerance: new_lengths = generate_new_vals(interpolated_path[-1].lengths, end.lengths, primitive_step) new_x = spec.grapple_points[grapple_point_index][0] new_y = spec.grapple_points[grapple_point_index][1] if is_ee1_grappled: new_angles = generate_new_vals(interpolated_path[-1].ee1_angles, end.ee1_angles, primitive_step) new_config = make_robot_config_from_ee1( new_x, new_y, new_angles, new_lengths, ee1_grappled=is_ee1_grappled, ee2_grappled=not is_ee1_grappled) else: new_angles = generate_new_vals(interpolated_path[-1].ee2_angles, end.ee2_angles, primitive_step) new_config = make_robot_config_from_ee2( new_x, new_y, new_angles, new_lengths, ee1_grappled=is_ee1_grappled, ee2_grappled=not is_ee1_grappled) if is_step_collision(spec, new_config): return [] interpolated_path.append(new_config) is_in_tolerance = test_config_distance(new_config, end, spec) return interpolated_path
def uniform_sample(spec): samples = [] sampled = 0 while (sampled < 300): grapple1 = np.random.choice(range(spec.num_grapple_points)) angles = [] for i in range(spec.num_segments): angles.append( Angle(radians=np.random.uniform(-11 / 12 * math.pi, 11 / 12 * math.pi))) lengths = np.random.uniform(spec.min_lengths[0], spec.max_lengths[0], spec.num_segments) lengths = lengths.tolist() config = make_robot_config_from_ee1(spec.grapple_points[grapple1][0], spec.grapple_points[grapple1][1], angles, lengths, ee1_grappled=True) if (check_valid(config, spec)): samples.append(config) sampled += 1 return samples
def generate_sample(spec): 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])) #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) while (not individual_config_collision_checking(spec, sample)): sample = generate_sample(spec) return sample
def find_graph_path(spec, obstacles, phase, partial_init=None, bridge_configs=None): """ This method performs a breadth first search of the state graph and return a list of configs which form a path through the state graph between the initial and the goal. Note that this path will not satisfy the primitive step requirement - you will need to interpolate between the configs in the returned list. :param spec: ProblemSpec object :param obstacles: Obstacles object :return: List of configs forming a path through the graph from initial to goal for i := range[1, N] do configs = uniformly sampling(); for config in all configs do if distance between c1 and c2 is below certain amount and path is valid do add a neighbor relationship between nodes; search graph if goal reached: break """ # set init and goal nodes, put into nodes list init_node = GraphNode(spec, spec.initial) if partial_init is None else GraphNode( spec, partial_init) nodes = [init_node] if bridge_configs is None: goal_node = GraphNode(spec, spec.goal) nodes.append(goal_node) else: for partial_goal in bridge_configs: nodes.append(GraphNode(spec, partial_goal)) # sample configs sample_number = 1000 if spec.num_segments <= 3 else 2000 for i in range(sample_number): sample_point = uniformly_sampling(spec, obstacles, phase) new_node = GraphNode(spec, sample_point) nodes.append(new_node) for node in nodes: if distance_checking(spec, phase, sample_point, node.config) \ and interpolation_checking(spec, phase, sample_point, node.config, obstacles): GraphNode.add_connection(node, new_node) # search the graph init_container = [init_node] # here, each key is a graph node, each value is the list of configs visited on the path to the graph node init_visited = {init_node: [init_node.config]} while len(init_container) > 0: current = init_container.pop(0) complete_find = False if bridge_configs is None: if test_config_equality(current.config, spec.goal, spec): complete_find = True else: for partial_goal in bridge_configs: if test_config_equality(current.config, partial_goal, spec): complete_find = True if complete_find is True: # found path to goal config_list = init_visited[current] path_list = [] for n in range(len(config_list) - 1): angles_first = config_list[ n].ee1_angles if phase % 2 == 0 else config_list[ n].ee2_angles angles_sec = config_list[ n + 1].ee1_angles if phase % 2 == 0 else config_list[ n + 1].ee2_angles between_angle = [] between_lengths = [] lengths_first = config_list[n].lengths lengths_sec = config_list[n + 1].lengths for i in range(len(angles_first)): between_angle.append(angles_sec[i].__sub__( angles_first[i])) between_lengths.append(lengths_sec[i] - lengths_first[i]) # interpolated the path into steps less than 0,001 for j in range(851): valid_angle_list = [] valid_length_list = [] for k in range(len(angles_first)): if phase % 2 == 0: valid_angle_list.append( config_list[n].ee1_angles[k].__add__( between_angle[k].__mul__(1 / 850 * j))) else: valid_angle_list.append( config_list[n].ee2_angles[k].__add__( between_angle[k].__mul__(1 / 850 * j))) valid_length_list.append(config_list[n].lengths[k] + between_lengths[k] * 1 / 850 * j) if phase % 2 == 0: x, y = config_list[n].points[0] new_config = make_robot_config_from_ee1( x, y, valid_angle_list, valid_length_list, ee1_grappled=True) path_list.append(new_config) else: x, y = config_list[n].points[-1] new_config = make_robot_config_from_ee2( x, y, valid_angle_list, valid_length_list, ee2_grappled=True) path_list.append(new_config) return path_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 interpolation_checking(spec, phase, config_first, config_sec, obstacles): """ test whether the path between two sample points is valid, the path is discretised into 20 segments, and verification checks will be applied to ensure that there are no invalid configurations in the path. :param config_first: sample points :param config_sec: exiting configs q <- config list x <- distance between angles list y <- distance between lengths of segment for j := range[0, 20] do for n := 0 to number of segments do new angles[n] = q.angles[n] + x[n]*0.05*j new lengths[n] = q.lengths[n] + y[n]*0.05*j using new angles and lengths to construct new configuration c if c is invalid -> return false return true note: 0.05 = 1/20 """ angles_first = config_first.ee1_angles if phase % 2 == 0 else config_first.ee2_angles angles_sec = config_sec.ee1_angles if phase % 2 == 0 else config_sec.ee2_angles between_angle = [] lengths_first = config_first.lengths lengths_sec = config_sec.lengths between_lengths = [] # calculate the lists of length and angle differences for i in range(len(angles_first)): between_angle.append(angles_sec[i].__sub__(angles_first[i])) between_lengths.append(lengths_sec[i] - lengths_first[i]) # discretize the each list into 20 parts for j in range(21): valid_angle_list = [] valid_length_list = [] for n in range(len(angles_first)): if phase % 2 == 0: valid_angle_list.append(config_first.ee1_angles[n].__add__( between_angle[n].__mul__(0.05 * j))) else: valid_angle_list.append(config_first.ee2_angles[n].__add__( between_angle[n].__mul__(0.05 * j))) valid_length_list.append(config_first.lengths[n] + between_lengths[n] * 0.05 * j) if phase % 2 == 0: x, y = config_first.points[0] new_config = make_robot_config_from_ee1(x, y, valid_angle_list, valid_length_list, ee1_grappled=True) else: x, y = config_first.points[-1] new_config = make_robot_config_from_ee2(x, y, valid_angle_list, valid_length_list, ee2_grappled=True) if not (collison_checking(new_config, spec, obstacles)): return False return True
def bridge_config(spec, config, index): angles = [] lengths = [] for i in range(spec.num_segments - 1): angles.append(Angle(degrees=random.uniform(-165, 165))) if spec.min_lengths != spec.max_lengths: lengths.append( random.uniform(spec.min_lengths[i], spec.max_lengths[i])) else: lengths = config.lengths[:-1] if index % 2 == 0: next_config = make_robot_config_from_ee1(spec.grapple_points[index][0], spec.grapple_points[index][1], angles, lengths, ee1_grappled=True, ee2_grappled=False) last_point = next_config.points[-1] else: next_config = make_robot_config_from_ee2(spec.grapple_points[index][0], spec.grapple_points[index][1], angles, lengths, ee1_grappled=False, ee2_grappled=True) last_point = next_config.points[0] delta_x = spec.grapple_points[index + 1][0] - last_point[0] delta_y = spec.grapple_points[index + 1][1] - last_point[1] last_length = (delta_x**2 + delta_y**2)**0.5 if delta_x == 0: if delta_y > 0: last_angle = math.pi / 2 else: last_angle = -math.pi / 2 elif delta_x > 0: last_angle = math.atan(delta_y / delta_x) else: last_angle = math.atan(delta_y / delta_x) + math.pi last_angle = Angle(last_angle) for angle in angles: last_angle -= angle angles.append(last_angle) if index % 2 == 0: lengths.append(last_length) else: lengths.insert(0, last_length) if index % 2 == 0: final_config = make_robot_config_from_ee1( spec.grapple_points[index][0], spec.grapple_points[index][1], angles, lengths, ee1_grappled=True, ee2_grappled=True) else: final_config = make_robot_config_from_ee2( spec.grapple_points[index][0], spec.grapple_points[index][1], angles, lengths, ee1_grappled=True, ee2_grappled=True) if detect_collision(spec, final_config): return GraphNode(spec, final_config) else: return bridge_config(spec, config, index)
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 self.goal_x = goal_eex self.goal_y = goal_eey self.goal_angles = [] for i in range(len(goal_angles)): self.goal_angles.append(in_degrees(goal_angles[i].radians)) self.initial_angles = [] for i in range(len(initial_angles)): self.initial_angles.append(in_degrees(initial_angles[i].radians))
def main(arglist): start = time.time() # print(np.random.uniform(0, 100, size=10)) print(arglist) testcase = arglist[0] problem_spec = ProblemSpec(testcase) # print(problem_spec.obstacles[0].corners) bottom_left_corner = [] x_length = [] y_length = [] for j in range(len(problem_spec.obstacles)): bottom_left_corner.append(problem_spec.obstacles[j].corners[0]) # print(bottom_left_corner) for j in range(len(problem_spec.obstacles)): x_length.append(problem_spec.obstacles[j].x_length) y_length.append(problem_spec.obstacles[j].y_length) # print(problem_spec.obstacles[0].corners[0][0]) # print('bc', bottom_left_corner[0][0]) # Create a PRM with X min, X max, Y min, X max, N samples, Obstacles, Grapple points, EE2 goal_x, EE2_goal_y, EE2_initial_x, EE2_initial_y, min_lengths, max_lengths, initial_ee1 prm = PRM(1, 0, 1, 0, 100, problem_spec.obstacles, problem_spec.grapple_points, problem_spec.goal.get_ee2()[0], problem_spec.goal.get_ee2()[1], problem_spec.initial.get_ee2()[0], problem_spec.initial.get_ee2()[1], problem_spec.min_lengths, problem_spec.max_lengths, problem_spec.initial.get_ee1(), problem_spec.goal_angles, problem_spec.initial_angles, problem_spec) # print(problem_spec.obstacles) # print('goal', problem_spec.goal_x) # Generate random points for the prm prm.generate_random_points() # Get k neighbours for the random points prm.get_k_neighbours() # print('obs', in_obstacle(problem_spec.obstacles[0],(0.3,0.2))) path = prm.solve() print('configs to goal', path) new_path = interpolate(path, problem_spec.num_segments) # We need to write a list of primitive steps, extrapolating between the configurations # EE1x, EE1y; angle1, angle2 ... anglen; length1, length 2 ... length 3 write_robot_config_list_to_file(arglist[1], new_path, problem_spec.num_segments) print('Time required = ', -start + time.time()) ''' for i in range(len(list)): print(list[i].configuration) ''' k = 0 x = [] y = [] for i in range(len(prm.nodes)): # print(prm.nodes[i].point) x.append(prm.nodes[i].point[0]) y.append(prm.nodes[i].point[1]) ''' For testing purposes ''' print('grapple points', problem_spec.grapple_points) print('Goal point', problem_spec.goal) print('Goal EE1', problem_spec.goal.get_ee1()) print('Goal EE2', problem_spec.goal.get_ee2()) print('Obstacles', problem_spec.obstacles[0].edges[0][0]) print('lengths', problem_spec.min_lengths, problem_spec.max_lengths) test = robot_config.make_robot_config_from_ee1( lengths=[0.2, 0.2, 0.2], angles=[30, 30, 30], x=problem_spec.initial.get_ee1()[0], y=problem_spec.initial.get_ee1()[0]) # print('Configuration: ', prm.nodes[9].configuration) print('Configuration goal: ', prm.nodes[0].configuration) # print('goal angles prm', problem_spec.goal_angles) # p1 = [0, 0] # p2 = [2, 2] # dist = math.sqrt( (p1[0] - p2[0])**2 + (p1[1] - p2[1])**2 ) # Take away from the tests below is that key=lambda lets you control which part is used for sorting # t = ((5,99), (6,20), (4,55), (3,200000)) # test = sorted(t, key=lambda x:x[1]) # print(test) # print(dist) '''''' fig = plt.figure(testcase) ax = fig.add_subplot(1, 1, 1) ''' Plot the obstacles''' for i in range(len(problem_spec.obstacles)): ax.add_patch( patches.Rectangle(bottom_left_corner[i], x_length[i], y_length[i], zorder=-1)) ''' Plot the sampled points ''' plt.scatter(x, y, color='red', s=30) ''' Plot the grapple points''' for i in range(len(problem_spec.grapple_points)): plt.scatter(problem_spec.grapple_points[i][0], problem_spec.grapple_points[i][1], color='blue', s=30) ''' Plot the goal point''' if prm.num_grapples % 2 == 0: print(prm.num_grapples) print('went through if') plt.scatter(problem_spec.goal.get_ee1()[0], problem_spec.goal.get_ee1()[1], color='green', s=100) else: print('else') plt.scatter(problem_spec.goal.get_ee2()[0], problem_spec.goal.get_ee2()[1], color='green', s=100) ''' Plot the start EE point''' plt.scatter(problem_spec.initial.get_ee2()[0], problem_spec.initial.get_ee2()[1], color='black', s=100) # plt.scatter(bottom_left_corner[0], bottom_left_corner[1], color='red', s=3000) # print(x_length) ''' Plot the potential paths between nodes''' for i in range(len(prm.nodes)): for j in range(len(prm.nodes[i].neighbours)): # print('SCATTER', prm.nodes[i].neighbours[j]) plt.plot([prm.nodes[i].x, prm.nodes[i].neighbours[j].x], [prm.nodes[i].y, prm.nodes[i].neighbours[j].y], 'y-') # Plot the correct path try: for i in range(len(path) - 1): # print(path[i]) plt.plot([path[i][0], path[i + 1][0]], [path[i][1], path[i + 1][1]], 'r-', zorder=3) counter = i except: pass # print(i) # plt.plot([x1, x2], [y1, y2], 'k-') plt.title(testcase) plt.xlabel('X coordinates') plt.ylabel('Y coordinates') plt.show()
def __init__(self, x_max, x_min, y_max, y_min, N, obstacle, grapple_points, goal_x, goal_y, initial_x, initial_y, min_lengths, max_lengths, initial_ee1, goal_angles, initial_angles, spec): self.x_max = x_max self.y_max = y_max self.x_min = x_min self.y_min = y_min self.num_nodes = N self.nodes = [] self.obstacle = obstacle self.current_grapple = list(grapple_points[0]) self.goal_grapple = [spec.goal_x, spec.goal_y] self.goal_length = spec.goal.lengths self.goal_angles = goal_angles self.initial_angles = initial_angles self.initial_ee1_x = initial_ee1[0] self.initial_ee1_y = initial_ee1[1] self.initial_length = spec.initial.lengths self.grapple = grapple_points self.min_lengths = min_lengths self.max_lengths = max_lengths self.num_links = len(min_lengths) self.initial_x = initial_x self.initial_y = initial_y self.goal = [goal_x, goal_y] self.spec = spec self.num_grapples = len(self.grapple) self.iteration = 0 """ Add the goal as a node - Always has ID of 0 """ if self.num_grapples % 2 == 0: goal = list(spec.goal.get_ee1()) self.nodes.append( Node(spec.goal.get_ee1()[0], spec.goal.get_ee1()[1], 0, self.goal_angles, x2=self.goal_grapple[0], y2=self.goal_grapple[1], length=self.goal_length)) else: self.nodes.append( Node(goal_x, goal_y, 0, self.goal_angles, x2=self.goal_grapple[0], y2=self.goal_grapple[1], length=self.goal_length)) # print(goal_x, goal_y, self.goal_angles) print('goal node', self.nodes[0].configuration) """ Add initial EE2 as node - Always had ID of 1 """ self.nodes.append( Node(initial_x, initial_y, 1, self.initial_angles, self.initial_ee1_x, self.initial_ee1_y, self.initial_length)) ''' Add grapple points as nodes - ID always 2 - num. grapple pts ''' if self.num_grapples > 1: grapple_configuration = self.bridge_me_bb(self.grapple[0], self.grapple[1]) self.nodes.append( Node(grapple_configuration[0], grapple_configuration[1], 2, grapple_configuration[2:2 + self.num_links], grapple_configuration[-2], grapple_configuration[-1], grapple_configuration[2 + self.num_links:-2])) self.nodes.append( Node(grapple_configuration[0], grapple_configuration[1], 3, grapple_configuration[2:2 + self.num_links], grapple_configuration[-2], grapple_configuration[-1], grapple_configuration[2 + self.num_links:-2])) node = self.nodes[2].configuration # print('node', node) angles = node[2:2 + self.num_links] for i in range(len(angles)): angles[i] = in_radians(angles[i]) # print(angles) lengths = node[2 + self.num_links:-2] # print('lengths', lengths) c = make_robot_config_from_ee1(node[-2], node[-1], angles, node[2 + self.num_links:-2], ee1_grappled=True) # print('ee2 angles', c.ee2_angles) # print('ee1 angles', c.ee1_angles) # print(c.ee2_angles[0].in_degrees()) angles = [c.ee2_angles[0].in_radians()] angles.extend(c.ee2_angles[1:]) # print('anggles', angles) lengths = c.lengths[:] lengths.reverse() for i in range(len(angles)): angles[i] = in_degrees(angles[i]) # new_node = Node(node[-2], node[-1], 2, angles, node[0], node[1], lengths) new_config = [node[-2], node[-1]] new_config.extend(angles) new_config.extend(lengths) new_config.extend([node[0], node[1]]) # print('NC', new_config) self.nodes[3].configuration = new_config ''' for i in range(len(grapple_points) - 1): print(grapple_points[i + 1]) self.nodes.append(Node(grapple_points[i + 1][0], grapple_points[i + 1][1], i + 2, 1)) ''' # print(self.nodes[0]) # print(goal_x, goal_y) # self.nodes.append(Node(1,1,1)) self.num_grapples = len(grapple_points)
def bridge_me_bb(self, grapple1, grapple2): """ Finds and returns the configuration to create a bridge between two grapple points :param grapple1: list of grapple point coords [x,y] :param grapple2: list of grapple point coords [x,y] :param joint_N1: coords of N-1 arm joint [x,y] :param joint_N2: coords of N-2 arm joint [x,y] :param num_links: number of links :param length_n: length of N-1 arm link :param ee1: coords of ee1 [x,y] :return: a configuration list [ee2x, ee2y, a1, a2 ... an, l1, l2 ... ln, ee1x, ee2y] """ while True: configuration = [grapple2[0], grapple2[1]] # Create random angles and lengths for N - 1 links # Angles are in radians angles = list(np.random.uniform(0, np.pi, 1)) # print(angles) angles.extend( list(np.random.uniform(-2.87979, 2.87979, self.num_links - 2))) # print(self.min_lengths, self.max_lengths, self.num_links - 1) lengths = list( np.random.uniform(self.min_lengths[0], self.max_lengths[0], self.num_links - 1)) config = make_robot_config_from_ee1(grapple1[0], grapple1[1], angles, lengths, ee1_grappled=True) joint_n1 = config.points[-1] joint_n2 = config.points[-2] length_n = lengths[-1] # Calculate parameters b = length_n a = np.sqrt((grapple2[0] - joint_n1[0])**2 + (grapple2[1] - joint_n1[1])**2) c = np.sqrt((grapple2[0] - joint_n2[0])**2 + (grapple2[1] - joint_n2[1])**2) c_angle = np.arccos((a**2 + b**2 - c**2) / (2 * a * b)) a_nangle = -(np.pi - c_angle) # If parameters satisfy conditions create a configuration from them and return it angles.append(a_nangle) lengths.append(a) config = make_robot_config_from_ee1(grapple1[0], grapple1[1], angles, lengths, ee1_grappled=True, ee2_grappled=True) if self.min_lengths[0] <= a <= self.max_lengths[ 0] and -2.87979 <= a_nangle <= 2.87979 and test_obstacle_collision( config, self.spec, self.obstacle) and self.test_self_collision( config, self.spec): for i in range(len(angles)): angles[i] = in_degrees(angles[i]) print(config.get_ee1(), config.get_ee2()) configuration.extend(angles) configuration.extend(lengths) configuration.append(grapple1[0]) configuration.append(grapple1[1]) print('config of bridge!!!!!', configuration) return configuration
def interpolate(robot_configuration, num_links): new_robot_config_list = [] # Get pos EE1 ee1 = robot_configuration[0][-2:] robot_config = [] copy_robot_config = copy.deepcopy(robot_configuration) for i in range(len(copy_robot_config)): # Remove angles and EE1 coords del copy_robot_config[i][2:2 + num_links] robot_config.append(copy_robot_config[i][:-2]) print('robot_config', robot_config) # For each node for i in range(len(robot_config) - 1): initial_angles = robot_configuration[i][2:2 + num_links] for angle in range(len(initial_angles)): initial_angles[angle] = in_radians(initial_angles[angle]) # print('len', len(robot_config)) c1 = np.array(robot_config[i]) c2 = np.array(robot_config[i + 1]) # print("c1c2", c1, c2) diff = c2 - c1 # print('c1', c1) # print('diff', diff) max_diff = max(abs(diff)) n_steps = math.ceil(max_diff / 0.001) # print('steps', n_steps) delta = diff / n_steps # print('delta', delta) # Remove nans and replace for 0 delta[np.isnan(delta)] = 0 # print('new delta', delta) # n_steps = n_steps.astype(int) # print('rounded', n_steps) ci = copy.deepcopy(c1) # print('c1 here',c1) # print("links", num_links*2) # Iterate over every step for step_number in range(n_steps): # Iterate over every angle and length # print("NUM STEPS!!!!", n_steps[j]) ci = c1 + (step_number * delta) print(ci) # print('c1__', c1) # print('c1[j], [k], delta[j], k*delta[j])', j, c1[j], [k], delta[j], k * delta[j]) # Convert back to degrees # for angle in range(num_links): # ci[angle] = in_degrees(ci[angle]) # ci.insert(0, robot_configuration[i][1]) # ci.insert(0, robot_configuration[i][0]) if num_links == 3: ## q2 = q2 - q1 # Fix the first angle angle_1 = initial_angles[0] length_1 = ci[2] print(ci[2:]) print(ee1) a_configuration = make_robot_config_from_ee1( x=ee1[0], y=ee1[1], angles=initial_angles, lengths=ci[2:]) print('pts', a_configuration.points) # Get end position of first link print('init angles', initial_angles) print('ci[2:]', ci[2:]) first_link = list(a_configuration.points[1]) f_x = first_link[0] f_y = first_link[1] a1 = ci[-2] a2 = ci[-1] # Get the distance to move from the non fixed link x = ci[0] - f_x y = ci[1] - f_y print('the config', robot_configuration[i], robot_configuration[i + 1]) print('compare', first_link, (ci[0], ci[1])) print(a1, a2, x, y) result = None ''' while result is None: try: angle_list = inverse_kinematics(a1, a2, x, y) result = 1 except: initial_angles[0] += 0.001 a_configuration = make_robot_config_from_ee1(x=ee1[0], y=ee1[1], angles=initial_angles, lengths=ci[2:]) first_link = list(a_configuration.points[1]) f_x = first_link[0] f_y = first_link[1] a1 = ci[-2] a2 = ci[-1] # Get the distance to move from the non fixed link x = ci[0] - f_x y = ci[1] - f_y ''' try: angle_list = inverse_kinematics(a1, a2, x, y) result = 1 except: pass angle_2 = angle_list[0][0] angle_3 = angle_list[0][1] elif num_links == 4: # Fix the first angle and second angle angle_1 = robot_configuration[i][2] length_1 = ci[2] angle_2 = robot_configuration[i][3] length_2 = ci[3] a1 = ci[-2] a2 = ci[-1] x = ci[0] y = ci[1] angle_list = inverse_kinematics(a1, a2, x, y) angle_2 = angle_list[1][0] angle_3 = angle_list[1][1] angle_1 = initial_angles[0] ci = list(ci) ci.append(robot_configuration[i][-2]) ci.append(robot_configuration[i][-1]) ci.insert(2, in_degrees(angle_3)) ci.insert(2, in_degrees(angle_2)) ci.insert(2, in_degrees(angle_1)) # print('new_ci', ci) new_robot_config_list.append(ci) print('c2', c2) c2 = list(c2) c2.insert(2, robot_configuration[-1][4]) c2.insert(2, robot_configuration[-1][3]) c2.insert(2, robot_configuration[-1][2]) c2.append(robot_configuration[i][-2]) c2.append(robot_configuration[i][-1]) print('c2 nw', c2) new_robot_config_list.append(c2) # print('a config', new_robot_config_list) # print(new_robot_config_list[-1]) # print(new_robot_config_list[-5]) # <- DO NOT Uncomment Unless you want a BAD TIME... IT'S HUGE print(len(new_robot_config_list)) return new_robot_config_list
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
def extend(spec, config1, config2, total_trials, extended): while (total_trials[0] < 1000): remained_length = (spec.num_segments - extended) * spec.max_lengths[0] trials = 0 if extended < spec.num_segments - 2: # remained_length = (spec.num_segments - extended) * spec.max_lengths[0] d = distance.euclidean(config1.points[-1], config2.points[0]) if d > remained_length: return False angle = angle_from_p1_to_p2(config1.points[-1], config2.points[0]) success = False while trials < 100 and not success: if remained_length > 2 * d: seg_angle = Angle(radians=np.random.rand() * math.pi * 2) seg_length = spec.min_lengths[0] else: seg_angle = Angle( radians=np.random.normal(angle.in_radians(), math.pi / 6)) seg_length = np.random.uniform(spec.min_lengths[0], spec.max_lengths[0]) success = True new_angles = config1.ee1_angles + [ seg_angle - sum(config1.ee1_angles) ] new_lengths = config1.lengths + [seg_length] new_config1 = make_robot_config_from_ee1(config1.points[0][0], config1.points[0][1], new_angles, new_lengths, ee1_grappled=True) if distance.euclidean( new_config1.points[-1], config2.points[-1] ) > remained_length - spec.max_lengths[0]: success = False elif not check_valid(new_config1, spec): success = False trials += 1 total_trials[0] += 1 if not success: return False remained_length -= spec.max_lengths[0] angle = angle_from_p1_to_p2(config2.points[0], new_config1.points[-1]) success = False while trials < 100 and not success: if remained_length > 2 * d: seg_angle = Angle(radians=np.random.rand() * math.pi * 2) seg_length = spec.min_lengths[0] else: seg_angle = Angle( radians=np.random.normal(angle.in_radians(), math.pi / 6)) seg_length = np.random.uniform( (spec.min_lengths[0], spec.max_lengths[0])) success = True new_angles = config2.ee2_angles + [ seg_angle - sum(config2.ee2_angles) ] new_lengths = config2.lengths + [seg_length] new_config2 = make_robot_config_from_ee2(config2.points[-1][0], config1.points[-1][1], new_angles, new_lengths, ee2_grappled=True) if not check_valid(new_config2, spec): success = False trials += 1 total_trials[0] += 1 if not success: return False result = extend(spec, new_config1, new_config2, total_trials, extended + 2) if result is not False: return result elif spec.num_segments - extended == 2: p1 = config1.points[-1] p2 = config2.points[0] d = distance.euclidean(p1, p2) if d > 2 * spec.max_lengths[0]: return False elif d > 2 * spec.min_lengths[0]: length1 = d / 2 length2 = d / 2 angle1 = angle_from_p1_to_p2(p1, p2) - sum(config1.ee1_angles) angle2 = Angle(radians=0) angle3 = angle_from_p1_to_p2( p2, config2.points[1]) - angle_from_p1_to_p2(p1, p2) new_lengths = config1.lengths + [length1, length2 ] + config2.lengths new_angles = config1.ee1_angles + [angle1, angle2, angle3] + [ angle for angle in config2.ee1_angles[1:] ] combined_config = make_robot_config_from_ee1( config1.points[0][0], config1.points[0][1], new_angles, new_lengths, ee1_grappled=True, ee2_grappled=True) if check_valid((combined_config, spec)): return combined_config else: return False else: return False elif spec.num_segments - extended == 1: p1 = config1.points[-1] p2 = config2.points[0] d = distance.euclidean(p1, p2) if d > spec.max_lengths[0]: return False elif d > spec.min_lengths[0]: angle1 = angle_from_p1_to_p2(p1, p2) - sum(config1.ee1_angles) angle2 = angle_from_p1_to_p2( p2, config2.points[1]) - angle_from_p1_to_p2(p1, p2) new_lengths = config1.lengths + [d] + config2.lengths new_angles = config1.ee1_angles + [angle1, angle2] + [ angle for angle in config2.ee2_angles[1:] ] combined_config = make_robot_config_from_ee1( config1.points[0][0], config1.points[0][1], new_angles, new_lengths, ee1_grappled=True, ee2_grappled=True) if check_valid((combined_config, spec)): return combined_config else: return False else: return False return False
def main(arglist): #def main(): input_file = arglist[0] output_file = arglist[1] #input_file = 'testcases/3g2_m1.txt' #output_file = 'new_output.txt' spec = ProblemSpec(input_file) init_node = GraphNode(spec, spec.initial) goal_node = GraphNode(spec, spec.goal) # Keep track of all samples generated nodes_in_graph = [] nodes_in_graph.append(init_node) nodes_in_graph.append(goal_node) #bridge = [] #new_bridge = create_bridge_config(spec) #bridge.append(new_bridge) #bridge.append(new_bridge) #bridge.append(new_bridge) #bridge.append(new_bridge) #bridge.append(new_bridge) #bridge.append(new_bridge) #bridge.append(new_bridge) #bridge.append(new_bridge) #bridge.append(new_bridge) #write_robot_config_list_to_file(output_file, bridge) #exit() search_remaining = True #connections_added = 0 while search_remaining: # Generate X new Samples for i in range(10): sample = generate_sample(spec) sample_node = GraphNode(spec, sample) # Connect sample to all samples if d < 0.2 and interpolated_path != None for node in nodes_in_graph: distance_bw_nodes = abs(sample.points[-1][1] - node.config.points[-1][1]) + abs( sample.points[-1][0] - node.config.points[-1][0]) #print(distance_bw_nodes) if distance_bw_nodes < 0.2: # Lazy Check mid_angles = [] mid_angles.extend(node.config.ee1_angles) for angle in range(len(sample.ee1_angles)): mid_angles[angle] = ( mid_angles[angle].in_degrees() + sample.ee1_angles[angle].in_degrees()) / 2 mid_lengths = [] mid_lengths.extend(node.config.lengths) for length in range(len(sample.lengths)): mid_lengths[length] = (mid_lengths[length] + sample.lengths[length]) / 2 mid_new_config = make_robot_config_from_ee1( sample.points[0][0], sample.points[0][1], mid_angles, mid_lengths, True) if individual_config_collision_checking( spec, mid_new_config) == False: # False shouldn't work GraphNode.add_connection(node, sample_node) #connections_added = connections_added + 1 # Full Check #interpolated_path = interpolate_path(node.config, sample_node.config, spec) #if interpolated_path != None: #GraphNode.add_connection(node, sample_node) #connections_added = connections_added + 1 nodes_in_graph.append(sample_node) #print(connections_added) # Search Graph path_found = find_graph_path(spec, init_node) if path_found != None: final_path = [] searched_paths = 0 for path_no in range(len(path_found) - 1): path_yay = interpolate_path(path_found[path_no], path_found[path_no + 1], spec) if path_yay == None: print("Some Error") for node in nodes_in_graph: if node == GraphNode(spec, path_found[path_no]): first_node = node elif node == GraphNode(spec, path_found[path_no + 1]): second_node = node first_node.neighbors.remove(second_node) second_node.neighbors.remove(first_node) break else: print("yay") final_path.extend(path_yay) # Break when goal reached searched_paths += 1 #search_completed = False if searched_paths == len(path_found) - 1: write_robot_config_list_to_file(output_file, final_path) search_remaining = False else: print("no path found") #GraphNode.add_connection(init_node, goal_node) #interpolate_path(spec.initial, spec.goal) steps = []
def interpolate_path(config_1, config_2, spec): path = [] path.append(config_1) # Make copy of config_1 new_config = make_robot_config_from_ee1(config_1.points[0][0], config_1.points[0][1], config_1.ee1_angles, config_1.lengths, True) # Number of Segments and Angles to iterate over no_of_angles = len(config_1.ee1_angles) list_delta = [] list_length_delta = [] # Add Delta i.e. InitConfig.angle - FinalConfig.angle for theta_angle in range(no_of_angles): list_delta.append((config_1.ee1_angles[theta_angle].in_radians() - config_2.ee1_angles[theta_angle].in_radians())) list_length_delta.append( (config_1.lengths[theta_angle] - config_2.lengths[theta_angle])) max_delta = 0 for delta in list_delta: if abs(delta) > max_delta: max_delta = abs(delta) max_length_delta = 0 for delta in list_length_delta: if abs(delta) > max_length_delta: max_length_delta = abs(delta) no_of_steps = max_delta / 0.001 no_of_steps_length = max_length_delta / 0.001 while (True): # TO DO - MANUAL COPY INSTEAD OF DEEP COPY # Deepcopy previous angles and lengths and change them on this config new_angles = copy.deepcopy(new_config.ee1_angles) new_lengths = copy.deepcopy(new_config.lengths) for angle_no in range(no_of_angles): # Increase Angles if (abs(new_config.ee1_angles[angle_no].in_radians() - config_2.ee1_angles[angle_no].in_radians()) >= 0.001): increment_val = -(list_delta[angle_no] / no_of_steps) new_angles[angle_no] = Angle( new_config.ee1_angles[angle_no].in_radians() + increment_val) # Increase Lengths if (abs(new_config.lengths[angle_no] - config_2.lengths[angle_no]) >= 0.001): increment_val = -(list_length_delta[angle_no] / no_of_steps_length) new_lengths[ angle_no] = new_config.lengths[angle_no] + increment_val new_config = make_robot_config_from_ee1(new_config.points[0][0], new_config.points[0][1], new_angles, new_lengths, True) #for angles in new_angles: #print(angles.in_degrees()) #for angle_no in range(no_of_angles): #print(new_config.ee1_angles[angle_no], end = "") #print(" ", end="") #print("") if individual_config_collision_checking(spec, new_config) == False: #print("F**k") #exit() return None path.append(new_config) if check_last_step_interpolation(new_config, config_2): break path.append(config_2) return path