def apply(self, state, **kwargs):
# TODO: identify surface automatically
with LockRenderer():
cone = get_viewcone(color=(1, 0, 0, 0.5))
set_pose(cone, get_link_pose(self.robot, self.link))
wait_for_duration(1e-2)
wait_for_duration(self._duration) # TODO: don't sleep if no viewer?
remove_body(cone)
wait_for_duration(1e-2)
if self.detect:
# TODO: the collision geometries are being visualized
# TODO: free the renderer
detections = get_visual_detections(self.robot,
camera_link=self.camera_frame)
print('Detections:', detections)
for body, dist in state.b_on.items():
obs = (body in detections) and (is_center_stable(
body, self.surface))
if obs or (self.surface not in state.task.rooms):
# TODO: make a command for scanning a room instead?
dist.obsUpdate(get_observation_fn(self.surface), obs)
#state.localized.update(detections)
# TODO: pose for each object that can be real or fake
yield
def set_delta_belief(task, b_on, body):
supports = task.get_supports(body)
if supports is None:
b_on[body] = DeltaDist(supports)
return
for bottom in task.get_supports(body):
if is_center_stable(body, bottom):
b_on[body] = DeltaDist(bottom)
return
raise RuntimeError('No support for body {}'.format(body))
def apply(self, state, **kwargs):
# TODO: identify surface automatically
cone = get_viewcone(color=(1, 0, 0, 0.5))
set_pose(cone, get_link_pose(self.robot, self.link))
wait_for_duration(self._duration) # TODO: don't sleep if no viewer?
remove_body(cone)
detections = get_visual_detections(self.robot)
print('Detections:', detections)
for body, dist in state.b_on.items():
obs = (body in detections) and (is_center_stable(
body, self.surface))
if obs or (self.surface not in state.task.rooms):
# TODO: make a command for scanning a room instead?
dist.obsUpdate(get_observation_fn(self.surface), obs)
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)