def pddlstream_from_problem(problem, teleport=False, movable_collisions=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
#initial_bq = Pose(robot, get_pose(robot))
initial_bq = Conf(robot, get_group_joints(robot, 'base'), get_group_conf(robot, 'base'))
init = [
('CanMove',),
('BConf', initial_bq),
('AtBConf', initial_bq),
Equal(('PickCost',), scale_cost(1)),
Equal(('PlaceCost',), scale_cost(1)),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
for arm in ARM_NAMES:
#for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('HandEmpty', arm), ('AtAConf', arm, conf)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body))
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', body, pose)]
for surface in problem.surfaces:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
goal = [AND]
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
init += [('BConf', goal_conf)]
goal += [('AtBConf', goal_conf)]
goal += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('On', b, s) for b, s in problem.goal_on] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
stream_map = {
'sample-pose': get_stable_gen(problem),
'sample-grasp': from_list_fn(get_grasp_gen(problem)),
'inverse-kinematics': from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)),
'plan-base-motion': from_fn(get_motion_gen(problem, teleport=teleport)),
'MoveCost': move_cost_fn,
'TrajPoseCollision': fn_from_constant(False),
'TrajArmCollision': fn_from_constant(False),
'TrajGraspCollision': fn_from_constant(False),
}
if USE_SYNTHESIZERS:
stream_map['plan-base-motion'] = empty_gen(),
# get_press_gen(problem, teleport=teleport)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def gen(o, p):
# default_conf = arm_conf(a, g.carry)
# joints = get_arm_joints(robot, a)
# TODO: check collisions with fixed links
target_point = point_from_pose(p.value)
base_generator = visible_base_generator(robot, target_point,
base_range)
while True:
for _ in range(max_attempts):
set_pose(o, p.value)
base_conf = next(base_generator)
#set_base_values(robot, base_conf)
set_joint_positions(robot, base_joints, base_conf)
if any(pairwise_collision(robot, b) for b in fixed):
continue
head_conf = inverse_visibility(robot, target_point)
if head_conf is None: # TODO: test if visible
continue
#bq = Pose(robot, get_pose(robot))
bq = Conf(robot, base_joints, base_conf)
hq = Conf(robot, head_joints, head_conf)
yield (bq, hq)
break
else:
yield None
def fn(robot, body, pose, grasp):
joints = get_base_joints(robot)
robot_pose = multiply(pose.value, invert(grasp.value))
base_values = base_values_from_pose(robot_pose)
conf = Conf(robot, joints, base_values)
conf.assign()
if any(pairwise_collision(robot, obst) for obst in problem.obstacles):
return None
return (conf, )
def fn(robot, body, pose, grasp):
# TODO: reverse into the pick or place for differential drive
joints = get_base_joints(robot)
robot_pose = multiply(pose.value, invert(grasp.value))
base_values = base_values_from_pose(robot_pose)
conf = Conf(robot, joints, base_values)
conf.assign()
if any(pairwise_collision(robot, obst) for obst in problem.obstacles):
return None
return (conf, )
def gen(rover, objective):
base_joints = get_base_joints(rover)
target_point = get_point(objective)
base_generator = visible_base_generator(rover, target_point,
base_range)
lower_limits, upper_limits = get_custom_limits(rover, base_joints,
custom_limits)
attempts = 0
while True:
if max_attempts <= attempts:
attempts = 0
yield None
attempts += 1
base_conf = next(base_generator)
if not all_between(lower_limits, base_conf, upper_limits):
continue
bq = Conf(rover, base_joints, base_conf)
bq.assign()
if any(pairwise_collision(rover, b) for b in obstacles):
continue
link_pose = get_link_pose(rover,
link_from_name(rover, KINECT_FRAME))
if use_cone:
mesh, _ = get_detection_cone(rover,
objective,
camera_link=KINECT_FRAME,
depth=max_range)
if mesh is None:
continue
cone = create_mesh(mesh, color=(0, 1, 0, 0.5))
local_pose = Pose()
else:
distance = get_distance(point_from_pose(link_pose),
target_point)
if max_range < distance:
continue
cone = create_cylinder(radius=0.01,
height=distance,
color=(0, 0, 1, 0.5))
local_pose = Pose(Point(z=distance / 2.))
set_pose(cone, multiply(link_pose, local_pose))
# TODO: colors corresponding to scanned object
if any(
pairwise_collision(cone, b) for b in obstacles
if b != objective and not is_placement(objective, b)):
# TODO: ensure that this works for the rover
remove_body(cone)
continue
if not reachable_test(rover, bq):
continue
print('Visibility attempts:', attempts)
y = Ray(cone, rover, objective)
yield Output(bq, y)
def gen(rock):
base_joints = get_group_joints(rover, 'base')
x, y, _ = get_point(rock)
lower_limits, upper_limits = get_custom_limits(rover, base_joints,
custom_limits)
while True:
theta = np.random.uniform(-np.pi, np.pi)
base_conf = [x, y, theta]
if not all_between(lower_limits, base_conf, upper_limits):
continue
bq = Conf(rover, base_joints, base_conf)
bq.assign()
if any(pairwise_collision(rover, b) for b in fixed):
continue
yield (bq, )
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, teleport=False, movable_collisions=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'world': 'world',
}
world = 'world'
initial_bq = Pose(robot, get_pose(robot))
init = [
('CanMove',),
('BConf', initial_bq), # TODO: could make pose as well...
('AtBConf', initial_bq),
('AtAConf', world, None),
('AtPose', world, world, None),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
#for arm in ARM_JOINT_NAMES:
for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf),
('HandEmpty', arm), ('AtAConf', arm, conf), ('AtPose', arm, arm, None)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body))
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', world, body, pose)]
for surface in problem.surfaces:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
goal = ['and']
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
init += [('BConf', goal_conf)]
goal += [('AtBConf', goal_conf)]
goal += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('On', b, s) for b, s in problem.goal_on] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
stream_map = {
'sample-pose': get_stable_gen(problem),
'sample-grasp': from_list_fn(get_grasp_gen(problem)),
'inverse-kinematics': from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)),
'plan-base-motion': from_fn(get_motion_gen(problem, teleport=teleport)),
#'plan-base-motion': empty_gen(),
'BTrajCollision': fn_from_constant(False),
}
# get_press_gen(problem, teleport=teleport)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def gen(o, p):
if o in task.rooms: # TODO: predicate instead
return
# default_conf = arm_conf(a, g.carry)
# joints = get_arm_joints(robot, a)
# TODO: check collisions with fixed links
target_point = point_from_pose(p.value)
base_generator = visible_base_generator(robot, target_point,
base_range)
while True:
set_pose(o, p.value) # p.assign()
bq = Conf(robot, base_joints, next(base_generator))
# bq = Pose(robot, get_pose(robot))
bq.assign()
if any(pairwise_collision(robot, b) for b in fixed):
yield None
else:
yield (bq, )
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 gen(rover, rock):
base_joints = get_base_joints(rover)
x, y, _ = get_point(rock)
lower_limits, upper_limits = get_custom_limits(rover, base_joints,
custom_limits)
while True:
for _ in range(max_attempts):
theta = np.random.uniform(-np.pi, np.pi)
base_conf = [x, y, theta]
if not all_between(lower_limits, base_conf, upper_limits):
continue
bq = Conf(rover, base_joints, base_conf)
bq.assign()
if any(pairwise_collision(rover, b) for b in obstacles):
continue
if not reachable_test(rover, bq):
continue
yield Output(bq)
break
else:
yield None
def gen(objective):
base_joints = get_group_joints(robot, 'base')
head_joints = get_group_joints(robot, 'head')
target_point = get_point(objective)
base_generator = visible_base_generator(robot, target_point,
base_range)
lower_limits, upper_limits = get_custom_limits(robot, base_joints,
custom_limits)
while True:
base_conf = next(base_generator)
if not all_between(lower_limits, base_conf, upper_limits):
continue
bq = Conf(robot, base_joints, base_conf)
bq.assign()
if any(pairwise_collision(robot, b) for b in fixed):
continue
head_conf = inverse_visibility(robot, target_point)
if head_conf is None: # TODO: test if visible
continue
hq = Conf(robot, head_joints, head_conf)
y = None
yield (bq, hq, y)
def gen_fn(robot):
joints = get_base_joints(robot)
sample_fn = get_halton_sample_fn(robot, joints, custom_limits=problem.custom_limits)
collision_fn = get_collision_fn(robot, joints, problem.obstacles, attachments=[],
self_collisions=False, disabled_collisions=set(),
custom_limits=problem.custom_limits, max_distance=MAX_DISTANCE)
handles = []
while True:
sample = sample_fn()
if not collision_fn(sample):
q = Conf(robot, joints, sample)
handles.extend(draw_point(point_from_conf(sample), size=0.05))
yield (q,)
def get_reachable_test(problem, iterations=10, **kwargs):
initial_confs = {
rover: Conf(rover, get_base_joints(rover))
for rover in problem.rovers
}
motion_fn = get_motion_fn(problem,
restarts=0,
iterations=iterations,
smooth=0,
**kwargs)
def test(rover, bq):
bq0 = initial_confs[rover]
result = motion_fn(rover, bq0, bq)
return result is not None
return test
def gen(o, p):
if o in task.rooms:
#bq = Pose(robot, unit_pose())
#bq = state.poses[robot]
bq = Conf(robot, base_joints, np.zeros(len(base_joints)))
#hq = Conf(robot, head_joints, np.zeros(len(head_joints)))
#ht = create_trajectory(robot, head_joints, plan_pause_scan_path(robot, tilt=tilt))
waypoints = plan_scan_path(task.robot, tilt=tilt)
set_group_conf(robot, 'head', waypoints[0])
path = plan_waypoints_joint_motion(robot,
head_joints,
waypoints[1:],
obstacles=None,
self_collisions=False)
ht = create_trajectory(robot, head_joints, path)
yield bq, ht.path[0], ht
else:
for bq, hq in vis_gen(o, p):
ht = Trajectory([hq])
yield bq, ht.path[0], ht
def fn(o, p, bq):
set_pose(o, p.value) # p.assign()
bq.assign()
if o in task.rooms:
waypoints = plan_scan_path(task.robot, tilt=ROOM_SCAN_TILT)
set_group_conf(robot, 'head', waypoints[0])
path = plan_waypoints_joint_motion(robot,
head_joints,
waypoints[1:],
obstacles=None,
self_collisions=False)
if path is None:
return None
ht = create_trajectory(robot, head_joints, path)
hq = ht.path[0]
else:
target_point = point_from_pose(p.value)
head_conf = inverse_visibility(robot, target_point)
if head_conf is None: # TODO: test if visible
return None
hq = Conf(robot, head_joints, head_conf)
ht = Trajectory([hq])
return (hq, ht)
def pddlstream_from_state(state, teleport=False):
task = state.task
robot = task.robot
# TODO: infer open world from task
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'base': 'base',
'left': 'left',
'right': 'right',
'head': 'head',
}
#base_conf = state.poses[robot]
base_conf = Conf(robot, get_group_joints(robot, 'base'),
get_group_conf(robot, 'base'))
scan_cost = 1
init = [
('BConf', base_conf),
('AtBConf', base_conf),
Equal(('MoveCost', ), 1),
Equal(('PickCost', ), 1),
Equal(('PlaceCost', ), 1),
Equal(('ScanCost', ), scan_cost),
Equal(('RegisterCost', ), 1),
]
holding_arms = set()
holding_bodies = set()
for attach in state.attachments.values():
holding_arms.add(attach.arm)
holding_bodies.add(attach.body)
init += [('Grasp', attach.body, attach.grasp),
('AtGrasp', attach.arm, attach.body, attach.grasp)]
for arm in ARM_NAMES:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('AtAConf', arm, conf)]
if arm in task.arms:
init += [('Controllable', arm)]
if arm not in holding_arms:
init += [('HandEmpty', arm)]
for body in task.get_bodies():
if body in holding_bodies:
continue
# TODO: no notion whether observable actually corresponds to the correct thing
pose = state.poses[body]
init += [
('Pose', body, pose),
('AtPose', body, pose),
('Observable', pose),
]
init += [('Scannable', body) for body in task.rooms + task.surfaces]
init += [('Registerable', body) for body in task.movable]
init += [('Graspable', body) for body in task.movable]
for body in task.get_bodies():
supports = task.get_supports(body)
if supports is None:
continue
for surface in supports:
p_obs = state.b_on[body].prob(surface)
cost = revisit_mdp_cost(0, scan_cost,
p_obs) # TODO: imperfect observation model
init += [('Stackable', body, surface),
Equal(('LocalizeCost', surface, body), clip_cost(cost))]
#if is_placement(body, surface):
if is_center_stable(body, surface):
if body in holding_bodies:
continue
pose = state.poses[body]
init += [('Supported', body, pose, surface)]
for body in task.get_bodies():
if state.is_localized(body):
init.append(('Localized', body))
else:
init.append(('Uncertain', body))
if body in state.registered:
init.append(('Registered', body))
goal = And(*[('Holding', a, b) for a, b in task.goal_holding] + \
[('On', b, s) for b, s in task.goal_on] + \
[('Localized', b) for b in task.goal_localized] + \
[('Registered', b) for b in task.goal_registered])
stream_map = {
'sample-pose':
get_stable_gen(task),
'sample-grasp':
from_list_fn(get_grasp_gen(task)),
'inverse-kinematics':
from_gen_fn(get_ik_ir_gen(task, teleport=teleport)),
'plan-base-motion':
from_fn(get_motion_gen(task, teleport=teleport)),
'test-vis-base':
from_test(get_in_range_test(task, VIS_RANGE)),
'test-reg-base':
from_test(get_in_range_test(task, REG_RANGE)),
'sample-vis-base':
accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, VIS_RANGE)),
max_attempts=25),
'sample-reg-base':
accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, REG_RANGE)),
max_attempts=25),
'inverse-visibility':
from_fn(get_inverse_visibility_fn(task)),
}
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def pddlstream_from_problem(problem, collisions=True, **kwargs):
# 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 = {}
camera = 'RGBD'
mode = 'color' # highres | lowres | color | depth
init = []
goal_literals = [
#('Calibrated', camera, problem.rovers[0])
#('Analyzed', problem.rovers[0], problem.rocks[0])
#('ReceivedAnalysis', problem.rocks[0])
Exists(['?rock'],
And(('Type', '?rock', 'stone'), ('ReceivedAnalysis', '?rock'))),
Exists(['?soil'],
And(('Type', '?soil', 'soil'), ('ReceivedAnalysis', '?soil'))),
]
init += [('Type', b, ty) for b, ty in problem.body_types]
init += [('Lander', l) for l in problem.landers]
init += [('Camera', camera), ('Supports', camera, mode), ('Mode', mode)]
for rover in problem.rovers:
base_joints = get_base_joints(rover)
q0 = Conf(rover, base_joints)
init += [('Rover', rover), ('OnBoard', camera, rover),
('Conf', rover, q0), ('AtConf', rover, q0)]
goal_literals += [('AtConf', rover, q0)]
for store in problem.stores:
init += [('Store', store)]
init += [('Free', rover, store) for rover in problem.rovers]
goal_literals += [('Free', rover, store) for rover in problem.rovers]
for name in problem.objectives:
init += [('Objective', name)]
goal_literals += [('ReceivedImage', name, mode)]
for name in problem.rocks:
init += [('Rock', name)]
goal_formula = And(*goal_literals)
custom_limits = {}
if problem.limits is not None:
for rover in problem.rovers:
custom_limits.update(get_base_custom_limits(rover, problem.limits))
stream_map = {
'test-cfree-ray-conf':
from_test(get_cfree_ray_test(problem, collisions=collisions)),
'test-reachable':
from_test(
get_reachable_test(problem,
custom_limits=custom_limits,
collisions=collisions,
**kwargs)),
'obj-inv-visible':
from_gen_fn(
get_inv_vis_gen(problem,
custom_limits=custom_limits,
collisions=collisions,
**kwargs)),
'com-inv-visible':
from_gen_fn(
get_inv_com_gen(problem,
custom_limits=custom_limits,
collisions=collisions,
**kwargs)),
'sample-above':
from_gen_fn(
get_above_gen(problem,
custom_limits=custom_limits,
collisions=collisions,
**kwargs)),
'sample-motion':
from_fn(
get_motion_fn(problem,
custom_limits=custom_limits,
collisions=collisions,
**kwargs)),
}
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)
def pddlstream_from_problem(problem, collisions=True, teleport=False):
#rovers = problem.rovers
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
camera = 'RGBD'
mode = 'color' # highres | lowres | color | depth
init = []
goal_literals = [
#('Calibrated', camera, problem.rovers[0])
#('Analyzed', problem.rovers[0], problem.rocks[0])
#('ReceivedAnalysis', problem.rocks[0])
]
init += [('Lander', l) for l in problem.landers]
init += [('Camera', camera), ('Supports', camera, mode), ('Mode', mode)]
for rover in problem.rovers:
base_joints = get_group_joints(rover, 'base')
bq0 = Conf(rover, base_joints)
head_joints = get_group_joints(rover, 'head')
hq0 = Conf(rover, head_joints)
init += [('Rover', rover), ('OnBoard', camera, rover),
('BConf', bq0), ('AtBConf', rover, bq0),
('HConf', hq0), ('AtHConf', rover, hq0)]
for name in problem.stores:
init += [('Store', name)]
init += [('Free', rover, name) for rover in problem.rovers]
for name in problem.objectives:
init += [('Objective', name)]
#initial_atoms += [IsClass(name, cl) for cl in CLASSES if cl in name]
goal_literals += [('ReceivedImage', name, mode)]
for name in problem.rocks + problem.soils:
# pose = Pose(name, get_pose(env.GetKinBody(name)))
init += [('Rock', name)] # , IsPose(name, pose), AtPose(name, pose)]
#init += [('IsClass', name, cl) for cl in CLASSES if cl in name]
#if problem.rocks:
# goal_literals.append(Exists(['?rock'], And(('Rock', '?rock'),
# ('ReceivedAnalysis', '?rock'))))
#if problem.soils:
# goal_literals.append(Exists(['?soil'], And(('Soil', '?soil'),
# ('ReceivedAnalysis', '?soil')))) # TODO: soil class
goal_formula = And(*goal_literals)
custom_limits = {}
if problem.limits is not None:
for rover in problem.rovers:
custom_limits.update(get_base_custom_limits(rover, problem.limits))
stream_map = {
'test-reachable': from_test(get_reachable_test(problem)),
'obj-inv-visible': from_gen_fn(get_inv_vis_gen(problem, custom_limits=custom_limits)),
'com-inv-visible': from_gen_fn(get_inv_com_gen(problem, custom_limits=custom_limits)),
'sample-above': from_gen_fn(get_above_gen(problem, custom_limits=custom_limits)),
'sample-base-motion': from_fn(get_base_motion_fn(problem, custom_limits=custom_limits, teleport=teleport)),
'sample-head-motion': from_fn(get_head_motion_fn(problem, teleport=teleport)),
}
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
def pddlstream_from_problem(problem,
base_limits=None,
collisions=True,
teleport=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'@sink': 'sink',
'@stove': 'stove',
}
#initial_bq = Pose(robot, get_pose(robot))
initial_bq = Conf(robot, get_group_joints(robot, 'base'),
get_group_conf(robot, 'base'))
init = [
('CanMove',),
('BConf', initial_bq),
('AtBConf', initial_bq),
Equal(('PickCost',), 1),
Equal(('PlaceCost',), 1),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
for arm in ARM_NAMES:
#for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('HandEmpty', arm),
('AtAConf', arm, conf)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body), init=True) # TODO: supported here
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', body, pose), ('Stackable', body, None)]
for surface in problem.surfaces:
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
for body, ty in problem.body_types:
init += [('Type', body, ty)]
bodies_from_type = get_bodies_from_type(problem)
goal_literals = []
if problem.goal_conf is not None:
goal_conf = Conf(robot, get_group_joints(robot, 'base'),
problem.goal_conf)
init += [('BConf', goal_conf)]
goal_literals += [('AtBConf', goal_conf)]
for ty, s in problem.goal_on:
bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(
ty) else [ty]
init += [('Stackable', b, s) for b in bodies]
goal_literals += [('On', ty, s)]
goal_literals += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
goal_formula = []
for literal in goal_literals:
parameters = [a for a in get_args(literal) if is_parameter(a)]
if parameters:
type_literals = [('Type', p, get_parameter_name(p))
for p in parameters]
goal_formula.append(
Exists(parameters, And(literal, *type_literals)))
else:
goal_formula.append(literal)
goal_formula = And(*goal_formula)
custom_limits = {}
if base_limits is not None:
custom_limits.update(get_custom_limits(robot, problem.base_limits))
stream_map = {
'sample-pose':
from_gen_fn(get_stable_gen(problem, collisions=collisions)),
'sample-grasp':
from_list_fn(get_grasp_gen(problem, collisions=collisions)),
#'sample-grasp': from_gen_fn(get_grasp_gen(problem, collisions=collisions)),
'inverse-kinematics':
from_gen_fn(
get_ik_ir_gen(problem,
custom_limits=custom_limits,
collisions=collisions,
teleport=teleport)),
'plan-base-motion':
from_fn(
get_motion_gen(problem,
custom_limits=custom_limits,
collisions=collisions,
teleport=teleport)),
'test-cfree-pose-pose':
from_test(get_cfree_pose_pose_test(collisions=collisions)),
'test-cfree-approach-pose':
from_test(get_cfree_approach_pose_test(problem,
collisions=collisions)),
'test-cfree-traj-pose':
from_test(get_cfree_traj_pose_test(robot, collisions=collisions)),
#'test-cfree-traj-grasp-pose': from_test(get_cfree_traj_grasp_pose_test(problem, collisions=collisions)),
#'MoveCost': move_cost_fn,
'Distance':
distance_fn,
}
#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
