def get_successors(self, state):
"""This is the successor state function of the problem. It returns a set
of states, with each containing a single cluttered body with an
augmented freedom."""
neighbors = set()
for body in state.originals:
buuid = body.oid
cuuid, rot_cuuid = self.const_dict["{}".format(body.oid)]
if rot_cuuid is not None:
rot_const = body.find_constraint(rot_cuuid)
body.remove_constraint(rot_const)
self.const_dict["{}".format(body.oid)] = (cuuid, None)
const = body.find_constraint(cuuid)
geometry = const.geometry
limit = state.get_max_displacement(body)
delta = limit / 4
if geometry["radius"] + delta <= limit:
geometry["radius"] += delta
successor = copy.deepcopy(state)
successor.find_body(buuid).find_constraint(
cuuid).geometry = geometry
successor = LocalSearch(
Middle(successor, self.engine, start_time=self.start_time),
start_time=self.start_time,
).simple()
neighbors.add(successor)
return neighbors
def __init__(self, init_state, engine, start_time=time.time()):
# For each original body, add a circular constraint of radius zero, and
# store the relationship between each body and its constraint in a dict
self.start_time = start_time
init_state = copy.deepcopy(init_state)
self.engine = engine
self.const_dict = {}
for body in init_state.originals:
center_x, center_y, angle = body.init_pose
geometry = {"center": (center_x, center_y), "radius": 0.0}
const = Constraint("circular", geometry)
rot_geometry = {"max": angle, "min": angle}
rot_const = Constraint("rotational", rot_geometry)
body.add_constraint(const)
body.add_constraint(rot_const)
buuid = body.oid
cuuid = const.oid
rot_cuuid = rot_const.oid
self.const_dict.update({"{}".format(buuid): (cuuid, rot_cuuid)})
middle = Middle(init_state, engine, start_time=start_time)
init_state = LocalSearch(middle, start_time=start_time).simple()
self.engine.configuration = init_state
super(Outer, self).__init__(init_state, maximality=False, lexi=True)
def __init__(self, init_state, engine, seed=1, start_time=time.time()):
"""Constructor/Initializer for the Middle class."""
self.start_time = start_time
self.seed = seed
self.engine = engine
inner = Inner(init_state, engine, start_time=start_time)
init_state = LocalSearch(inner, start_time=start_time).simple()
super(Middle, self).__init__(init_state, maximality=False, lexi=True)
def get_successors(self, state):
"""Get the neighbors."""
colliding = self.engine.get_collision_info(state)["list"]
colliding_movable = [x for x in colliding if x in state.movable]
neighbors = set()
for body in colliding_movable:
buuid = body.oid
free_cells = get_free_cells(self.seed, state, body)
neighbors = set()
for free_cell in free_cells:
self.engine.configuration = state
successor = copy.deepcopy(state)
pose = create_pose(free_cell, "center")
successor.find_body(buuid).pose = pose
inner = Inner(successor, self.engine)
local_search = LocalSearch(inner, start_time=self.__start_time)
successor = local_search.simple()
neighbors.add(successor)
return neighbors
def __init__(self,
init_state,
engine,
batch_size=10,
start_time=time.time()):
self.batch_size = batch_size
self.engine = engine
self.start_time = start_time
init_state = LocalSearch(Inner(init_state, engine), timeout=1).simple()
super(RandomPotentialField, self).__init__(init_state,
maximality=False,
lexi=False)
def get_neighbors(self, state):
"""Returns the list of states that are successors."""
s_aabb = state.surface.aabb_info
anti_padding = s_aabb["2D diagonal length"] * 0.0125
s_x_min = s_aabb["min x"] + anti_padding
s_y_min = s_aabb["min y"] + anti_padding
s_x_max = s_aabb["max x"] - anti_padding
s_y_max = s_aabb["max x"] - anti_padding
state = copy.deepcopy(state)
for body in state.get_movable_bodies():
pose_x = np.random.uniform(s_x_min, s_x_max)
pose_y = np.random.uniform(s_y_min, s_y_max)
pose_theta = np.random.uniform(0, 2 * np.pi)
body.pose = [pose_x, pose_y, pose_theta]
return [
LocalSearch(
Inner(state, self.engine, start_time=self.start_time),
start_time=self.start_time,
).simple()
]
def get_successors(self, state):
"""Given state S with configurations C0, C1, ..., Cn; n = |A|, it
returns m successor states S'1, S'2, ..., S'm; m <= n
Each successor state S'x has configurations
C0, C1, ..., C'z, ..., C'y, ..., C'n
where C'y is Cy with its collisions resolved by relocating objects J
C'z is the earliest configuration where an instance of J exists
Instances of objects J in C'z, C'z+1, ..., C'n relocated as in C'y
Each successor state is generated by resolving the collisions of a
single configuration in S."""
def propagate(step_index, config, successor):
"""
Propagates the changes in successor (with reference to configuration
to all other configurations in successor.
"""
# Reference configuration to which we will adjust the rest to
ref_config = config
ref_config = successor.configurations[step_index]
max_index = len(successor.configurations)
# For forward propagation, include the config index targeted
forward_prop_indices = list(range(step_index + 1, max_index))
# For backward propagation
backward_prop_indices = list(reversed(range(1, step_index)))
for ref_body_ind, ref_body in enumerate(ref_config.movable):
for_stop_flag = False
for for_ind in forward_prop_indices:
if for_stop_flag:
break
config = successor.configurations[for_ind]
body = config.movable[ref_body_ind]
same_cons = body.constraints == ref_body.constraints
if same_cons and not for_stop_flag:
body.pose = ref_body.pose
else:
for_stop_flag = True
back_stop_flag = False
for back_ind in backward_prop_indices:
config = successor.configurations[back_ind]
body = config.movable[ref_body_ind]
same_cons = body.constraints == ref_body.constraints
if same_cons and not back_stop_flag:
body.pose = ref_body.pose
else:
back_stop_flag = True
return successor
successors = set()
for index, configuration in enumerate(state.configurations):
in_collision = self.engine.get_collision_info(
configuration)["status"]
if in_collision:
# Create a new plan
successor = copy.deepcopy(state)
# Select the configuration to resolve collisions in
successor_configuration = successor.configurations[index]
# Define collision resolving local search problem
middle = Middle(copy.deepcopy(successor_configuration),
self.engine)
# Attempt to resolve collisions in selected configuration
new_config = LocalSearch(middle,
timeout=1,
start_time=self.start_time,
random_restart=False).simple()
# Propagate the changes done by resolving the collisions to
# the rest of the configurations in the plan.
successor = propagate(index, new_config, successor)
# Add the successor to the set of successors
successors.add(successor)
return successors
def generate_placement(
query,
algorithm="outer",
collision_threshold=0.01,
verbose=False,
camera_distance=10.0,
name="instance",
engine=None,
):
"""
Attempts returning a collision-free placement for the configuration.
PARAMETERS
----------
query : JSON
The parsed query.
algorithm : str
Name of algorithm to use: 'outer', 'middle', etc.
collision_threshold : float
Penetration depth threshold for collision detection.
verbose : bool
Verbosity.
camera_distance : float
Distance of camera from center of surface.
name : str
Filename base to use for debug files.
RETURNS
-------
solution : Configuration
The (attempted to be) solved configuration.
"""
if engine is None:
engine = Engine()
engine.connect(visual=verbose)
engine.collision_threshold = collision_threshold
config = engine.load_configuration(query)
if verbose:
img = engine.get_image(distance=camera_distance, new=False)
img.save(DEBUG_PATH + "/{}-initial_placement.png".format(name))
img = engine.get_image(distance=camera_distance)
img.save(DEBUG_PATH + "/{}-random_placement.png".format(name))
start_time = time.time()
random_restart = True
if algorithm.lower() == "random_sample":
problem = Random(config, engine, start_time=start_time)
elif algorithm.lower() == "inner":
problem = Inner(config, engine, start_time=start_time)
random_restart = False
elif algorithm.lower() == "random_restart":
problem = RandomPotentialField(config, engine, start_time=start_time)
elif algorithm.lower() == "middle":
problem = Middle(config, engine, start_time=start_time)
elif algorithm.lower() == "outer":
problem = Outer(config, engine, start_time=start_time)
else:
raise ValueError("Queried algorithm '{}' is unknown.".format(name))
if verbose:
solution, iterations = LocalSearch(
problem, start_time=start_time, random_restart=random_restart
).simple(verbose=True)
else:
solution = LocalSearch(
problem, start_time=start_time, random_restart=random_restart
).simple()
col_info = engine.get_collision_info(solution)
move_info = solution.movement_info
no_collisions, penetration = col_info["number"], col_info["severity"]
no_org_moved, org_movement = move_info["number"], move_info["severity"]
time_elapsed = time.time() - start_time
results = {
"algorithm": algorithm,
"cumulative original objects movement": "{:.4f} m".format(org_movement),
"cumulative penetration depth": "{:.4f} m".format(penetration),
"number of collisions": no_collisions,
# "number of iterations": iterations,
"number of original objects moved": no_org_moved,
"placement time": "{:.4f} s".format(time_elapsed),
}
if verbose:
print ("\n")
print ("Placement Generation complete!")
pprint.pprint(results)
print ("\n")
img = engine.get_image(distance=camera_distance)
img.save(DEBUG_PATH + "/{}-goal_placement.png".format(name))
with open(DEBUG_PATH + "/{}-goal_placement.json".format(name), "w") as my_ans:
json.dump(config.dump_json(), my_ans)
return solution
def colliding(
plan,
continuous_configuration_df,
discrete_configuration_df,
collision_threshold=0.01,
verbose=False,
):
"""
Checks to find if a plan is collision-free.
PARAMETERS
----------
continuous_configuration_df : DataFrame
A dataframe describing the continuous configuration.
discrete_configuration_df : DataFrame
A dataframe describing the discretized configuration.
collision_threshold : float
The minimium penetration depth between two bodies before a collision is
declared.
verbose : bool
Verbosity
RETURNS
-------
is_colliding : bool
Whether or not the plan contains a configuration in collision.
"""
engine = Engine()
engine.connect(visual=verbose)
engine.collision_threshold = collision_threshold
# Create constrained (according to DLVHEX cells) sequence of configs
constrained_configs = get_constrained_configs(plan,
continuous_configuration_df,
discrete_configuration_df,
engine)
planner_ids = continuous_configuration_df[["oid", "name", "plannerID"]]
# Get search state from queries
state = Plan(constrained_configs, engine, planner_ids)
# Initialize search problem
problem = Consistent(state, engine)
# If the initialized plan has no collisions, return False
if problem.get_value(state)[0] == 0:
state.save_plan(filename="plan", screenshots=verbose)
engine.disconnect()
return False
# Else, search for solution
solution = LocalSearch(problem, timeout=2, random_restart=False).simple()
# If the plan cannot be freed from collisions, return True
if problem.get_value(solution)[0] > 0:
engine.disconnect()
return True
# Else, store the plan with continuous poses in a csv and return False
solution.save_plan(filename="plan", screenshots=verbose)
engine.disconnect()
return False