def parallel_connect_worker(batches, interp_fn, distance_fn, sim_context_cls,
sim_config):
sim_context = sim_context_cls(sim_config, setup=False)
sim_context.setup(sim_overrides={"run_parallel": True, "use_gui": False})
sim = sim_context.get_sim_instance()
_ = sim_context.get_logger()
_ = sim_context.get_state_space()
svc = sim_context.get_state_validity()
sawyer_robot = sim_context.get_robot()
_ = sawyer_robot.get_simulator_id()
# Disabled collisions during planning with certain exclusions in place.
# collision_exclusions = sim_context.get_collision_exclusions()
collision_exclusions = {}
with DisabledCollisionsContext(sim, **collision_exclusions):
connections = []
for batch in batches:
q_sample = batch[0]
neighbors = batch[1]
for q_near in neighbors:
local_path = interp_fn(np.array(q_near),
np.array(q_sample),
steps=10)
valid = subdivision_evaluate(svc.validate, local_path)
if valid:
connections.append(
[q_near, q_sample,
distance_fn(local_path)])
return connections
def _extend(self, q_near, q_rand):
local_path = self.interp_fn(np.array(q_near), np.array(q_rand))
valid = subdivision_evaluate(self.svc.validate, local_path)
if valid:
return True, local_path
else:
return False, []
def get_lazy_path(self):
success = False
# The successful path we will build by iteratively moving through the best current path and redirecting it as needed according
# to state validity etc,.
successful_vertex_sequence = []
# Find the initial best path in the graph, if it exists.
current_best_plan = self._get_best_path("start", "goal")
while not success:
if current_best_plan is None or len(current_best_plan) == 1:
return []
points = [(_id,
self.graph.vs[self.graph.vs['id'].index(_id)]['value'])
for _id in current_best_plan]
for idx, _ in enumerate(points):
# we first get the current or 'from' vertex id of wherever we are in the path.
from_id = points[idx][0]
# we first get the current or 'from' value of wherever we are in the path.
from_value = points[idx][1]
to_id = points[idx + 1][0] # get the 'to' point vertex id
to_value = points[idx + 1][1] # get the 'to' point value
if to_id in successful_vertex_sequence:
continue
if self._validate(to_value): # validate the 'to' point value
# generate an interpolated path between 'from' and 'to'
segment = self.interp_fn(np.array(from_value),
np.array(to_value))
# perform segment evaluation for state validity etc
valid = subdivision_evaluate(self.svc.validate, segment)
if valid:
# if valid, we add the 'to' vertex id since we know we can reach it
successful_vertex_sequence.append(to_id)
goal_idx = self.graph.vs[self.graph.vs['name'].index(
"goal")].index
if to_id == goal_idx:
success = True # if we made a path to goal, then planning was a success and we can break out
break
current_best_plan = self._get_best_path(
"start", "goal")
else:
self._remove_edge(from_id, to_id)
current_best_plan = self._get_best_path(
"start", "goal")
successful_vertex_sequence = []
break # we break out of looping with for loop to generate a new best path
else:
# if the 'to' point is invalid, then we remove it and associated edges from the graph
self._remove_node_and_edges(to_id)
successful_vertex_sequence = []
current_best_plan = self._get_best_path("start", "goal")
break
successful_vertex_sequence.insert(
0, self.graph.vs[self.graph.vs['name'].index("start")].index)
return [
self.graph.vs['id'].index(_id)
for _id in successful_vertex_sequence
]
def shortcut(plan):
idx_range = [i for i in range(0, len(plan))]
indexed_plan = list(zip(idx_range, plan))
for curr_idx, vid1 in indexed_plan:
for test_idx, vid2 in indexed_plan[::-1]:
v1 = self.graph.vs[vid1]
v2 = self.graph.vs[vid2]
p1 = self.graph.vs[vid1]["value"]
p2 = self.graph.vs[vid2]["value"]
segment = self.interp_fn(np.array(p1), np.array(p2))
valid = subdivision_evaluate(self.svc.validate, segment)
if test_idx == curr_idx + 1:
break
if valid and curr_idx < test_idx:
del plan[curr_idx + 1:test_idx]
return False, plan
return True, plan
def parallel_connect_worker(batches):
########################################################
# Create the Simulator and pass it a Logger (optional) #
########################################################
logger = Logger()
if not Simulator.is_instantiated():
sim = Simulator(logger=logger, use_ros=False, use_gui=False,
use_real_time=True) # Initialize the Simulator
else:
sim = Simulator.get_instance()
#####################################
# Create a Robot, or two, or three. #
#####################################
sawyer_robot = Sawyer("sawyer0", [0, 0, 0.9], fixed_base=1)
#############################################
# Create sim environment objects and assets #
#############################################
ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0])
sawyer_id = sawyer_robot.get_simulator_id()
# Exclude the ground plane and the pedestal feet from disabled collisions.
excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane
pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx
# The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions.
excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)]
##########
# EXTEND #
##########
valid_samples = []
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, excluded_bodies, excluded_body_link_pairs):
#########################
# STATE SPACE SELECTION #
#########################
# This inherently uses UniformSampler but a different sampling class could be injected.
state_space = SawyerConfigurationSpace()
##############################
# STATE VALIDITY FORMULATION #
##############################
# Certain links in Sawyer seem to be permentently in self collision. This is how to remove them by name when getting all link pairs to check for self collision.
excluded_pairs = [(get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "right_l0").idx),
(get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "head").idx)]
link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs)
self_collision_fn = partial(
self_collision_test, robot=sawyer_robot, link_pairs=link_pairs)
# Create constraint checks
# def constraint_test(q):
# upright_orientation = [
# 0.0005812598018143569, 0.017721236427960724, -0.6896867930096543, 0.723890701324838]
# axis = 'z'
# threshold = 15
# world_pose, _ = sawyer_robot.solve_forward_kinematics(q)
# return orientation(upright_orientation, world_pose[1], threshold, axis)
# In this case, we only have a self_col_fn.
svc = StateValidityChecker(
self_col_func=self_collision_fn, col_func=None, validity_funcs=None)
# svc = StateValidityChecker(
# self_col_func=self_collision_fn, col_func=None, validity_funcs=[constraint_test])
pairs = []
for batch in batches:
q_sample = batch[0]
neighbors = batch[1]
for q_near in neighbors:
local_path = parametric_lerp(
np.array(q_near), np.array(q_sample), steps=10)
valid = subdivision_evaluate(svc.validate, local_path)
if valid:
pairs.append(
[q_near, q_sample, cumulative_distance(local_path)])
return pairs
# Goal is always at the 1 index.
graph.vs[1]["value"] = end
for sample in samples:
graph.add_vertex(None, **{'value': list(sample)})
for pair in connections:
graph.add_edge(graph.vs['value'].index(list(pair[0])), graph.vs['value'].index(
list(pair[1])), **{'weight': pair[2]})
distances, neighbors = nn.query(np.array(start), k=10)
for q_near in neighbors:
if graph.vs['value'].index(q_near) != 0:
local_path = parametric_lerp(
np.array(start), np.array(q_near), steps=10)
valid = subdivision_evaluate(svc.validate, local_path)
if valid:
graph.add_edge(graph.vs['value'].index(start), graph.vs['value'].index(list(q_near)), **{'weight': pair[2]})
break
distances, neighbors = nn.query(np.array(end), k=10)
for q_near in neighbors:
if graph.vs['value'].index(q_near) != 0:
local_path = parametric_lerp(
np.array(start), np.array(q_near), steps=10)
valid = subdivision_evaluate(svc.validate, local_path)
if valid:
graph.add_edge(graph.vs['value'].index(end), graph.vs['value'].index(list(q_near)), **{'weight': pair[2]})
break
graph_path = graph.get_shortest_paths(
'start', 'goal', weights='weight', mode='ALL')[0]