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
