def inter_config(c1: robot_config.RobotConfig, c2: robot_config.RobotConfig, spec: problem_spec.ProblemSpec): c1angles = c1.ee1_angles c1lengths = c1.lengths ee = c1.get_ee1() #interpolate mid between confs newAngles = [] for idx, angObj in enumerate(c1angles): curr_rad = angObj.in_radians() end_rad = c2.ee1_angles[idx].in_radians() mid_rad = (curr_rad + end_rad) / 2 newAngle = angle.Angle(radians=mid_rad) newAngles.append(newAngle) newLen = [] for idx, segment in enumerate(c1lengths): newSeg = (segment + c2.lengths[idx]) / 2 newLen.append(newSeg) newConf = robot_config.make_robot_config_from_ee1(ee[0], ee[1], newAngles, newLen, True, False) return newConf
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 removeUnwantedPoints(points, spec): # Remove unwatned points # length should not be greater than the sum of all lenght # length should not be less than that of min config., that we got by 15 degrees angle initial_point = spec.initial.points[0] config = spec.initial # list the unexpected angle unacceptable_angles = [[15, 15, 15], [15, 15, -15], [15, -15, 15], [-15, 15, 15], [15, -15, -15], [-15, 15, -15], [-15, 15, -15], [-15, -15, -15]] # for these unexpected angles d = [0] * len(unacceptable_angles) for an in range(len(unacceptable_angles)): config_plus_15 = make_robot_config_from_ee1(config.points[0][0], config.points[0][1], unacceptable_angles[an], spec.max_lengths, ee1_grappled=True) d[an] = distancePoints(initial_point, config_plus_15.points[-1]) for p in points: # if the distance is greater then maz lenght if ((distancePoints(p, initial_point) > sum(spec.max_lengths)) # or less than min distnace or (distancePoints(p, initial_point) < min(d))): points.remove(p) return points
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 random_steps(config: robot_config.RobotConfig, spec: problem_spec.ProblemSpec, goal_coord: tuple, dist_to_grap: float): RAD_THRESH = RAD_THRESH_GLOBAL LEN_THRESH = LEN_THRESH_GLOBAL ee = config.get_ee1() if config.ee1_grappled else config.get_ee2() lengths = config.lengths angles = config.ee1_angles if config.ee1_grappled else config.ee2_angles rand_configs = [] total_bots = 0 while (len(rand_configs) < N and total_bots < 100): # print(len(rand_configs)) newAngles = [] for angObj in angles: curr_rad = angObj.in_radians() lower_limit = (curr_rad-math.pi*RAD_THRESH) if ((curr_rad-math.pi*RAD_THRESH)>=(-MAX_RADIANS) and\ (curr_rad-math.pi*RAD_THRESH)<=MAX_RADIANS) else -MAX_RADIANS upper_limit = (curr_rad+math.pi*RAD_THRESH) if ((curr_rad+math.pi*RAD_THRESH)>=(-MAX_RADIANS) and\ (curr_rad+math.pi*RAD_THRESH)<=MAX_RADIANS) else MAX_RADIANS # lower_limit = -math.pi # upper_limit = math.pi newAngle = angle.Angle( radians=(random.uniform(lower_limit, upper_limit))) newAngles.append(newAngle) newLen = [] for idx, segment in enumerate(lengths): lower_limit = (segment - LEN_THRESH_GLOBAL) if ( segment - LEN_THRESH_GLOBAL ) > spec.min_lengths[idx] else spec.min_lengths[idx] upper_limit = (segment + LEN_THRESH_GLOBAL) if ( segment + LEN_THRESH_GLOBAL ) < spec.max_lengths[idx] else spec.max_lengths[idx] # lower_limit = spec.min_lengths[idx] # upper_limit = spec.max_lengths[idx] newSeg = random.uniform(lower_limit, upper_limit) newLen.append(newSeg) newConf = robot_config.make_robot_config_from_ee1( ee[0], ee[1], newAngles, newLen, True, False) # man_dist_to_grapple = heuristic(newConf, goal_config) #goal_points = goal_config.get_ee2() man_dist_to_grapple = distance_to_grapple(newConf, goal_coord) # print(man_dist_to_grapple) # print(len(rand_configs)) total_bots += 1 # print(total_bots) if ((config_validity(newConf, spec) and \ (man_dist_to_grapple<dist_to_grap))): #and \inter_config_validity(config, newConf, spec)): rand_configs.append(newConf) return rand_configs
def generate_bridge_bots(start_conf: robot_config.RobotConfig, spec: problem_spec.ProblemSpec): RAD_THRESH = RAD_THRESH_GLOBAL LEN_THRESH = LEN_THRESH_GLOBAL valid_bridges = [] ee = start_conf.get_ee1() lengths = start_conf.lengths angles = start_conf.ee1_angles while (len(valid_bridges) < 1): newAngles = [] for angObj in angles: curr_rad = angObj.in_radians() lower_limit = (curr_rad-math.pi*RAD_THRESH) if ((curr_rad-math.pi*RAD_THRESH)>=(-MAX_RADIANS) and\ (curr_rad-math.pi*RAD_THRESH)<=MAX_RADIANS) else -MAX_RADIANS upper_limit = (curr_rad+math.pi*RAD_THRESH) if ((curr_rad+math.pi*RAD_THRESH)>=(-MAX_RADIANS) and\ (curr_rad+math.pi*RAD_THRESH)<=MAX_RADIANS) else MAX_RADIANS newAngle = angle.Angle( radians=(random.uniform(lower_limit, upper_limit))) newAngles.append(newAngle) newLen = [] for idx, segment in enumerate(lengths): lower_limit = (segment - LEN_THRESH_GLOBAL) if ( segment - LEN_THRESH_GLOBAL ) > spec.min_lengths[idx] else spec.min_lengths[idx] upper_limit = (segment + LEN_THRESH_GLOBAL) if ( segment + LEN_THRESH_GLOBAL ) < spec.max_lengths[idx] else spec.max_lengths[idx] # lower_limit = spec.min_lengths[idx] # upper_limit = spec.max_lengths[idx] newSeg = random.uniform(lower_limit, upper_limit) newLen.append(newSeg) newConf = robot_config.make_robot_config_from_ee1( ee[0], ee[1], newAngles, newLen, True, False) man_dist_to_grapple = inter_config_distance_heuristic( newConf, start_conf) #goal_points = goal_config.get_ee2() # man_dist_to_grapple = distance_to_grapple(newConf, goal_coord) # print(man_dist_to_grapple) # print(len(rand_configs)) # print(total_bots) if ((config_validity(newConf, spec) and \ (man_dist_to_grapple<0.01))): #and \inter_config_validity(config, newConf, spec)): valid_bridges.append(newConf) return valid_bridges
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 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 state_graph(n, k, r, init, goal, spec): """ Construct a graph by performing Probabilistic Roadmap. :param n: number of nodes to put in the roadmap :param k: number of closest neighbors to examine for each configuration :param r: the maximum distance between each configurations (in radians) :param init: the initial configuration :param spec: the specification of the problem :return: (list, list) vertices and edges that represents the graph. """ vertices = [] edges = [] current = init.config while len(vertices) < n: if current.ee1_grappled: collision = True while collision: angles = [] lengths = [] for i in current.ee1_angles: angles.append(Angle(degrees=random.randint(-165, 165))) if spec.min_lengths[0] != spec.max_lengths[0]: for i in range(len(spec.min_lengths)): lengths.append(random.uniform(spec.min_lengths[i], spec.max_lengths[i])) else: lengths = current.lengths next_node = make_robot_config_from_ee1( current.points[0][0], current.points[0][1], angles, lengths, ee1_grappled=True, ) if test_obstacle_collision(next_node, spec, spec.obstacles): vertices.append(GraphNode(spec, next_node)) collision = False vertices.append(init) vertices.append(goal) for q in vertices: closest = find_closest(q, k, vertices) for target in closest: if (q, target) not in edges: iteration_local_planner(q, target, spec) ### # edges.append((target, q)) return vertices, edges
def create_mid(q, target, spec): mid_angles = [] mid_lengths = [] for i, j in zip(q.config.ee1_angles, target.config.ee1_angles): k = (i.in_degrees() + j.in_degrees()) / 2 mid_angles.append(Angle(degrees=k)) for i, j in zip(q.config.lengths, target.config.lengths): k = (i + j) / 2 mid_lengths.append(k) mid_conf = make_robot_config_from_ee1( q.config.points[0][0], q.config.points[0][1], mid_angles, mid_lengths, ee1_grappled=True, ) return GraphNode(spec, mid_conf)
def pointsToConfig(pointsConfig): angles = [] length = [] for i in range(0, len(pointsConfig) - 1): angles.append( getAngleM((pointsConfig[i][0], pointsConfig[i][1]), (pointsConfig[i + 1][0], pointsConfig[i + 1][1]))) length.append( distancePoints((pointsConfig[i][0], pointsConfig[i][1]), (pointsConfig[i + 1][0], pointsConfig[i + 1][1]))) for i in range(1, len(angles)): angles[i] -= sum(angles[0:i]) anglesAngles = [] for i in range(0, len(angles)): anglesAngles.append(Angle(degrees=angles[i])) # Making it from ee1 config = make_robot_config_from_ee1(pointsConfig[0][0], pointsConfig[0][1], anglesAngles, length, ee1_grappled=True) return config
target_grapple = grapple final_path = get_solution_to_target(current_init_robot, target_grapple) output_min_solution_to_file(final_path) # last_bot = final_path[len(final_path)-1] # bridge_path = get_bridge_to_target(last_bot, target_grapple) # output_min_solution_to_file(bridge_path) #switch ees bridge_bot: robot_config.RobotConfig = final_path[len(final_path) - 1] ee = bridge_bot.get_ee2() angles = bridge_bot.ee2_angles # x, y = bridge_bot.points[0] # angles = bridge_bot.ee1_angles[:] lengths = bridge_bot.lengths lengths.reverse() proto_next_bot = robot_config.make_robot_config_from_ee1( ee[0], ee[1], angles, lengths, True, False) next_bot = proto_next_bot while (not config_validity(next_bot, problem)): print("invalid :(") exit() current_init_robot = next_bot final_path_final = get_solution_to_target(current_init_robot, goal_config.get_ee2()) output_min_solution_to_file(final_path_final) last_bot_final = final_path_final[len(final_path_final) - 1] merge_to_final_bot(last_bot_final, goal_config)
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 primitive_step(configs): result = [] result.append(configs[0]) for x in range(len(configs) - 1): current_angles = configs[x].ee1_angles next_angles = configs[x+1].ee1_angles current_lengths = configs[x].lengths next_lengths = configs[x+1].lengths abs_difference_angles = [] abs_difference_lengths = [] difference_angles = [] difference_lengths = [] # length of lengths and angles are the same for y in range(len(current_angles)): diff_angle = next_angles[y].in_radians() - current_angles[y].in_radians() abs_diff_angle = abs(diff_angle) abs_difference_angles.append(abs_diff_angle) difference_angles.append(diff_angle) diff_length = next_lengths[y] - current_lengths[y] abs_diff_length = abs(diff_length) abs_difference_lengths.append(abs_diff_length) difference_lengths.append(diff_length) # print("length : " + str(abs_difference_lengths)) # print("angle : " + str(abs_difference_angles)) segments = max(max(abs_difference_angles), max(abs_difference_lengths)) # print("segments : " + str(segments)) # print() configs_needed = int(math.ceil(segments/0.001)) direction_vector_angles = [] direction_vector_lengths = [] for dv in range(len(abs_difference_angles)): direction_vector_angles.append(difference_angles[dv] / configs_needed) direction_vector_lengths.append(difference_lengths[dv] / configs_needed) # print("dvdirection_vector_angles) for step in range(configs_needed): angles = [] lengths = [] for i in range(len(direction_vector_angles)): angle = Angle(radians=current_angles[i].in_radians() + (step * direction_vector_angles[i])) angles.append(angle) length = current_lengths[i] + (step * direction_vector_lengths[i]) lengths.append(length) new_config = make_robot_config_from_ee1(configs[0].get_ee1()[0], configs[0].get_ee1()[1], angles, lengths, configs[0].ee1_grappled, ee2_grappled=False) result.append(new_config) result.append(configs[x+1]) return result
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