def check_valid(config, spec):
if len(config.points) <= 1:
return True
return (tester.test_angle_constraints(config, spec)
and tester.test_self_collision(config)
and tester.test_obstacle_collision(config, spec, spec.obstacles)
and tester.test_environment_bounds(config))
def detect_collision(spec, config):
if test_environment_bounds(config) and test_angle_constraints(config, spec) and \
test_length_constraints(config, spec) and test_grapple_point_constraint(config, spec) and \
test_self_collision(config, spec) and test_obstacle_collision(config, spec, spec.obstacles):
return True
else:
return False
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 individual_config_collision_checking(spec, config):
# Obstacle Collision - true for pass, false for fail
# Self Collision - return true for pass, false for fail
# Environment Bound - return true for pass, false for fail
# Angle Constraints - true for pass, false for fail
return test_obstacle_collision(
config, spec, spec.obstacles) and test_self_collision(
config, spec) and test_environment_bounds(
config) and test_angle_constraints(config, spec)
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 collison_checking(new_config, spec, obstacles):
"""
return true for pass, false for fail
1. It will not collide with any of the obstacles
2. It will not collide with itself
3. The entire Canadarm robotic arm must lie inside the workspace
"""
if (test_obstacle_collision(new_config, spec, obstacles)
and test_self_collision(new_config, spec)
and test_environment_bounds(new_config)):
return True
return False
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 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 solve(spec, output):
"""
An example solve method containing code to perform 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.
If you wish to use this code, you may either copy this code into your own file or add your existing code to this
file.
:param spec: ProblemSpec object
:return: List of configs forming a path through the graph from initial to goal
"""
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
# TODO: Insert your code to build the state graph here
# *** example for adding neighbors ***
# if path between n1 and n2 is collision free:
# n1.neighbors.append(n2)
# n2.neighbors.append(n1)
vertices, edges = state_graph(350, 45, 5, init_node, goal_node, spec)
# 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)
# print(current.__hash__())
if test_config_equality(current.config, spec.goal, spec):
# found path to goal
# print(init_visited[current])
print("found")
# for i in init_visited[current]:
# print(str(i))
write_robot_config_list_to_file(output, primitive_step(init_visited[current]))
print("finish")
return init_visited[current]
successors = current.get_successors()
for suc in successors:
if suc not in init_visited:
if test_self_collision(suc.config, spec) \
and test_environment_bounds(suc.config) \
and test_obstacle_collision(suc.config, spec, spec.obstacles):
init_container.append(suc)
init_visited[suc] = init_visited[current] + [suc.config]
def sample_config(spec):
"""
Creates two random RobotConfig objects. If non-grappled EE of only one RobotConfig collides with an obstacle, then
the other RobotConfig is returned.
:param spec: The ProblemSpec object
:return: RobotConfig Object, otherwise None
"""
# TODO: MAKE THIS SO THAT IT CHECKS COORDINATES OF 2 END EFFECTORS TO DECIDE WHETHER TO SAMPLE POINT
# Right now this is uniform random sampling
config = generate_config(spec)
collides = test_obstacle_collision(config, spec, spec.obstacles)
if collides:
return config
else:
return None
def valid_config(config, spec):
"""
#Imported from tester
#Self Collision
#Bound check
#Angle constraints
#Length constraints
#Obstacle collision
"""
if test_self_collision(config, spec) == False:
return False
if test_environment_bounds(config) == False:
return False
if test_angle_constraints(config, spec) == False:
return False
if test_length_constraints(config, spec) == False:
return False
if test_obstacle_collision(config, spec, spec.obstacles) == False:
return False
return True
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
