def __init__(self,
robots,
limits,
movable=[],
collisions=True,
goal_holding={},
goal_confs={}):
self.robots = tuple(robots)
self.limits = limits
self.movable = tuple(movable)
self.collisions = collisions
self.goal_holding = goal_holding
self.goal_confs = goal_confs
self.obstacles = tuple(body for body in get_bodies()
if body not in self.robots + self.movable)
self.custom_limits = {}
if self.limits is not None:
for robot in self.robots:
self.custom_limits.update(get_custom_limits(
robot, self.limits))
self.initial_confs = {
robot: Conf(robot, get_base_joints(robot))
for robot in self.robots
}
self.initial_poses = {body: Pose(body) for body in self.movable}
def __init__(self,
body_from_name,
robots,
limits,
collisions=True,
goal_confs={}):
self.body_from_name = dict(body_from_name)
self.robots = tuple(robots)
self.limits = limits
self.collisions = collisions
self.goal_confs = dict(goal_confs)
robot_bodies = set(map(self.get_body, self.robots))
self.obstacles = tuple(body for body in get_bodies()
if body not in robot_bodies)
self.custom_limits = {}
if self.limits is not None:
for body in robot_bodies:
self.custom_limits.update(
get_custom_limits(body, self.limits, yaw_limit=(0, 0)))
self.initial_confs = {
name: Conf(self.get_body(name),
get_base_joints(self.get_body(name)))
for name in self.robots
}
def pddlstream_from_problem(problem):
# TODO: push and attach to movable objects
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
# TODO: action to generically connect to the roadmap
# TODO: could check individual vertices first
# TODO: dynamically generate the roadmap in interesting parts of the space
# TODO: visibility graphs for sparse roadmaps
# TODO: approximate robot with isotropic geometry
# TODO: make the effort finite if applied to the roadmap vertex
samples = []
init = []
for robot, conf in problem.initial_confs.items():
samples.append(conf)
init += [('Robot', robot), ('Conf', robot, conf),
('AtConf', robot, conf), ('Free', robot)]
for body, pose in problem.initial_poses.items():
init += [('Body', body), ('Pose', body, pose), ('AtPose', body, pose)]
goal_literals = []
goal_literals += [('Holding', robot, body)
for robot, body in problem.goal_holding.items()]
for robot, base_values in problem.goal_confs.items():
q_goal = Conf(robot, get_base_joints(robot), base_values)
samples.append(q_goal)
init += [('Conf', robot, q_goal)]
goal_literals += [('AtConf', robot, q_goal)]
goal_formula = And(*goal_literals)
# TODO: assuming holonomic for now
[body] = problem.robots
with LockRenderer():
init += create_vertices(problem, body, samples)
#init += create_edges(problem, body, samples)
stream_map = {
'test-cfree-conf-pose': from_test(get_test_cfree_conf_pose(problem)),
'test-cfree-traj-pose': from_test(get_test_cfree_traj_pose(problem)),
# TODO: sample pushes rather than picks/places
'sample-grasp': from_gen_fn(get_grasp_generator(problem)),
'compute-ik': from_fn(get_ik_fn(problem)),
'compute-motion': from_fn(get_motion_fn(problem)),
'test-reachable': from_test(lambda *args: False),
'Cost': get_cost_fn(problem),
}
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)
def pddlstream_from_problem(problem, teleport=False):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {'{}'.format(name).lower(): name
for name in problem.initial_confs.keys()}
edges = set()
init = [
]
for name, conf in problem.initial_confs.items():
init += [
('Safe',),
('Robot', name),
('Conf', conf),
('AtConf', name, conf),
Equal(('Speed', name), DIST_PER_TIME),
Equal(('BatteryCapacity', name), MAX_ENERGY),
Equal(('RechargeRate', name), CHARGE_PER_TIME),
Equal(('ConsumptionRate', name), BURN_PER_TIME),
Equal(('Energy', name), INITIAL_ENERGY),
]
goal_literals = [
#('Safe',),
#('Unachievable',),
]
for name, base_values in problem.goal_confs.items():
body = problem.get_body(name)
joints = get_base_joints(body)
#extend_fn = get_extend_fn(body, joints, resolutions=5*BASE_RESOLUTIONS)
#q_init = problem.initial_confs[name]
#path = [q_init] + [Conf(body, joints, q) for q in extend_fn(q_init.values, base_values)]
#edges.update(zip(path, path[1:]))
#q_goal = path[-1]
q_goal = Conf(body, joints, base_values)
init += [('Conf', q_goal)]
goal_literals += [('AtConf', name, q_goal)]
goal_formula = And(*goal_literals)
robot = list(map(problem.get_body, problem.initial_confs))[0]
with LockRenderer():
init += [('Conf', q) for _, n, q in create_vertices(problem, robot, [], samples_per_ft2=6)]
#vertices = {v for edge in edges for v in edge}
#handles = []
#for vertex in vertices:
# handles.extend(draw_point(point_from_conf(vertex.values), size=0.05))
#for conf1, conf2 in edges:
# traj = linear_trajectory(conf1, conf2)
# init += [
# ('Conf', conf1),
# ('Traj', traj),
# ('Conf', conf2),
# ('Motion', conf1, traj, conf2),
# ]
#draw_edges(edges)
cfree_traj_traj_test = get_test_cfree_traj_traj(problem)
cfree_traj_traj_list_gen_fn = from_test(cfree_traj_traj_test)
traj_traj_collision_test = negate_test(cfree_traj_traj_test)
stream_map = {
'test-cfree-conf-conf': cfree_traj_traj_list_gen_fn,
'test-cfree-traj-conf': cfree_traj_traj_list_gen_fn,
'test-cfree-traj-traj': cfree_traj_traj_list_gen_fn,
'ConfConfCollision': traj_traj_collision_test,
'TrajConfCollision': traj_traj_collision_test,
'TrajTrajCollision': traj_traj_collision_test,
'compute-motion': from_fn(get_motion_fn2(problem)),
'TrajDistance': get_distance_fn(),
}
#stream_map = 'debug'
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
return problem, edges