def write_robot_config_list_to_file(filename, robot_config_list, num_links): """ Write an output file for the given list of RobotConfigs. We recommend using this method to generate your output file. :param filename: name of output file (e.g. 2nd argument given to program) :param robot_config_list: list of RobotConfig objects forming a path :param num_links: The number of links for the robot arm """ f = open(filename, 'w') j = 0 for rc in range(len(robot_config_list)): lengths = [] angles = [] # Create robot config to get EE1 positions for i in range(num_links): lengths.append(robot_config_list[rc][-i]) angles.append(robot_config_list[rc][i + 2]) config = robot_config.make_robot_config_from_ee2( lengths=lengths, angles=angles, x=robot_config_list[rc][0], y=robot_config_list[rc][0]) ee1 = list(config.get_ee1()) f.write("{0}; {1}; {2}\n".format( str(ee1)[1:-1], str(angles)[1:-1], str(lengths)[1:-1])) f.close()
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 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 sample(spec): samples = [] samples += sample_double_grappled(spec) samples += uniform_sample(spec) double_samples = [] for s in samples: angles = s.ee1_angles.copy() lengths = s.lengths.copy() lengths.reverse() double_samples.append( make_robot_config_from_ee2(s.points[0][0], s.points[0][1], angles, lengths, ee1_grappled=s.ee2_grappled, ee2_grappled=s.ee1_grappled)) samples += double_samples Visualiser(spec, samples) return samples
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_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 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 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 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 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 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 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 generate_random_points(self): """ Generate N random sample points""" print('grapple', self.grapple) for i in range(self.num_grapples): total = 0 grapple = list(self.grapple[i]) print('n_gple', grapple) while total < round(self.num_nodes / self.num_grapples): angles = [random.randrange(180)] # print('annnngles', angles, type(angles)) angles.extend( list(np.random.uniform(-165, 165, 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[0], self.max_lengths[0], self.num_links) # print('angles',angles) # print(self.initial_x) if i % 2 == 0: config = robot_config.make_robot_config_from_ee1( lengths=lengths, angles=radian_angles, x=grapple[0], y=grapple[1], ee1_grappled=True) point = list(config.get_ee2()) p = Node(point[0], point[1], total + self.num_grapples + 3, angles, self.current_grapple[0], self.current_grapple[1], lengths) else: config = robot_config.make_robot_config_from_ee2( lengths=lengths, angles=radian_angles, x=grapple[0], y=grapple[1], ee2_grappled=True) point = list(config.get_ee1()) p = Node(point[0], point[1], total + self.num_grapples + 3, angles, grapple[0], grapple[1], lengths) # print(self.initial_ee1_x, self.initial_ee1_y) # print(self.initial_ee1_x, self.initial_ee1_y) # print(point) # print(point) ''' 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))