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