def inter_config(c1: robot_config.RobotConfig, c2: robot_config.RobotConfig,
spec: problem_spec.ProblemSpec):
c1angles = c1.ee1_angles
c1lengths = c1.lengths
ee = c1.get_ee1()
#interpolate mid between confs
newAngles = []
for idx, angObj in enumerate(c1angles):
curr_rad = angObj.in_radians()
end_rad = c2.ee1_angles[idx].in_radians()
mid_rad = (curr_rad + end_rad) / 2
newAngle = angle.Angle(radians=mid_rad)
newAngles.append(newAngle)
newLen = []
for idx, segment in enumerate(c1lengths):
newSeg = (segment + c2.lengths[idx]) / 2
newLen.append(newSeg)
newConf = robot_config.make_robot_config_from_ee1(ee[0], ee[1], newAngles,
newLen, True, False)
return newConf
def uniformly_sampling(spec, obstacles, stage):
while True:
sampling_angle = []
sampling_length = []
for i in range(spec.num_segments):
angle = random.uniform(-165, 165)
sampling_angle.append(Angle(degrees=float(angle)))
length = random.uniform(spec.min_lengths[i], spec.max_lengths[i])
sampling_length.append(length)
if stage % 2 == 0:
new_config = make_robot_config_from_ee1(
spec.grapple_points[stage][0],
spec.grapple_points[stage][1],
sampling_angle,
sampling_length,
ee1_grappled=True)
else:
new_config = make_robot_config_from_ee2(
spec.grapple_points[stage][0],
spec.grapple_points[stage][1],
sampling_angle,
sampling_length,
ee2_grappled=True)
if test_obstacle_collision(new_config, spec, obstacles) and test_self_collision(new_config, spec) and \
test_environment_bounds(new_config):
return new_config
def removeUnwantedPoints(points, spec):
# Remove unwatned points
# length should not be greater than the sum of all lenght
# length should not be less than that of min config., that we got by 15 degrees angle
initial_point = spec.initial.points[0]
config = spec.initial
# list the unexpected angle
unacceptable_angles = [[15, 15, 15], [15, 15, -15], [15, -15, 15], [-15, 15, 15], [15, -15, -15], [-15, 15, -15],
[-15, 15, -15], [-15, -15, -15]]
# for these unexpected angles
d = [0] * len(unacceptable_angles)
for an in range(len(unacceptable_angles)):
config_plus_15 = make_robot_config_from_ee1(config.points[0][0], config.points[0][1], unacceptable_angles[an],
spec.max_lengths, ee1_grappled=True)
d[an] = distancePoints(initial_point, config_plus_15.points[-1])
for p in points:
# if the distance is greater then maz lenght
if ((distancePoints(p, initial_point) > sum(spec.max_lengths))
# or less than min distnace
or (distancePoints(p, initial_point) < min(d))):
points.remove(p)
return points
def mid_point(config_1, config_2):
"""Return the mid config for """
angles_new = []
length_new = []
if config_1.ee1_grappled == True:
angles_1 = copy.deepcopy(config_1.ee1_angles)
elif config_1.ee2_grappled == True:
angles_1 = copy.deepcopy(config_1.ee2_angles)
if config_2.ee1_grappled == True:
angles_2 = copy.deepcopy(config_2.ee1_angles)
elif config_2.ee2_grappled == True:
angles_2 = copy.deepcopy(config_2.ee2_angles)
for i in range(len(angles_1)):
an = (angles_1[i]) / 2 + (angles_2[i]) / 2
angles_new.append(an)
for i in range(len(config_1.lengths)):
length_new.append((config_1.lengths[i] + config_2.lengths[i]) / 2)
if config_1.ee1_grappled == True:
config = make_robot_config_from_ee1(config_1.points[0][0], config_1.points[0][1], angles_new, length_new,
ee1_grappled=True)
elif config_1.ee2_grappled == True:
config = make_robot_config_from_ee2(config_1.points[len(config_1.points) - 1][0], config_1.points[len(config_1.points) - 1][1], angles_new, length_new,
ee2_grappled=True)
return config
def random_steps(config: robot_config.RobotConfig,
spec: problem_spec.ProblemSpec, goal_coord: tuple,
dist_to_grap: float):
RAD_THRESH = RAD_THRESH_GLOBAL
LEN_THRESH = LEN_THRESH_GLOBAL
ee = config.get_ee1() if config.ee1_grappled else config.get_ee2()
lengths = config.lengths
angles = config.ee1_angles if config.ee1_grappled else config.ee2_angles
rand_configs = []
total_bots = 0
while (len(rand_configs) < N and total_bots < 100):
# print(len(rand_configs))
newAngles = []
for angObj in angles:
curr_rad = angObj.in_radians()
lower_limit = (curr_rad-math.pi*RAD_THRESH) if ((curr_rad-math.pi*RAD_THRESH)>=(-MAX_RADIANS) and\
(curr_rad-math.pi*RAD_THRESH)<=MAX_RADIANS) else -MAX_RADIANS
upper_limit = (curr_rad+math.pi*RAD_THRESH) if ((curr_rad+math.pi*RAD_THRESH)>=(-MAX_RADIANS) and\
(curr_rad+math.pi*RAD_THRESH)<=MAX_RADIANS) else MAX_RADIANS
# lower_limit = -math.pi
# upper_limit = math.pi
newAngle = angle.Angle(
radians=(random.uniform(lower_limit, upper_limit)))
newAngles.append(newAngle)
newLen = []
for idx, segment in enumerate(lengths):
lower_limit = (segment - LEN_THRESH_GLOBAL) if (
segment - LEN_THRESH_GLOBAL
) > spec.min_lengths[idx] else spec.min_lengths[idx]
upper_limit = (segment + LEN_THRESH_GLOBAL) if (
segment + LEN_THRESH_GLOBAL
) < spec.max_lengths[idx] else spec.max_lengths[idx]
# lower_limit = spec.min_lengths[idx]
# upper_limit = spec.max_lengths[idx]
newSeg = random.uniform(lower_limit, upper_limit)
newLen.append(newSeg)
newConf = robot_config.make_robot_config_from_ee1(
ee[0], ee[1], newAngles, newLen, True, False)
# man_dist_to_grapple = heuristic(newConf, goal_config)
#goal_points = goal_config.get_ee2()
man_dist_to_grapple = distance_to_grapple(newConf, goal_coord)
# print(man_dist_to_grapple)
# print(len(rand_configs))
total_bots += 1
# print(total_bots)
if ((config_validity(newConf, spec) and \
(man_dist_to_grapple<dist_to_grap))): #and \inter_config_validity(config, newConf, spec)):
rand_configs.append(newConf)
return rand_configs
def generate_bridge_bots(start_conf: robot_config.RobotConfig,
spec: problem_spec.ProblemSpec):
RAD_THRESH = RAD_THRESH_GLOBAL
LEN_THRESH = LEN_THRESH_GLOBAL
valid_bridges = []
ee = start_conf.get_ee1()
lengths = start_conf.lengths
angles = start_conf.ee1_angles
while (len(valid_bridges) < 1):
newAngles = []
for angObj in angles:
curr_rad = angObj.in_radians()
lower_limit = (curr_rad-math.pi*RAD_THRESH) if ((curr_rad-math.pi*RAD_THRESH)>=(-MAX_RADIANS) and\
(curr_rad-math.pi*RAD_THRESH)<=MAX_RADIANS) else -MAX_RADIANS
upper_limit = (curr_rad+math.pi*RAD_THRESH) if ((curr_rad+math.pi*RAD_THRESH)>=(-MAX_RADIANS) and\
(curr_rad+math.pi*RAD_THRESH)<=MAX_RADIANS) else MAX_RADIANS
newAngle = angle.Angle(
radians=(random.uniform(lower_limit, upper_limit)))
newAngles.append(newAngle)
newLen = []
for idx, segment in enumerate(lengths):
lower_limit = (segment - LEN_THRESH_GLOBAL) if (
segment - LEN_THRESH_GLOBAL
) > spec.min_lengths[idx] else spec.min_lengths[idx]
upper_limit = (segment + LEN_THRESH_GLOBAL) if (
segment + LEN_THRESH_GLOBAL
) < spec.max_lengths[idx] else spec.max_lengths[idx]
# lower_limit = spec.min_lengths[idx]
# upper_limit = spec.max_lengths[idx]
newSeg = random.uniform(lower_limit, upper_limit)
newLen.append(newSeg)
newConf = robot_config.make_robot_config_from_ee1(
ee[0], ee[1], newAngles, newLen, True, False)
man_dist_to_grapple = inter_config_distance_heuristic(
newConf, start_conf)
#goal_points = goal_config.get_ee2()
# man_dist_to_grapple = distance_to_grapple(newConf, goal_coord)
# print(man_dist_to_grapple)
# print(len(rand_configs))
# print(total_bots)
if ((config_validity(newConf, spec) and \
(man_dist_to_grapple<0.01))): #and \inter_config_validity(config, newConf, spec)):
valid_bridges.append(newConf)
return valid_bridges
def load_output(filename):
# return a list of RobotConfig objects
robot_configs = []
f = open(filename, 'r')
for line in f:
ee1_xy_str, ee1_angles_str, lengths_str = line.strip().split(';')
ee1x, ee1y = tuple([float(i) for i in ee1_xy_str.split(' ')])
ee1_angles = [Angle(degrees=float(i)) for i in ee1_angles_str.strip().split(' ')]
lengths = [float(i) for i in lengths_str.strip().split(' ')]
robot_configs.append(make_robot_config_from_ee1(ee1x, ee1y, ee1_angles, lengths))
return robot_configs
def check_path(config1, config2, spec, obstacles):
angles1 = config1.ee1_angles
angles2 = config2.ee1_angles
angle_distance = []
if config1.ee2_grappled is True:
angles1 = config1.ee2_angles
angles2 = config2.ee2_angles
lengths1 = config1.lengths
lengths2 = config2.lengths
length_distance = []
for j in range(len(angles1)):
angle_distance.append(angles2[j].__sub__(angles1[j]))
length_distance.append(lengths2[j] - lengths1[j])
for j in range(20):
new_angle_list = []
new_length_list = []
for k in range(len(angles1)):
if config1.ee1_grappled is True:
new_angle_list.append(config1.ee1_angles[k].__add__(
angle_distance[k].__mul__(0.05 * j)))
elif config1.ee2_grappled is True:
new_angle_list.append(config1.ee2_angles[k].__add__(
angle_distance[k].__mul__(0.05 * j)))
new_length_list.append(config1.lengths[k] +
length_distance[k] * 0.05 * j)
x, y = config1.points[0]
new_config = make_robot_config_from_ee1(x,
y,
new_angle_list,
new_length_list,
ee1_grappled=True,
ee2_grappled=False)
if config1.ee2_grappled is True:
x, y = config1.points[-1]
new_config = make_robot_config_from_ee2(
x,
y,
new_angle_list,
list(reversed(new_length_list)),
ee1_grappled=False,
ee2_grappled=True)
if not (test_obstacle_collision(new_config, spec, obstacles)
and test_self_collision(new_config, spec)
and test_environment_bounds(new_config)):
return False
return True
def state_graph(n, k, r, init, goal, spec):
"""
Construct a graph by performing Probabilistic Roadmap.
:param n: number of nodes to put in the roadmap
:param k: number of closest neighbors to examine for each configuration
:param r: the maximum distance between each configurations (in radians)
:param init: the initial configuration
:param spec: the specification of the problem
:return: (list, list) vertices and edges that represents the graph.
"""
vertices = []
edges = []
current = init.config
while len(vertices) < n:
if current.ee1_grappled:
collision = True
while collision:
angles = []
lengths = []
for i in current.ee1_angles:
angles.append(Angle(degrees=random.randint(-165, 165)))
if spec.min_lengths[0] != spec.max_lengths[0]:
for i in range(len(spec.min_lengths)):
lengths.append(random.uniform(spec.min_lengths[i], spec.max_lengths[i]))
else:
lengths = current.lengths
next_node = make_robot_config_from_ee1(
current.points[0][0],
current.points[0][1],
angles,
lengths,
ee1_grappled=True,
)
if test_obstacle_collision(next_node, spec, spec.obstacles):
vertices.append(GraphNode(spec, next_node))
collision = False
vertices.append(init)
vertices.append(goal)
for q in vertices:
closest = find_closest(q, k, vertices)
for target in closest:
if (q, target) not in edges:
iteration_local_planner(q, target, spec)
###
# edges.append((target, q))
return vertices, edges
def create_mid(q, target, spec):
mid_angles = []
mid_lengths = []
for i, j in zip(q.config.ee1_angles, target.config.ee1_angles):
k = (i.in_degrees() + j.in_degrees()) / 2
mid_angles.append(Angle(degrees=k))
for i, j in zip(q.config.lengths, target.config.lengths):
k = (i + j) / 2
mid_lengths.append(k)
mid_conf = make_robot_config_from_ee1(
q.config.points[0][0],
q.config.points[0][1],
mid_angles,
mid_lengths,
ee1_grappled=True,
)
return GraphNode(spec, mid_conf)
def pointsToConfig(pointsConfig):
angles = []
length = []
for i in range(0, len(pointsConfig) - 1):
angles.append(
getAngleM((pointsConfig[i][0], pointsConfig[i][1]), (pointsConfig[i + 1][0], pointsConfig[i + 1][1])))
length.append(
distancePoints((pointsConfig[i][0], pointsConfig[i][1]), (pointsConfig[i + 1][0], pointsConfig[i + 1][1])))
for i in range(1, len(angles)):
angles[i] -= sum(angles[0:i])
anglesAngles = []
for i in range(0, len(angles)):
anglesAngles.append(Angle(degrees=angles[i]))
# Making it from ee1
config = make_robot_config_from_ee1(pointsConfig[0][0], pointsConfig[0][1], anglesAngles, length, ee1_grappled=True)
return config
target_grapple = grapple
final_path = get_solution_to_target(current_init_robot, target_grapple)
output_min_solution_to_file(final_path)
# last_bot = final_path[len(final_path)-1]
# bridge_path = get_bridge_to_target(last_bot, target_grapple)
# output_min_solution_to_file(bridge_path)
#switch ees
bridge_bot: robot_config.RobotConfig = final_path[len(final_path) - 1]
ee = bridge_bot.get_ee2()
angles = bridge_bot.ee2_angles
# x, y = bridge_bot.points[0]
# angles = bridge_bot.ee1_angles[:]
lengths = bridge_bot.lengths
lengths.reverse()
proto_next_bot = robot_config.make_robot_config_from_ee1(
ee[0], ee[1], angles, lengths, True, False)
next_bot = proto_next_bot
while (not config_validity(next_bot, problem)):
print("invalid :(")
exit()
current_init_robot = next_bot
final_path_final = get_solution_to_target(current_init_robot,
goal_config.get_ee2())
output_min_solution_to_file(final_path_final)
last_bot_final = final_path_final[len(final_path_final) - 1]
merge_to_final_bot(last_bot_final, goal_config)
def __init__(self, input_file):
# parse input file
f = open(input_file, 'r')
# parse arm constraints
try:
self.num_segments = int(next_valid_line(f))
except Exception:
print("Invalid value for number of segments")
sys.exit(1)
self.min_lengths = [float(i) for i in next_valid_line(f).split(' ')]
assert len(self.min_lengths) == self.num_segments, \
"Number of minimum lengths does not match number of segments"
self.max_lengths = [float(i) for i in next_valid_line(f).split(' ')]
assert len(self.max_lengths) == self.num_segments, \
"Number of maximum lengths does not match number of segments"
# parse initial configuration
initial_grappled = int(next_valid_line(f))
assert initial_grappled == 1 or initial_grappled == 2, "Initial end effector number is not 1 or 2"
try:
initial_eex, initial_eey = [
float(i) for i in next_valid_line(f).split(' ')
]
except Exception:
print("Invalid value(s) for initial end effector position")
sys.exit(1)
initial_angles = [
Angle(degrees=float(i)) for i in next_valid_line(f).split(' ')
]
assert len(initial_angles) == self.num_segments, \
"Number of initial angles does not match number of segments"
initial_lengths = [float(i) for i in next_valid_line(f).split(' ')]
assert len(initial_lengths) == self.num_segments, \
"Number of initial lengths does not match number of segments"
if initial_grappled == 1:
self.initial = make_robot_config_from_ee1(initial_eex,
initial_eey,
initial_angles,
initial_lengths,
ee1_grappled=True)
else:
self.initial = make_robot_config_from_ee2(initial_eex,
initial_eey,
initial_angles,
initial_lengths,
ee2_grappled=True)
# parse goal configuration
goal_grappled = int(next_valid_line(f))
assert goal_grappled == 1 or goal_grappled == 2, "Goal end effector number is not 1 or 2"
try:
goal_eex, goal_eey = [
float(i) for i in next_valid_line(f).split(' ')
]
except Exception:
print("Invalid value(s) for goal end effector 1 position")
sys.exit(1)
goal_angles = [
Angle(degrees=float(i)) for i in next_valid_line(f).split(' ')
]
assert len(goal_angles) == self.num_segments, \
"Number of goal ee1 angles does not match number of segments"
goal_lengths = [float(i) for i in next_valid_line(f).split(' ')]
assert len(goal_lengths) == self.num_segments, \
"Number of goal lengths does not match number of segments"
if goal_grappled == 1:
self.goal = make_robot_config_from_ee1(goal_eex,
goal_eey,
goal_angles,
goal_lengths,
ee1_grappled=True)
else:
self.goal = make_robot_config_from_ee2(goal_eex,
goal_eey,
goal_angles,
goal_lengths,
ee2_grappled=True)
# parse grapple points
try:
self.num_grapple_points = int(next_valid_line(f))
except Exception:
print("Invalid value for number of grapple points")
sys.exit(1)
grapple_points = []
for i in range(self.num_grapple_points):
try:
grapple_points.append(
tuple([float(i) for i in next_valid_line(f).split(' ')]))
except Exception:
print("Invalid value(s) for grapple point " + str(i) +
" position")
sys.exit(1)
self.grapple_points = grapple_points
# parse obstacles
try:
self.num_obstacles = int(next_valid_line(f))
except Exception:
print("Invalid value for number of obstacles")
sys.exit(1)
obstacles = []
for i in range(self.num_obstacles):
try:
x1, y1, x2, y2 = [
float(i) for i in next_valid_line(f).split(' ')
]
obstacles.append(Obstacle(x1, y1, x2, y2))
except Exception:
print("Invalid value(s) for obstacle " + str(i))
sys.exit(1)
self.obstacles = obstacles
def create_state_graph(spec, obstacles):
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
nodes = [init_node, goal_node]
for stage in range(spec.num_grapple_points):
for i in range(500):
sample_point = uniformly_sampling(spec, obstacles, stage)
new_node = GraphNode(spec, sample_point)
nodes.append(new_node)
for node in nodes:
if check_distance(
spec, sample_point, node.config) and check_path(
sample_point, node.config, spec, obstacles):
node.neighbors.append(new_node)
new_node.neighbors.append(node)
init_container = [init_node]
init_visited = {init_node: [init_node.config]}
while len(init_container) > 0:
current = init_container.pop(0)
if test_config_equality(current.config, spec.goal, spec):
result = init_visited[current]
# return result
new_list = []
for i in range(len(result) - 1):
angles1 = result[i].ee1_angles
angles2 = result[i + 1].ee1_angles
if result[i].ee2_grappled is True:
angles1 = result[i].ee2_angles
angles2 = result[i + 1].ee2_angles
angle_distance = []
lengths1 = result[i].lengths
lengths2 = result[i + 1].lengths
length_distance = []
for j in range(len(angles1)):
angle_distance.append(angles2[j].__sub__(angles1[j]))
length_distance.append(lengths2[j] - lengths1[j])
for j in range(501):
new_angle_list = []
new_length_list = []
for k in range(len(angles1)):
if result[i].ee1_grappled is True:
new_angle_list.append(
result[i].ee1_angles[k].__add__(
angle_distance[k].__mul__(0.002 * j)))
elif result[i].ee2_grappled is True:
new_angle_list.append(
result[i].ee2_angles[k].__add__(
angle_distance[k].__mul__(0.002 * j)))
new_length_list.append(result[i].lengths[k] +
length_distance[k] * 0.002 * j)
if result[i].ee1_grappled is True:
x, y = result[i].points[0]
new_config = make_robot_config_from_ee1(
x,
y,
new_angle_list,
new_length_list,
ee1_grappled=True,
ee2_grappled=False)
if test_obstacle_collision(
new_config, spec,
obstacles) and test_self_collision(
new_config, spec
) and test_environment_bounds(new_config):
new_list.append(new_config)
elif result[i].ee2_grappled is True:
x, y = result[i].points[-1]
new_config = make_robot_config_from_ee2(
x,
y,
new_angle_list,
list(reversed(new_length_list)),
ee1_grappled=False,
ee2_grappled=True)
if test_obstacle_collision(
new_config, spec,
obstacles) and test_self_collision(
new_config, spec
) and test_environment_bounds(new_config):
new_list.append(new_config)
return new_list
successors = current.get_successors()
for suc in successors:
if suc not in init_visited:
init_container.append(suc)
init_visited[suc] = init_visited[current] + [suc.config]
def generate_sub_goals(spec, stage, obstacles):
start_point = spec.grapple_points[stage]
goal_point = spec.grapple_points[stage+1]
sub_goals = []
for i in range(10):
find_goal = False
while find_goal is False:
sampling_angle = []
sampling_length = []
for j in range(spec.num_segments - 1):
angle = random.uniform(-165, 165)
sampling_angle.append(Angle(degrees=float(angle)))
length = random.uniform(spec.min_lengths[j], spec.max_lengths[j])
sampling_length.append(length)
if stage % 2 == 0:
x1, y1 = start_point
x2, y2 = goal_point
partial_config = make_robot_config_from_ee1(x1, y1, sampling_angle, sampling_length, ee1_grappled=True)
if (partial_config.points[-1][0] - x2) ** 2 + (partial_config.points[-1][1] - y2) ** 2 < spec.max_lengths[spec.num_segments-1] ** 2:
sampling_length.append(math.sqrt((partial_config.points[-1][0] - x2) ** 2 + (partial_config.points[-1][1] - y2) ** 2))
required_net_angle = math.atan((partial_config.points[-1][1] - y2) / (partial_config.points[-1][0] - x2))
if y2 > partial_config.points[-1][1] and x2 < partial_config.points[-1][0]:
required_net_angle += math.pi
elif y2 < partial_config.points[-1][1] and x2 < partial_config.points[-1][0]:
required_net_angle -= math.pi
current_net_angle = 0
for angle in sampling_angle:
current_net_angle += angle.in_radians()
sampling_angle.append(Angle(radians=float(required_net_angle-current_net_angle)))
sub_goal_config = make_robot_config_from_ee1(x1, y1, sampling_angle, sampling_length, ee1_grappled=True, ee2_grappled=True)
if test_obstacle_collision(sub_goal_config, spec, obstacles) and test_self_collision(sub_goal_config, spec)\
and test_environment_bounds(sub_goal_config):
sub_goals.append(sub_goal_config)
find_goal = True
else:
x1, y1 = start_point
x2, y2 = goal_point
partial_config = make_robot_config_from_ee2(x1, y1, sampling_angle, sampling_length, ee2_grappled=True)
if (partial_config.points[0][0] - x2) ** 2 + (partial_config.points[0][1] - y2) ** 2 < spec.max_lengths[0] ** 2:
sampling_length.insert(0, math.sqrt((partial_config.points[0][0] - x2) ** 2 + (partial_config.points[0][1] - y2) ** 2))
required_net_angle = math.atan((partial_config.points[0][1] - y2) / (partial_config.points[0][0] - x2))
if required_net_angle < 0:
required_net_angle += math.pi
current_net_angle = 0
for angle in sampling_angle:
current_net_angle += angle.in_radians()
sampling_angle.append(Angle(radians=float(required_net_angle-current_net_angle)))
sub_goal_config = make_robot_config_from_ee2(x1, y1, sampling_angle, sampling_length, ee1_grappled=True, ee2_grappled=True)
if test_obstacle_collision(sub_goal_config, spec, obstacles) and test_self_collision(sub_goal_config, spec)\
and test_environment_bounds(sub_goal_config):
sub_goals.append(sub_goal_config)
find_goal = True
return sub_goals
def create_state_graph(spec, obstacles, stage, sub_init=None, sub_goals=None):
if sub_init is None:
init_node = GraphNode(spec, spec.initial)
else:
init_node = GraphNode(spec, sub_init)
nodes = [init_node]
if sub_goals is None:
goal_node = GraphNode(spec, spec.goal)
nodes.append(goal_node)
else:
for sub_goal in sub_goals:
nodes.append(GraphNode(spec, sub_goal))
for i in range(2000):
sample_point = uniformly_sampling(spec, obstacles, stage)
new_node = GraphNode(spec, sample_point)
nodes.append(new_node)
for node in nodes:
if check_distance(spec, sample_point, node.config, stage):
if check_path(sample_point, node.config, spec, obstacles, stage):
node.neighbors.append(new_node)
new_node.neighbors.append(node)
init_container = [init_node]
init_visited = {init_node: [init_node.config]}
while len(init_container) > 0:
current = init_container.pop(0)
find_goal = False
if sub_goals is None:
if test_config_equality(current.config, spec.goal, spec):
find_goal = True
else:
for sub_goal in sub_goals:
if test_config_equality(current.config, sub_goal, spec):
find_goal = True
if find_goal is True:
result = init_visited[current]
# return result
new_list = []
for i in range(len(result) - 1):
angles1 = result[i].ee1_angles
angles2 = result[i + 1].ee1_angles
if stage % 2 == 1:
angles1 = result[i].ee2_angles
angles2 = result[i + 1].ee2_angles
angle_distance = []
lengths1 = result[i].lengths
lengths2 = result[i + 1].lengths
length_distance = []
for j in range(len(angles1)):
angle_distance.append(angles2[j].__sub__(angles1[j]))
length_distance.append(lengths2[j] - lengths1[j])
for j in range(501):
new_angle_list = []
new_length_list = []
for k in range(len(angles1)):
if stage % 2 == 0:
new_angle_list.append(result[i].ee1_angles[k].__add__(angle_distance[k].__mul__(0.002 * j)))
elif stage % 2 == 1:
new_angle_list.append(result[i].ee2_angles[k].__add__(angle_distance[k].__mul__(0.002 * j)))
new_length_list.append(result[i].lengths[k] + length_distance[k] * 0.002 * j)
if stage % 2 == 0:
x, y = result[i].points[0]
new_config = make_robot_config_from_ee1(x, y, new_angle_list, new_length_list,
ee1_grappled=True)
new_list.append(new_config)
elif stage % 2 == 1:
x, y = result[i].points[-1]
new_config = make_robot_config_from_ee2(x, y, new_angle_list, new_length_list,
ee2_grappled=True)
new_list.append(new_config)
return new_list
successors = current.get_successors()
for suc in successors:
if suc not in init_visited:
init_container.append(suc)
init_visited[suc] = init_visited[current] + [suc.config]
def primitive_step(configs):
result = []
result.append(configs[0])
for x in range(len(configs) - 1):
current_angles = configs[x].ee1_angles
next_angles = configs[x+1].ee1_angles
current_lengths = configs[x].lengths
next_lengths = configs[x+1].lengths
abs_difference_angles = []
abs_difference_lengths = []
difference_angles = []
difference_lengths = []
# length of lengths and angles are the same
for y in range(len(current_angles)):
diff_angle = next_angles[y].in_radians() - current_angles[y].in_radians()
abs_diff_angle = abs(diff_angle)
abs_difference_angles.append(abs_diff_angle)
difference_angles.append(diff_angle)
diff_length = next_lengths[y] - current_lengths[y]
abs_diff_length = abs(diff_length)
abs_difference_lengths.append(abs_diff_length)
difference_lengths.append(diff_length)
# print("length : " + str(abs_difference_lengths))
# print("angle : " + str(abs_difference_angles))
segments = max(max(abs_difference_angles), max(abs_difference_lengths))
# print("segments : " + str(segments))
# print()
configs_needed = int(math.ceil(segments/0.001))
direction_vector_angles = []
direction_vector_lengths = []
for dv in range(len(abs_difference_angles)):
direction_vector_angles.append(difference_angles[dv] / configs_needed)
direction_vector_lengths.append(difference_lengths[dv] / configs_needed)
# print("dvdirection_vector_angles)
for step in range(configs_needed):
angles = []
lengths = []
for i in range(len(direction_vector_angles)):
angle = Angle(radians=current_angles[i].in_radians() + (step * direction_vector_angles[i]))
angles.append(angle)
length = current_lengths[i] + (step * direction_vector_lengths[i])
lengths.append(length)
new_config = make_robot_config_from_ee1(configs[0].get_ee1()[0], configs[0].get_ee1()[1], angles,
lengths, configs[0].ee1_grappled, ee2_grappled=False)
result.append(new_config)
result.append(configs[x+1])
return result
def randomsample(config_initial, number_samples, spec, graphnodes,a_count):
"""Created sample robot configs and for valid configs creates nodes
New nodes are checked with existing nodes for neighbours and appended to the neighbours list"""
configs = []
# Check if graphnodes is empty(1st iteration) and append source configs to it
if len(graphnodes) != 0:
for config in config_initial:
graphnodes.append(config)
i = 0
while i < number_samples:
# choose a random config from base configs
random_number = random.randint(0, len(config_initial) - 1)
config = config_initial[random_number].config
# choose a random angle and add +value to it
if config.ee1_grappled == True:
angles = copy.deepcopy(config.ee1_angles)
elif config.ee2_grappled == True:
angles = copy.deepcopy(config.ee2_angles)
lengths = copy.deepcopy(config.lengths)
angle_add = random.randint(-50, 50)
random_number = random.randint(0, len(angles) - 1)
angles[random_number] = angles[random_number] + angle_add
random_number = random.randint(0, len(angles) - 1)
lengths[random_number] = random.uniform(spec.min_lengths[random_number], spec.max_lengths[random_number])
# Make new config with new angle and length
if config.ee1_grappled == True:
config_new = make_robot_config_from_ee1(config.points[0][0], config.points[0][1], angles, config.lengths,
ee1_grappled=True)
elif config.ee2_grappled == True:
config_new = make_robot_config_from_ee2(config.points[len(config.points) - 1][0],
config.points[len(config.points) - 1][1], angles, config.lengths,
ee2_grappled=True)
if (valid_config(config_new, spec) == False):
continue
i = i + 1
configs.append(config_new)
# Creating a new node with the config and updating other nodes if it is a neighbour
current_node = copy.deepcopy(GraphNode(spec, config_new))
for node in graphnodes:
if (neighbour_check(node, current_node, spec)) != False:
node.neighbors.append(current_node)
current_node.neighbors.append(node)
graphnodes.append(current_node)
dummy_print=[]
for i in graphnodes:
dummy_print.append(i.config)
write_robot_config_list_to_file("test_output.txt", dummy_print)
#return graphnodes
sampling_specific_points = pointsBetweenPassage(spec)
sampling_specific_points.extend(pointsNearObstruction(spec))
sampling_specific_points = list(set(sampling_specific_points))
sampling_specific_points = removeUnwantedPoints(sampling_specific_points, spec)
if a_count==3:
if config.ee1_grappled == True:
config_for_every_point = config3Obstacle(sampling_specific_points, spec, spec.grapple_points, spec.min_lengths, spec.max_lengths )
elif config.ee2_grappled == True:
grapple_points = copy.deepcopy(spec.grapple_points)
grapple_points.reverse()
min_lengths = copy.deepcopy(spec.min_lengths)
min_lengths.reverse()
max_lengths = copy.deepcopy(spec.max_lengths)
max_lengths.reverse()
config_for_every_point = config3Obstacle(sampling_specific_points, spec, grapple_points, min_lengths, max_lengths )
elif a_count==4:
if config.ee1_grappled == True:
config_for_every_point = config4Obstacle(sampling_specific_points, spec,spec.grapple_points, spec.min_lengths, spec.max_lengths)
elif config.ee2_grappled == True:
grapple_points = copy.deepcopy(spec.grapple_points)
grapple_points.reverse()
min_lengths = copy.deepcopy(spec.min_lengths)
min_lengths.reverse()
max_lengths = copy.deepcopy(spec.max_lengths)
max_lengths.reverse()
config_for_every_point = config4Obstacle(sampling_specific_points, spec, grapple_points, min_lengths, max_lengths )
else:
return graphnodes
for random_config in config_for_every_point:
if random_config== []:
continue
else:
random_config=random_config[0]
configs.append(random_config)
current_node = copy.deepcopy(GraphNode(spec, random_config))
for node in graphnodes:
if (neighbour_check(node, current_node, spec)) != False:
node.neighbors.append(current_node)
current_node.neighbors.append(node)
graphnodes.append(current_node)
return graphnodes
