def write_robot_config_list_to_file(filename, robot_config_list, num_links):
"""
Write an output file for the given list of RobotConfigs. We recommend using this method to generate your output
file.
:param filename: name of output file (e.g. 2nd argument given to program)
:param robot_config_list: list of RobotConfig objects forming a path
:param num_links: The number of links for the robot arm
"""
f = open(filename, 'w')
j = 0
for rc in range(len(robot_config_list)):
lengths = []
angles = []
# Create robot config to get EE1 positions
for i in range(num_links):
lengths.append(robot_config_list[rc][-i])
angles.append(robot_config_list[rc][i + 2])
config = robot_config.make_robot_config_from_ee2(
lengths=lengths,
angles=angles,
x=robot_config_list[rc][0],
y=robot_config_list[rc][0])
ee1 = list(config.get_ee1())
f.write("{0}; {1}; {2}\n".format(
str(ee1)[1:-1],
str(angles)[1:-1],
str(lengths)[1:-1]))
f.close()
def generate_sample(spec, config, index):
angles = []
lengths = []
for i in range(spec.num_segments):
angles.append(Angle(random.uniform(-165, 165)))
if spec.min_lengths != spec.max_lengths:
lengths.append(
random.uniform(spec.min_lengths[i], spec.max_lengths[i]))
else:
lengths = config.lengths
if index % 2 == 0:
next_config = make_robot_config_from_ee1(spec.grapple_points[index][0],
spec.grapple_points[index][1],
angles,
lengths,
ee1_grappled=True,
ee2_grappled=False)
else:
next_config = make_robot_config_from_ee2(spec.grapple_points[index][0],
spec.grapple_points[index][1],
angles,
lengths,
ee1_grappled=False,
ee2_grappled=True)
if detect_collision(spec, next_config):
node = GraphNode(spec, next_config)
return node
else:
return generate_sample(spec, config, index)
def sample_double_grappled(spec):
samples = []
for i in range(spec.num_grapple_points):
for j in range(i + 1, spec.num_grapple_points):
for k in range(10):
c1 = make_robot_config_from_ee1(spec.grapple_points[i][0],
spec.grapple_points[i][1], [],
[], True, False)
c2 = make_robot_config_from_ee2(spec.grapple_points[j][0],
spec.grapple_points[j][1], [],
[], False, True)
s = extend(spec, c1, c2, [0], 0)
if s is not False:
samples.append(s)
return samples
def sample(spec):
samples = []
samples += sample_double_grappled(spec)
samples += uniform_sample(spec)
double_samples = []
for s in samples:
angles = s.ee1_angles.copy()
lengths = s.lengths.copy()
lengths.reverse()
double_samples.append(
make_robot_config_from_ee2(s.points[0][0],
s.points[0][1],
angles,
lengths,
ee1_grappled=s.ee2_grappled,
ee2_grappled=s.ee1_grappled))
samples += double_samples
Visualiser(spec, samples)
return samples
def interpolation_path(c1, c2, spec):
max_delta = dist(c1, c2, spec)
n_steps = math.ceil(max_delta / 0.001)
path = [c1]
for i in range(1, n_steps):
angles = []
lengths = []
if c1.ee1_grappled and c2.ee1_grappled:
for j in range(spec.num_segments):
angles.append((c2.ee1_angles[j] -
c1.ee1_angles[j]).in_radians() * i / n_steps +
c1.ee1_angles[j])
lengths.append((c2.lengths[j] - c1.lengths[j]) * i / n_steps +
c1.lengths[j])
next_config = make_robot_config_from_ee1(c1.get_ee1()[0],
c1.get_ee1()[1],
angles,
lengths,
ee1_grappled=True,
ee2_grappled=False)
else:
for j in range(spec.num_segments):
angles.append((c2.ee2_angles[j] -
c1.ee2_angles[j]).in_radians() * i / n_steps +
c1.ee2_angles[j])
lengths.append((c2.lengths[j] - c1.lengths[j]) * i / n_steps +
c1.lengths[j])
next_config = make_robot_config_from_ee2(c1.get_ee2()[0],
c1.get_ee2()[1],
angles,
lengths,
ee1_grappled=False,
ee2_grappled=True)
if i % PATH_CHECK_STEP == 0:
if detect_collision(spec, next_config):
path.append(next_config)
else:
return False
else:
path.append(next_config)
path.append(c2)
return path
def generate_sample(spec, grapple_point_index):
"""
Generates a sample robot config taking a problem spec as input.
"""
new_angles = generate_angles(
spec.num_segments) # num_segments == num_angles
new_lengths = generate_lengths(spec.min_lengths, spec.max_lengths)
# for now only implementing grapple points == 1
if spec.num_grapple_points == 1:
is_ee1_grappled = spec.initial.ee1_grappled
robot_config = make_robot_config_from_ee1(
spec.grapple_points[0][0],
spec.grapple_points[0][1],
new_angles,
new_lengths,
ee1_grappled=is_ee1_grappled,
ee2_grappled=not is_ee1_grappled)
else:
is_ee1_grappled = True if grapple_point_index == 0 else False #This is essentially hardcoding, fix it if you have time
new_x = spec.grapple_points[grapple_point_index][0]
new_y = spec.grapple_points[grapple_point_index][1]
if is_ee1_grappled:
robot_config = make_robot_config_from_ee1(
new_x,
new_y,
new_angles,
new_lengths,
ee1_grappled=is_ee1_grappled,
ee2_grappled=not is_ee1_grappled)
else:
robot_config = make_robot_config_from_ee2(
new_x,
new_y,
new_angles,
new_lengths,
ee1_grappled=is_ee1_grappled,
ee2_grappled=not is_ee1_grappled)
return robot_config
def uniformly_sampling(spec, obstacles, phase):
"""
Sample a configuration q by choosing a random arm posture. Check whether the random arm position is within the
minimum and maximum conditions, whether it is within the obstacle, whether it collides with the obstacle or itself
(if q -> F, then add to G), repeat this operation until N samples are created .
Each configuration becomes a node to be added to the state diagram.
"""
while True:
sampling_angles = []
sampling_lengths = []
for i in range(spec.num_segments):
# The angle between adjacent arm segments cannot be tighter than 15 degrees
# (i.e. angles 2, 3, 4... must be between -165 and +165).
sample_angle = random.uniform(-165, 165)
sampling_angles.append(Angle(degrees=float(sample_angle)))
# The segment lengths must be within the bounds specified in the input file
# (i.e. within min and max lengths)
sample_length = random.uniform(spec.min_lengths[i],
spec.max_lengths[i])
sampling_lengths.append(sample_length)
if phase % 2 == 0: # if grapple points is 1 or odd
new_config = make_robot_config_from_ee1(
spec.grapple_points[phase][0],
spec.grapple_points[phase][1],
sampling_angles,
sampling_lengths,
ee1_grappled=True)
else:
new_config = make_robot_config_from_ee2(
spec.grapple_points[phase][0],
spec.grapple_points[phase][1],
sampling_angles,
sampling_lengths,
ee2_grappled=True)
# Verification inspection
if collison_checking(new_config, spec, obstacles):
return new_config
def interpolate_path_segment(spec, start, end, grapple_point_index):
primitive_step = spec.PRIMITIVE_STEP
interpolated_path = [start]
is_in_tolerance = test_config_distance(start, end, spec)
is_ee1_grappled = start.ee1_grappled
while not is_in_tolerance:
new_lengths = generate_new_vals(interpolated_path[-1].lengths,
end.lengths, primitive_step)
new_x = spec.grapple_points[grapple_point_index][0]
new_y = spec.grapple_points[grapple_point_index][1]
if is_ee1_grappled:
new_angles = generate_new_vals(interpolated_path[-1].ee1_angles,
end.ee1_angles, primitive_step)
new_config = make_robot_config_from_ee1(
new_x,
new_y,
new_angles,
new_lengths,
ee1_grappled=is_ee1_grappled,
ee2_grappled=not is_ee1_grappled)
else:
new_angles = generate_new_vals(interpolated_path[-1].ee2_angles,
end.ee2_angles, primitive_step)
new_config = make_robot_config_from_ee2(
new_x,
new_y,
new_angles,
new_lengths,
ee1_grappled=is_ee1_grappled,
ee2_grappled=not is_ee1_grappled)
if is_step_collision(spec, new_config):
return []
interpolated_path.append(new_config)
is_in_tolerance = test_config_distance(new_config, end, spec)
return interpolated_path
def bridge_config(spec, config, index):
angles = []
lengths = []
for i in range(spec.num_segments - 1):
angles.append(Angle(degrees=random.uniform(-165, 165)))
if spec.min_lengths != spec.max_lengths:
lengths.append(
random.uniform(spec.min_lengths[i], spec.max_lengths[i]))
else:
lengths = config.lengths[:-1]
if index % 2 == 0:
next_config = make_robot_config_from_ee1(spec.grapple_points[index][0],
spec.grapple_points[index][1],
angles,
lengths,
ee1_grappled=True,
ee2_grappled=False)
last_point = next_config.points[-1]
else:
next_config = make_robot_config_from_ee2(spec.grapple_points[index][0],
spec.grapple_points[index][1],
angles,
lengths,
ee1_grappled=False,
ee2_grappled=True)
last_point = next_config.points[0]
delta_x = spec.grapple_points[index + 1][0] - last_point[0]
delta_y = spec.grapple_points[index + 1][1] - last_point[1]
last_length = (delta_x**2 + delta_y**2)**0.5
if delta_x == 0:
if delta_y > 0:
last_angle = math.pi / 2
else:
last_angle = -math.pi / 2
elif delta_x > 0:
last_angle = math.atan(delta_y / delta_x)
else:
last_angle = math.atan(delta_y / delta_x) + math.pi
last_angle = Angle(last_angle)
for angle in angles:
last_angle -= angle
angles.append(last_angle)
if index % 2 == 0:
lengths.append(last_length)
else:
lengths.insert(0, last_length)
if index % 2 == 0:
final_config = make_robot_config_from_ee1(
spec.grapple_points[index][0],
spec.grapple_points[index][1],
angles,
lengths,
ee1_grappled=True,
ee2_grappled=True)
else:
final_config = make_robot_config_from_ee2(
spec.grapple_points[index][0],
spec.grapple_points[index][1],
angles,
lengths,
ee1_grappled=True,
ee2_grappled=True)
if detect_collision(spec, final_config):
return GraphNode(spec, final_config)
else:
return bridge_config(spec, config, index)
def __init__(self, input_file):
# parse input file
f = open(input_file, 'r')
# parse arm constraints
try:
self.num_segments = int(next_valid_line(f))
except Exception:
print("Invalid value for number of segments")
sys.exit(1)
self.min_lengths = [float(i) for i in next_valid_line(f).split(' ')]
assert len(self.min_lengths) == self.num_segments, \
"Number of minimum lengths does not match number of segments"
self.max_lengths = [float(i) for i in next_valid_line(f).split(' ')]
assert len(self.max_lengths) == self.num_segments, \
"Number of maximum lengths does not match number of segments"
# parse initial configuration
initial_grappled = int(next_valid_line(f))
assert initial_grappled == 1 or initial_grappled == 2, "Initial end effector number is not 1 or 2"
try:
initial_eex, initial_eey = [
float(i) for i in next_valid_line(f).split(' ')
]
except Exception:
print("Invalid value(s) for initial end effector position")
sys.exit(1)
initial_angles = [
Angle(degrees=float(i)) for i in next_valid_line(f).split(' ')
]
assert len(initial_angles) == self.num_segments, \
"Number of initial angles does not match number of segments"
initial_lengths = [float(i) for i in next_valid_line(f).split(' ')]
assert len(initial_lengths) == self.num_segments, \
"Number of initial lengths does not match number of segments"
if initial_grappled == 1:
self.initial = make_robot_config_from_ee1(initial_eex,
initial_eey,
initial_angles,
initial_lengths,
ee1_grappled=True)
else:
self.initial = make_robot_config_from_ee2(initial_eex,
initial_eey,
initial_angles,
initial_lengths,
ee2_grappled=True)
# parse goal configuration
goal_grappled = int(next_valid_line(f))
assert goal_grappled == 1 or goal_grappled == 2, "Goal end effector number is not 1 or 2"
try:
goal_eex, goal_eey = [
float(i) for i in next_valid_line(f).split(' ')
]
except Exception:
print("Invalid value(s) for goal end effector 1 position")
sys.exit(1)
goal_angles = [
Angle(degrees=float(i)) for i in next_valid_line(f).split(' ')
]
assert len(goal_angles) == self.num_segments, \
"Number of goal ee1 angles does not match number of segments"
goal_lengths = [float(i) for i in next_valid_line(f).split(' ')]
assert len(goal_lengths) == self.num_segments, \
"Number of goal lengths does not match number of segments"
if goal_grappled == 1:
self.goal = make_robot_config_from_ee1(goal_eex,
goal_eey,
goal_angles,
goal_lengths,
ee1_grappled=True)
else:
self.goal = make_robot_config_from_ee2(goal_eex,
goal_eey,
goal_angles,
goal_lengths,
ee2_grappled=True)
# parse grapple points
try:
self.num_grapple_points = int(next_valid_line(f))
except Exception:
print("Invalid value for number of grapple points")
sys.exit(1)
grapple_points = []
for i in range(self.num_grapple_points):
try:
grapple_points.append(
tuple([float(i) for i in next_valid_line(f).split(' ')]))
except Exception:
print("Invalid value(s) for grapple point " + str(i) +
" position")
sys.exit(1)
self.grapple_points = grapple_points
# parse obstacles
try:
self.num_obstacles = int(next_valid_line(f))
except Exception:
print("Invalid value for number of obstacles")
sys.exit(1)
obstacles = []
for i in range(self.num_obstacles):
try:
x1, y1, x2, y2 = [
float(i) for i in next_valid_line(f).split(' ')
]
obstacles.append(Obstacle(x1, y1, x2, y2))
except Exception:
print("Invalid value(s) for obstacle " + str(i))
sys.exit(1)
self.obstacles = obstacles
self.goal_x = goal_eex
self.goal_y = goal_eey
self.goal_angles = []
for i in range(len(goal_angles)):
self.goal_angles.append(in_degrees(goal_angles[i].radians))
self.initial_angles = []
for i in range(len(initial_angles)):
self.initial_angles.append(in_degrees(initial_angles[i].radians))
def interpolation_checking(spec, phase, config_first, config_sec, obstacles):
"""
test whether the path between two sample points is valid, the path is discretised into 20 segments,
and verification checks will be applied to ensure that there are no invalid configurations in the path.
:param config_first: sample points
:param config_sec: exiting configs
q <- config
list x <- distance between angles
list y <- distance between lengths of segment
for j := range[0, 20] do
for n := 0 to number of segments do
new angles[n] = q.angles[n] + x[n]*0.05*j
new lengths[n] = q.lengths[n] + y[n]*0.05*j
using new angles and lengths to construct new configuration c
if c is invalid -> return false
return true
note: 0.05 = 1/20
"""
angles_first = config_first.ee1_angles if phase % 2 == 0 else config_first.ee2_angles
angles_sec = config_sec.ee1_angles if phase % 2 == 0 else config_sec.ee2_angles
between_angle = []
lengths_first = config_first.lengths
lengths_sec = config_sec.lengths
between_lengths = []
# calculate the lists of length and angle differences
for i in range(len(angles_first)):
between_angle.append(angles_sec[i].__sub__(angles_first[i]))
between_lengths.append(lengths_sec[i] - lengths_first[i])
# discretize the each list into 20 parts
for j in range(21):
valid_angle_list = []
valid_length_list = []
for n in range(len(angles_first)):
if phase % 2 == 0:
valid_angle_list.append(config_first.ee1_angles[n].__add__(
between_angle[n].__mul__(0.05 * j)))
else:
valid_angle_list.append(config_first.ee2_angles[n].__add__(
between_angle[n].__mul__(0.05 * j)))
valid_length_list.append(config_first.lengths[n] +
between_lengths[n] * 0.05 * j)
if phase % 2 == 0:
x, y = config_first.points[0]
new_config = make_robot_config_from_ee1(x,
y,
valid_angle_list,
valid_length_list,
ee1_grappled=True)
else:
x, y = config_first.points[-1]
new_config = make_robot_config_from_ee2(x,
y,
valid_angle_list,
valid_length_list,
ee2_grappled=True)
if not (collison_checking(new_config, spec, obstacles)):
return False
return True
def generate_bridge_configs(spec, obstacles, phase):
"""
start point (x1, y1) is the current grapple point
goal point (x2, y2) is the next grapple point
create an empty sub goal list to keep the goal we find
for i := range[1, 10] do
for j := range[1, num of segments - 1] do
sample some lengths and angles
if grapple point is even order do
partial config = generate configuration (start point, length, and angle)
if the distance between ee2 and goal point is within the length limit do
calculate the angle and length of the last segment
use the parameters to generate a completed configuration
if the configuration is collision free do
add the configuration into bridge_configs list
else do the former steps but with ee2 grappled at the grapple point
"""
start_point = spec.grapple_points[phase]
end_point = spec.grapple_points[phase + 1]
bridge_configs = []
for i in range(10):
complete_find = False
# repeat until 10 bridges generated
while complete_find is False:
sampling_angles = []
sampling_lengths = []
# finding a set of angles and lengths which form a bridge is to generate the first n-1 links
for j in range(spec.num_segments - 1):
sample_angle = random.uniform(-165, 165)
sampling_angles.append(Angle(degrees=float(sample_angle)))
sample_length = random.uniform(spec.min_lengths[j],
spec.max_lengths[j])
sampling_lengths.append(sample_length)
if phase % 2 == 0:
x1, y1 = start_point
x2, y2 = end_point
partial_bridge = make_robot_config_from_ee1(x1,
y1,
sampling_angles,
sampling_lengths,
ee1_grappled=True)
if (partial_bridge.points[-1][0] - x2) ** 2 + (partial_bridge.points[-1][1] - y2) ** 2 < \
spec.max_lengths[spec.num_segments - 1] ** 2:
# using tan(delta_y / delta_x)
calculated_net_angle = math.atan(
(partial_bridge.points[-1][1] - y2) /
(partial_bridge.points[-1][0] - x2))
# length sqrt(delta_x^2 + delta_y^2)
sampling_lengths.append(
math.sqrt((partial_bridge.points[-1][0] - x2)**2 +
(partial_bridge.points[-1][1] - y2)**2))
if y2 > partial_bridge.points[-1][
1] and x2 < partial_bridge.points[-1][0]:
calculated_net_angle += math.pi
elif y2 < partial_bridge.points[-1][
1] and x2 < partial_bridge.points[-1][0]:
calculated_net_angle -= math.pi
partial_net_angle = Angle(radians=0)
for n in range(len(sampling_angles)):
partial_net_angle += sampling_angles[n]
sampling_angles.append(
Angle(radians=float(calculated_net_angle -
partial_net_angle.in_radians())))
bridge_config = make_robot_config_from_ee1(
x1,
y1,
sampling_angles,
sampling_lengths,
ee1_grappled=True,
ee2_grappled=True)
if collison_checking(bridge_config, spec, obstacles):
bridge_configs.append(bridge_config)
complete_find = True
else:
x1, y1 = start_point
x2, y2 = end_point
partial_bridge = make_robot_config_from_ee2(x1,
y1,
sampling_angles,
sampling_lengths,
ee2_grappled=True)
if (partial_bridge.points[0][0] - x2) ** 2 + (partial_bridge.points[0][1] - y2) ** 2 < \
spec.max_lengths[0] ** 2:
calculated_net_angle = math.atan(
(partial_bridge.points[0][1] - y2) /
(partial_bridge.points[0][0] - x2))
sampling_lengths.insert(
0,
math.sqrt((partial_bridge.points[0][0] - x2)**2 +
(partial_bridge.points[0][1] - y2)**2))
if y2 > partial_bridge.points[0][
1] and x2 < partial_bridge.points[0][0]:
calculated_net_angle += math.pi
elif y2 < partial_bridge.points[0][
1] and x2 < partial_bridge.points[0][0]:
calculated_net_angle -= math.pi
partial_net_angle = Angle(radians=0)
for n in range(len(sampling_angles)):
partial_net_angle += sampling_angles[n]
sampling_angles.append(
Angle(radians=float(calculated_net_angle -
partial_net_angle.in_radians())))
bridge_config = make_robot_config_from_ee2(
x1,
y1,
sampling_angles,
sampling_lengths,
ee1_grappled=True,
ee2_grappled=True)
if collison_checking(bridge_config, spec, obstacles):
bridge_configs.append(bridge_config)
complete_find = True
return bridge_configs
def find_graph_path(spec,
obstacles,
phase,
partial_init=None,
bridge_configs=None):
"""
This method performs a breadth first search of the state graph and return a list of configs which form a path
through the state graph between the initial and the goal. Note that this path will not satisfy the primitive step
requirement - you will need to interpolate between the configs in the returned list.
:param spec: ProblemSpec object
:param obstacles: Obstacles object
:return: List of configs forming a path through the graph from initial to goal
for i := range[1, N] do
configs = uniformly sampling();
for config in all configs do
if distance between c1 and c2 is below certain amount and path is valid do
add a neighbor relationship between nodes;
search graph
if goal reached:
break
"""
# set init and goal nodes, put into nodes list
init_node = GraphNode(spec,
spec.initial) if partial_init is None else GraphNode(
spec, partial_init)
nodes = [init_node]
if bridge_configs is None:
goal_node = GraphNode(spec, spec.goal)
nodes.append(goal_node)
else:
for partial_goal in bridge_configs:
nodes.append(GraphNode(spec, partial_goal))
# sample configs
sample_number = 1000 if spec.num_segments <= 3 else 2000
for i in range(sample_number):
sample_point = uniformly_sampling(spec, obstacles, phase)
new_node = GraphNode(spec, sample_point)
nodes.append(new_node)
for node in nodes:
if distance_checking(spec, phase, sample_point, node.config) \
and interpolation_checking(spec, phase, sample_point, node.config, obstacles):
GraphNode.add_connection(node, new_node)
# search the graph
init_container = [init_node]
# here, each key is a graph node, each value is the list of configs visited on the path to the graph node
init_visited = {init_node: [init_node.config]}
while len(init_container) > 0:
current = init_container.pop(0)
complete_find = False
if bridge_configs is None:
if test_config_equality(current.config, spec.goal, spec):
complete_find = True
else:
for partial_goal in bridge_configs:
if test_config_equality(current.config, partial_goal, spec):
complete_find = True
if complete_find is True:
# found path to goal
config_list = init_visited[current]
path_list = []
for n in range(len(config_list) - 1):
angles_first = config_list[
n].ee1_angles if phase % 2 == 0 else config_list[
n].ee2_angles
angles_sec = config_list[
n + 1].ee1_angles if phase % 2 == 0 else config_list[
n + 1].ee2_angles
between_angle = []
between_lengths = []
lengths_first = config_list[n].lengths
lengths_sec = config_list[n + 1].lengths
for i in range(len(angles_first)):
between_angle.append(angles_sec[i].__sub__(
angles_first[i]))
between_lengths.append(lengths_sec[i] - lengths_first[i])
# interpolated the path into steps less than 0,001
for j in range(851):
valid_angle_list = []
valid_length_list = []
for k in range(len(angles_first)):
if phase % 2 == 0:
valid_angle_list.append(
config_list[n].ee1_angles[k].__add__(
between_angle[k].__mul__(1 / 850 * j)))
else:
valid_angle_list.append(
config_list[n].ee2_angles[k].__add__(
between_angle[k].__mul__(1 / 850 * j)))
valid_length_list.append(config_list[n].lengths[k] +
between_lengths[k] * 1 / 850 *
j)
if phase % 2 == 0:
x, y = config_list[n].points[0]
new_config = make_robot_config_from_ee1(
x,
y,
valid_angle_list,
valid_length_list,
ee1_grappled=True)
path_list.append(new_config)
else:
x, y = config_list[n].points[-1]
new_config = make_robot_config_from_ee2(
x,
y,
valid_angle_list,
valid_length_list,
ee2_grappled=True)
path_list.append(new_config)
return path_list
successors = current.get_successors()
for suc in successors:
if suc not in init_visited:
init_container.append(suc)
init_visited[suc] = init_visited[current] + [suc.config]
def extend(spec, config1, config2, total_trials, extended):
while (total_trials[0] < 1000):
remained_length = (spec.num_segments - extended) * spec.max_lengths[0]
trials = 0
if extended < spec.num_segments - 2:
# remained_length = (spec.num_segments - extended) * spec.max_lengths[0]
d = distance.euclidean(config1.points[-1], config2.points[0])
if d > remained_length:
return False
angle = angle_from_p1_to_p2(config1.points[-1], config2.points[0])
success = False
while trials < 100 and not success:
if remained_length > 2 * d:
seg_angle = Angle(radians=np.random.rand() * math.pi * 2)
seg_length = spec.min_lengths[0]
else:
seg_angle = Angle(
radians=np.random.normal(angle.in_radians(), math.pi /
6))
seg_length = np.random.uniform(spec.min_lengths[0],
spec.max_lengths[0])
success = True
new_angles = config1.ee1_angles + [
seg_angle - sum(config1.ee1_angles)
]
new_lengths = config1.lengths + [seg_length]
new_config1 = make_robot_config_from_ee1(config1.points[0][0],
config1.points[0][1],
new_angles,
new_lengths,
ee1_grappled=True)
if distance.euclidean(
new_config1.points[-1], config2.points[-1]
) > remained_length - spec.max_lengths[0]:
success = False
elif not check_valid(new_config1, spec):
success = False
trials += 1
total_trials[0] += 1
if not success:
return False
remained_length -= spec.max_lengths[0]
angle = angle_from_p1_to_p2(config2.points[0],
new_config1.points[-1])
success = False
while trials < 100 and not success:
if remained_length > 2 * d:
seg_angle = Angle(radians=np.random.rand() * math.pi * 2)
seg_length = spec.min_lengths[0]
else:
seg_angle = Angle(
radians=np.random.normal(angle.in_radians(), math.pi /
6))
seg_length = np.random.uniform(
(spec.min_lengths[0], spec.max_lengths[0]))
success = True
new_angles = config2.ee2_angles + [
seg_angle - sum(config2.ee2_angles)
]
new_lengths = config2.lengths + [seg_length]
new_config2 = make_robot_config_from_ee2(config2.points[-1][0],
config1.points[-1][1],
new_angles,
new_lengths,
ee2_grappled=True)
if not check_valid(new_config2, spec):
success = False
trials += 1
total_trials[0] += 1
if not success:
return False
result = extend(spec, new_config1, new_config2, total_trials,
extended + 2)
if result is not False:
return result
elif spec.num_segments - extended == 2:
p1 = config1.points[-1]
p2 = config2.points[0]
d = distance.euclidean(p1, p2)
if d > 2 * spec.max_lengths[0]:
return False
elif d > 2 * spec.min_lengths[0]:
length1 = d / 2
length2 = d / 2
angle1 = angle_from_p1_to_p2(p1, p2) - sum(config1.ee1_angles)
angle2 = Angle(radians=0)
angle3 = angle_from_p1_to_p2(
p2, config2.points[1]) - angle_from_p1_to_p2(p1, p2)
new_lengths = config1.lengths + [length1, length2
] + config2.lengths
new_angles = config1.ee1_angles + [angle1, angle2, angle3] + [
angle for angle in config2.ee1_angles[1:]
]
combined_config = make_robot_config_from_ee1(
config1.points[0][0],
config1.points[0][1],
new_angles,
new_lengths,
ee1_grappled=True,
ee2_grappled=True)
if check_valid((combined_config, spec)):
return combined_config
else:
return False
else:
return False
elif spec.num_segments - extended == 1:
p1 = config1.points[-1]
p2 = config2.points[0]
d = distance.euclidean(p1, p2)
if d > spec.max_lengths[0]:
return False
elif d > spec.min_lengths[0]:
angle1 = angle_from_p1_to_p2(p1, p2) - sum(config1.ee1_angles)
angle2 = angle_from_p1_to_p2(
p2, config2.points[1]) - angle_from_p1_to_p2(p1, p2)
new_lengths = config1.lengths + [d] + config2.lengths
new_angles = config1.ee1_angles + [angle1, angle2] + [
angle for angle in config2.ee2_angles[1:]
]
combined_config = make_robot_config_from_ee1(
config1.points[0][0],
config1.points[0][1],
new_angles,
new_lengths,
ee1_grappled=True,
ee2_grappled=True)
if check_valid((combined_config, spec)):
return combined_config
else:
return False
else:
return False
return False
def generate_random_points(self):
""" Generate N random sample points"""
print('grapple', self.grapple)
for i in range(self.num_grapples):
total = 0
grapple = list(self.grapple[i])
print('n_gple', grapple)
while total < round(self.num_nodes / self.num_grapples):
angles = [random.randrange(180)]
# print('annnngles', angles, type(angles))
angles.extend(
list(np.random.uniform(-165, 165, self.num_links - 1)))
radian_angles = deepcopy(angles)
for i in range(len(angles)):
radian_angles[i] = in_radians(radian_angles[i])
# print(angles)
lengths = np.random.uniform(self.min_lengths[0],
self.max_lengths[0],
self.num_links)
# print('angles',angles)
# print(self.initial_x)
if i % 2 == 0:
config = robot_config.make_robot_config_from_ee1(
lengths=lengths,
angles=radian_angles,
x=grapple[0],
y=grapple[1],
ee1_grappled=True)
point = list(config.get_ee2())
p = Node(point[0], point[1], total + self.num_grapples + 3,
angles, self.current_grapple[0],
self.current_grapple[1], lengths)
else:
config = robot_config.make_robot_config_from_ee2(
lengths=lengths,
angles=radian_angles,
x=grapple[0],
y=grapple[1],
ee2_grappled=True)
point = list(config.get_ee1())
p = Node(point[0], point[1], total + self.num_grapples + 3,
angles, grapple[0], grapple[1], lengths)
# print(self.initial_ee1_x, self.initial_ee1_y)
# print(self.initial_ee1_x, self.initial_ee1_y)
# print(point)
# print(point)
'''
p = Node(np.random.uniform(self.x_min, self.x_max, 1)[0],
np.random.uniform(self.y_min, self.y_max, 1)[0],
total + self.num_grapples + 2, angles)
'''
# print(p.point)
if not in_obstacle(self.obstacle, p) and self.inside_world(
p) and test_obstacle_collision(
config, self.spec,
self.obstacle) and self.test_self_collision(
config, self.spec):
self.nodes.append(p)
# print(point)
# print(angles)
# print(p.configuration)
# Indent line below to ensure N points generated
total += 1
# print(total)
print('succesful no. nodes: ', len(self.nodes))
