def push_target_fn(p1, p2):
poses = get_push_poses(p1, p2)
confs = get_push_confs(poses)
facts = get_push_facts(poses, confs)
if 2 <= len(poses):
return WildOutput([], facts)
return WildOutput([tuple(confs)], facts)
def fn(q2):
facts = []
for q1 in vertices:
(t, ) = plan_motion(q1, q2)
facts.extend([
('Motion', q1, t, q2),
('Traj', t),
])
vertices.append(q2)
return WildOutput(facts=facts)
def push_direction_gen_fn(p1):
# TODO: if any movable objects collide with the route, activate them by adding a predicate
for direction in DIRECTIONS:
p2 = array(MAX_PUSH*direction + p1)
poses = get_push_poses(p1, p2)
confs = get_push_confs(poses)
#facts = get_push_facts(poses, confs)
facts = []
#yield [], facts
yield WildOutput([(confs[0], poses[1], confs[1])], facts)
def wild_gen_fn(name, conf1, conf2, *args):
is_initial = (conf1.element is None) and (conf2.element is not None)
is_goal = (conf1.element is not None) and (conf2.element is None)
if is_initial:
supporters = []
elif is_goal:
supporters = list(element_bodies)
else:
supporters = [conf1.element
] # TODO: can also do according to levels
retrace_supporters(conf1.element, incoming_supporters, supporters)
element_obstacles = {element_bodies[e] for e in supporters}
obstacles = set(static_obstacles) | element_obstacles
if not collisions:
obstacles = set()
robot = index_from_name(robots, name)
conf1.assign()
joints = get_movable_joints(robot)
# TODO: break into pieces at the furthest part from the structure
weights = JOINT_WEIGHTS
resolutions = np.divide(RESOLUTION * np.ones(weights.shape), weights)
disabled_collisions = get_disabled_collisions(robot)
#path = [conf1, conf2]
path = plan_joint_motion(robot,
joints,
conf2.positions,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
weights=weights,
resolutions=resolutions,
restarts=3,
iterations=100,
smooth=100)
if not path:
return
path = [conf1.positions] + path[1:-1] + [conf2.positions]
traj = MotionTrajectory(robot, joints, path)
command = Command([traj])
edges = [
(conf1, command, conf2),
(conf2, command, conf1), # TODO: reverse
]
outputs = []
#outputs = [(command,)]
facts = []
for q1, cmd, q2 in edges:
facts.extend([
('Traj', name, cmd),
('CTraj', name, cmd),
('MoveAction', name, q1, q2, cmd),
])
yield WildOutput(outputs, facts)
def wild_gen_fn(name, node1, element, node2):
# TODO: could cache this
# sequence = [result.get_mapping()['?e'].value for result in CURRENT_STREAM_PLAN]
# index = sequence.index(element)
# printed = sequence[:index]
# TODO: this might need to be recomputed per iteration
# TODO: condition on plan/downstream constraints
# TODO: stream fusion
# TODO: split element into several edges
robot = index_from_name(robots, name)
q0 = initial_confs[name]
#generator = gen_fn_from_robot[robot](node1, element)
for print_cmd, in gen_fn_from_robot[robot](node1, element):
# TODO: need to merge safe print_cmd.colliding
q1 = Conf(robot, print_cmd.start_conf, node=node1, element=element)
q2 = Conf(robot, print_cmd.end_conf, node=node2, element=element)
if return_home:
# TODO: can decompose into individual movements as well
output1 = next(wild_move_fn(name, q0, q1), None)
if not output1:
continue
transit_cmd1 = output1.values[0][0]
print_cmd.trajectories = transit_cmd1.trajectories + print_cmd.trajectories
output2 = next(wild_move_fn(name, q2, q0), None)
if not output2:
continue
transit_cmd2 = output2.values[0][0]
print_cmd.trajectories = print_cmd.trajectories + transit_cmd2.trajectories
q1 = q2 = q0 # TODO: must assert that AtConf holds
outputs = [(q1, q2, print_cmd)]
# Prevents premature collision checks
facts = [('CTraj', name, print_cmd)
] # + [('Dummy',)] # To force to be wild
if collisions:
facts.extend(
('Collision', print_cmd, e2) for e2 in print_cmd.colliding)
yield WildOutput(outputs, facts)
def to_wild(self):
return WildOutput(self.assignments, self.facts)
def push_target_fn(p1, p2):
poses = get_push_poses(p1, p2)
confs = get_push_confs(poses)
if len(poses) == 2:
return WildOutput(values=[tuple(confs)])
return WildOutput(facts=get_push_facts(poses, confs))