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)