def fn(robot, conf, body, grasp): conf.assign() link = link_from_name(robot, BASE_LINK) world_from_body = multiply(get_link_pose(robot, link), grasp.value) pose = Pose(body, world_from_body) pose.assign() if any(pairwise_collision(body, obst) for obst in problem.obstacles): return None return (pose, )
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 gen(robot, body): link = link_from_name(robot, BASE_LINK) with BodySaver(robot): set_base_conf(robot, np.zeros(3)) robot_pose = get_link_pose(robot, link) robot_aabb = get_turtle_aabb(robot) # _, upper = robot_aabb # radius = upper[0] extent = get_aabb_extent(robot_aabb) radius = max(extent[:2]) / 2. #draw_aabb(robot_aabb) center, (diameter, height) = approximate_as_cylinder(body) distance = radius + diameter / 2. + epsilon _, _, z = get_point(body) # Assuming already placed stably for [scale] in unit_generator(d=1, use_halton=use_halton): #theta = PI # 0 | PI theta = random.uniform(*theta_interval) position = np.append(distance * unit_from_theta(theta=theta), [z]) # TODO: halton yaw = scale * 2 * PI - PI quat = quat_from_euler(Euler(yaw=yaw)) body_pose = (position, quat) robot_from_body = multiply(invert(robot_pose), body_pose) grasp = Pose(body, robot_from_body) # TODO: grasp instead of pose if draw: world_pose = multiply(get_link_pose(robot, link), grasp.value) set_pose(body, world_pose) handles = draw_pose(get_pose(body), length=1) wait_for_user() remove_handles(handles) #continue yield (grasp, )
def apply(self, state, **kwargs): with LockRenderer(): self.cone = get_viewcone(color=(1, 0, 0, 0.5)) state.poses[self.cone] = None cone_pose = Pose(self.cone, unit_pose()) attach = Attach(self.robot, self.group, cone_pose, self.cone) attach.assign() wait_for_duration(1e-2) for _ in attach.apply(state, **kwargs): yield
def gen(robot, body): link = link_from_name(robot, BASE_LINK) with BodySaver(robot): set_base_conf(robot, np.zeros(3)) robot_pose = get_link_pose(robot, link) robot_aabb = get_turtle_aabb(robot) #draw_aabb(robot_aabb) lower, upper = robot_aabb center, (diameter, height) = approximate_as_cylinder(body) _, _, z = get_point(body) # Assuming already placed stably position = Point(x=upper[0] + diameter / 2., z=z) for [scale] in halton_generator(d=1): yaw = scale * 2 * np.pi - np.pi quat = quat_from_euler(Euler(yaw=yaw)) body_pose = (position, quat) robot_from_body = multiply(invert(robot_pose), body_pose) grasp = Pose(body, robot_from_body) yield (grasp, )
def pddlstream_from_problem(problem, 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 = {} #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 += [('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 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)) initial_bq = Conf(robot, get_group_joints(robot, 'base'), get_group_conf(robot, 'base')) 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 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 apply(self, state, **kwargs): self.cone = get_viewcone(color=(1, 0, 0, 0.5)) state.poses[self.cone] = None attach = Attach(self.robot, self.group, Pose(self.cone, unit_pose()), self.cone) attach.apply(state, **kwargs)