def fix_pose(self, name, pose=None, fraction=0.5): body = self.get_body(name) if pose is None: pose = get_pose(body) else: set_pose(body, pose) # TODO: can also drop in simulation x, y, z = point_from_pose(pose) roll, pitch, yaw = euler_from_quat(quat_from_pose(pose)) quat = quat_from_euler(Euler(yaw=yaw)) set_quat(body, quat) surface_name = self.get_supporting(name) if surface_name is None: return None, None if fraction == 0: new_pose = (Point(x, y, z), quat) return new_pose, surface_name surface_aabb = compute_surface_aabb(self, surface_name) new_z = (1 - fraction) * z + fraction * stable_z_on_aabb( body, surface_aabb) point = Point(x, y, new_z) set_point(body, point) print('{} error: roll={:.3f}, pitch={:.3f}, z-delta: {:.3f}'.format( name, roll, pitch, new_z - z)) new_pose = (point, quat) return new_pose, surface_name
def water_test(q): if attachment is None: return False set_joint_positions(body, joints, q) attachment.assign() attachment_pose = get_pose(attachment.child) pose = multiply(attachment_pose, reference_pose) # TODO: confirm not inverted roll, pitch, _ = euler_from_quat(quat_from_pose(pose)) violation = (MAX_ROTATION < abs(roll)) or (MAX_ROTATION < abs(pitch)) #if violation: # TODO: check whether different confs can be waypoints for each object # print(q, violation) # wait_for_user() return violation
def main(floor_width=2.0): # Creates a pybullet world and a visualizer for it connect(use_gui=True) identity_pose = (unit_point(), unit_quat()) origin_handles = draw_pose( identity_pose, length=1.0 ) # Draws the origin coordinate system (x:RED, y:GREEN, z:BLUE) # Bodies are described by an integer index floor = create_box(w=floor_width, l=floor_width, h=0.001, color=TAN) # Creates a tan box object for the floor set_point(floor, [0, 0, -0.001 / 2.]) # Sets the [x,y,z] translation of the floor obstacle = create_box(w=0.5, l=0.5, h=0.1, color=RED) # Creates a red box obstacle set_point( obstacle, [0.5, 0.5, 0.1 / 2.]) # Sets the [x,y,z] position of the obstacle print('Position:', get_point(obstacle)) set_euler(obstacle, [0, 0, np.pi / 4 ]) # Sets the [roll,pitch,yaw] orientation of the obstacle print('Orientation:', get_euler(obstacle)) with LockRenderer( ): # Temporarily prevents the renderer from updating for improved loading efficiency with HideOutput(): # Temporarily suppresses pybullet output robot = load_model(ROOMBA_URDF) # Loads a robot from a *.urdf file robot_z = stable_z( robot, floor ) # Returns the z offset required for robot to be placed on floor set_point(robot, [0, 0, robot_z]) # Sets the z position of the robot dump_body(robot) # Prints joint and link information about robot set_all_static() # Joints are also described by an integer index # The turtlebot has explicit joints representing x, y, theta x_joint = joint_from_name(robot, 'x') # Looks up the robot joint named 'x' y_joint = joint_from_name(robot, 'y') # Looks up the robot joint named 'y' theta_joint = joint_from_name( robot, 'theta') # Looks up the robot joint named 'theta' joints = [x_joint, y_joint, theta_joint] base_link = link_from_name( robot, 'base_link') # Looks up the robot link named 'base_link' world_from_obstacle = get_pose( obstacle ) # Returns the pose of the origin of obstacle wrt the world frame obstacle_aabb = get_subtree_aabb(obstacle) draw_aabb(obstacle_aabb) random.seed(0) # Sets the random number generator state handles = [] for i in range(10): for handle in handles: remove_debug(handle) print('\nIteration: {}'.format(i)) x = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, x_joint, x) # Sets the current value of the x joint y = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, y_joint, y) # Sets the current value of the y joint yaw = random.uniform(-np.pi, np.pi) set_joint_position(robot, theta_joint, yaw) # Sets the current value of the theta joint values = get_joint_positions( robot, joints) # Obtains the current values for the specified joints print('Joint values: [x={:.3f}, y={:.3f}, yaw={:.3f}]'.format(*values)) world_from_robot = get_link_pose( robot, base_link) # Returns the pose of base_link wrt the world frame position, quaternion = world_from_robot # Decomposing pose into position and orientation (quaternion) x, y, z = position # Decomposing position into x, y, z print('Base link position: [x={:.3f}, y={:.3f}, z={:.3f}]'.format( x, y, z)) euler = euler_from_quat( quaternion) # Converting from quaternion to euler angles roll, pitch, yaw = euler # Decomposing orientation into roll, pitch, yaw print('Base link orientation: [roll={:.3f}, pitch={:.3f}, yaw={:.3f}]'. format(roll, pitch, yaw)) handles.extend( draw_pose(world_from_robot, length=0.5) ) # # Draws the base coordinate system (x:RED, y:GREEN, z:BLUE) obstacle_from_robot = multiply( invert(world_from_obstacle), world_from_robot) # Relative transformation from robot to obstacle robot_aabb = get_subtree_aabb( robot, base_link) # Computes the robot's axis-aligned bounding box (AABB) lower, upper = robot_aabb # Decomposing the AABB into the lower and upper extrema center = (lower + upper) / 2. # Computing the center of the AABB extent = upper - lower # Computing the dimensions of the AABB handles.extend(draw_aabb(robot_aabb)) collision = pairwise_collision( robot, obstacle ) # Checks whether robot is currently colliding with obstacle print('Collision: {}'.format(collision)) wait_for_duration(1.0) # Like sleep() but also updates the viewer wait_for_user() # Like raw_input() but also updates the viewer # Destroys the pybullet world disconnect()
def gen(obj_name, surface_name): obj_body = world.get_body(obj_name) surface_body = world.kitchen if surface_name in ENV_SURFACES: surface_body = world.environment_bodies[surface_name] surface_aabb = compute_surface_aabb(world, surface_name) learned_poses = load_placements(world, surface_name) if learned else [ ] # TODO: GROW_PLACEMENT yaw_range = (-np.pi, np.pi) #if world.is_real(): # center = -np.pi/4 # half_extent = np.pi / 16 # yaw_range = (center-half_extent, center+half_extent) while True: for _ in range(max_attempts): if surface_name in STOVES: surface_link = link_from_name(world.kitchen, surface_name) world_from_surface = get_link_pose(world.kitchen, surface_link) z = stable_z_on_aabb( obj_body, surface_aabb) - point_from_pose(world_from_surface)[2] yaw = random.uniform(*yaw_range) body_pose_surface = Pose(Point(z=z + z_offset), Euler(yaw=yaw)) body_pose_world = multiply(world_from_surface, body_pose_surface) elif learned: if not learned_poses: return surface_pose_world = get_surface_reference_pose( surface_body, surface_name) sampled_pose_surface = multiply( surface_pose_world, random.choice(learned_poses)) [x, y, _] = point_from_pose(sampled_pose_surface) _, _, yaw = euler_from_quat( quat_from_pose(sampled_pose_surface)) dx, dy = np.random.normal( scale=pos_scale, size=2) if pos_scale else np.zeros(2) # TODO: avoid reloading z = stable_z_on_aabb(obj_body, surface_aabb) yaw = random.uniform(*yaw_range) #yaw = wrap_angle(yaw + np.random.normal(scale=rot_scale)) quat = quat_from_euler(Euler(yaw=yaw)) body_pose_world = ([x + dx, y + dy, z + z_offset], quat) # TODO: project onto the surface else: # TODO: halton sequence # unit_generator(d, use_halton=True) body_pose_world = sample_placement_on_aabb( obj_body, surface_aabb, epsilon=z_offset, percent=2.0) if body_pose_world is None: continue # return? if visibility and not is_visible_by_camera( world, point_from_pose(body_pose_world)): continue # TODO: make sure the surface is open when doing this robust = True if robust_radius != 0.: for theta in np.linspace(0, 5 * np.pi, num=8): x, y = robust_radius * unit_from_theta(theta) delta_body = Pose(Point(x, y)) delta_world = multiply(body_pose_world, delta_body) set_pose(obj_body, delta_world) if not test_supported(world, obj_body, surface_name, collisions=collisions): robust = False break set_pose(obj_body, body_pose_world) if robust and test_supported( world, obj_body, surface_name, collisions=collisions): rp = create_relative_pose(world, obj_name, surface_name) yield (rp, ) break else: yield None
def pdddlstream_from_problem(belief, additional_init=[], fixed_base=True, **kwargs): world = belief.world # One world per state task = world.task # One task per world print(task) domain_pddl = read(get_file_path(__file__, '../pddl/domain.pddl')) # TODO: repackage stream outputs to avoid recomputation # Despite the base not moving, it could be re-estimated init_bq = belief.base_conf init_aq = belief.arm_conf init_gq = belief.gripper_conf carry_aq = world.carry_conf init_aq = carry_aq if are_confs_close(init_aq, carry_aq) else init_aq # TODO: the following doesn't work. Maybe because carry_conf is used elsewhere #carry_aq = init_aq if are_confs_close(init_aq, world.carry_conf) else world.carry_conf #calibrate_aq = init_aq if are_confs_close(init_aq, world.calibrate_conf) else world.calibrate_conf # Don't need this now that returning to old confs #open_gq = init_gq if are_confs_close(init_gq, world.open_gq) else world.open_gq #closed_gq = init_gq if are_confs_close(init_gq, world.closed_gq) else world.closed_gq open_gq = world.open_gq closed_gq = world.closed_gq constant_map = { '@world': 'world', '@gripper': 'gripper', '@stove': 'stove', '@none': None, '@rest_aq': carry_aq, #'@calibrate_aq': calibrate_aq, '@open_gq': open_gq, '@closed_gq': closed_gq, '@open': OPEN, '@closed': CLOSED, '@top': TOP_GRASP, '@side': SIDE_GRASP, '@bq0': init_bq, } top_joint = JOINT_TEMPLATE.format(TOP_DRAWER) bottom_joint = JOINT_TEMPLATE.format(BOTTOM_DRAWER) init = [ ('BConf', init_bq), ('AtBConf', init_bq), ('AConf', init_bq, carry_aq), #('RestAConf', carry_aq), #('AConf', init_bq, calibrate_aq), ( 'Stationary', ), ('AConf', init_bq, init_aq), ('AtAConf', init_aq), ('GConf', open_gq), ('GConf', closed_gq), ('Grasp', None, None), ('AtGrasp', None, None), ('Above', top_joint, bottom_joint), ('Adjacent', top_joint, bottom_joint), ('Adjacent', bottom_joint, top_joint), ('Calibrated', ), ('CanMoveBase', ), ('CanMoveArm', ), ('CanMoveGripper', ), ] + list(task.init) + list(additional_init) for action_name, cost in ACTION_COSTS.items(): function_name = '{}Cost'.format(title_from_snake(action_name)) function = (function_name, ) init.append(Equal(function, cost)) # TODO: stove state init += [('Stackable', name, surface) for name, surface in task.goal_on.items()] + \ [('Stackable', name, stove) for name, stove in product(task.goal_cooked, STOVES)] + \ [('Pressed', name) for name in belief.pressed] + \ [('Cookable', name) for name in task.goal_cooked] + \ [('Cooked', name) for name in belief.cooked] + \ [('Status', status) for status in DOOR_STATUSES] + \ [('Knob', knob) for knob in KNOBS] + \ [('Joint', knob) for knob in KNOBS] + \ [('Liquid', liquid) for _, liquid in task.init_liquid] + \ [('HasLiquid', cup, liquid) for cup, liquid in belief.liquid] + \ [('StoveKnob', STOVE_TEMPLATE.format(loc), KNOB_TEMPLATE.format(loc)) for loc in STOVE_LOCATIONS] + \ [('GraspType', ty) for ty in task.grasp_types] # TODO: grasp_type per object #[('Type', obj_name, 'stove') for obj_name in STOVES] + \ #[('Camera', name) for name in world.cameras] if task.movable_base: init.append(('MovableBase', )) if fixed_base: init.append(('InitBConf', init_bq)) if task.noisy_base: init.append(('NoisyBase', )) compute_pose_kin = get_compute_pose_kin(world) compute_angle_kin = get_compute_angle_kin(world) initial_poses = {} for joint_name, init_conf in belief.door_confs.items(): if joint_name in DRAWER_JOINTS: init.append(('Drawer', joint_name)) if joint_name in CABINET_JOINTS: init.append(('Cabinet', joint_name)) joint = joint_from_name(world.kitchen, joint_name) surface_name = surface_from_joint(joint_name) init.append(('SurfaceJoint', surface_name, joint_name)) # Relies on the fact that drawers have identical surface and link names link_name = get_link_name(world.kitchen, child_link_from_joint(joint)) #link_name = str(link_name.decode('UTF-8')) #link_name = str(link_name.encode('ascii','ignore')) for conf in { init_conf, world.open_kitchen_confs[joint], world.closed_kitchen_confs[joint] }: # TODO: return to initial poses? world_pose, = compute_angle_kin(link_name, joint_name, conf) init.extend([ ('Joint', joint_name), ('Angle', joint_name, conf), ('Obstacle', link_name), ('AngleKin', link_name, world_pose, joint_name, conf), ('WorldPose', link_name, world_pose), ]) if joint in world.kitchen_joints: init.extend([ ('Sample', world_pose), #('Value', world_pose), # comment out? ]) if conf == init_conf: initial_poses[link_name] = world_pose init.extend([ ('AtAngle', joint_name, conf), ('AtWorldPose', link_name, world_pose), ]) for surface_name in ALL_SURFACES: if surface_name in OPEN_SURFACES: init.append(('Counter', surface_name)) # Fixed surface if surface_name in DRAWERS: init.append(('Drawer', surface_name)) surface = surface_from_name(surface_name) surface_link = link_from_name(world.kitchen, surface.link) parent_joint = parent_joint_from_link(surface_link) if parent_joint not in world.kitchen_joints: # TODO: attach to world frame? world_pose = RelPose(world.kitchen, surface_link, init=True) initial_poses[surface_name] = world_pose init += [ #('RelPose', surface_name, world_pose, 'world'), ('WorldPose', surface_name, world_pose), #('AtRelPose', surface_name, world_pose, 'world'), ('AtWorldPose', surface_name, world_pose), ('Sample', world_pose), #('Value', world_pose), ] init.extend([ ('CheckNearby', surface_name), #('InitPose', world_pose), ('Localized', surface_name), ]) for grasp_type in task.grasp_types: if (surface_name in OPEN_SURFACES) or has_place_database( world.robot_name, surface_name, grasp_type): init.append(('AdmitsGraspType', surface_name, grasp_type)) if belief.grasped is None: init.extend([ ('HandEmpty', ), ('GConf', init_gq), ('AtGConf', init_gq), ]) else: obj_name = belief.grasped.body_name assert obj_name not in belief.pose_dists grasp = belief.grasped init += [ # Static #('Graspable', obj_name), ('Grasp', obj_name, grasp), ('IsGraspType', obj_name, grasp, grasp.grasp_type), # Fluent ('AtGrasp', obj_name, grasp), ('Holding', obj_name), ('Localized', obj_name), ] init.extend(('ValidGraspType', obj_name, grasp_type) for grasp_type in task.grasp_types if implies(world.is_real(), is_valid_grasp_type(obj_name, grasp_type))) for obj_name in world.movable: obj_type = type_from_name(obj_name) if obj_type in BOWLS: init.append(('Bowl', obj_name)) else: init.append( ('Obstacle', obj_name)) # TODO: hack to place within bowls if obj_type in COOKABLE: init.append(('Cookable', obj_name)) if obj_type in POURABLE: init.append(('Pourable', obj_name)) init += [ ('Entity', obj_name), ('CheckNearby', obj_name), ] + [('Stackable', obj_name, counter) for counter in set(ALL_SURFACES) & set(COUNTERS)] # TODO: track poses over time to produce estimates for obj_name, pose_dist in belief.pose_dists.items(): dist_support = pose_dist.dist.support() localized = pose_dist.is_localized() graspable = True if localized: init.append(('Localized', obj_name)) [rel_pose] = dist_support roll, pitch, yaw = euler_from_quat( quat_from_pose(rel_pose.get_reference_from_body())) if (MAX_ERROR < abs(roll)) or (MAX_ERROR < abs(pitch)): graspable = False print( '{} has an invalid orientation: roll={:.3f}, pitch={:.3f}'. format(obj_name, roll, pitch)) if graspable: #init.append(('Graspable', obj_name)) init.extend(('ValidGraspType', obj_name, grasp_type) for grasp_type in task.grasp_types if implies(world.is_real(), is_valid_grasp_type(obj_name, grasp_type))) # Could also fully decompose into points (but many samples) # Could immediately add likely points for collision checking for rel_pose in (dist_support if localized else pose_dist.decompose()): surface_name = rel_pose.support if surface_name is None: # Treats as obstacle # TODO: could temporarily add to fixed world_pose = rel_pose init += [ ('WorldPose', obj_name, world_pose), ('AtWorldPose', obj_name, world_pose), ] poses = [world_pose] #raise RuntimeError(obj_name, supporting) else: surface_pose = initial_poses[surface_name] world_pose, = compute_pose_kin(obj_name, rel_pose, surface_name, surface_pose) init += [ # Static ('RelPose', obj_name, rel_pose, surface_name), ('WorldPose', obj_name, world_pose), ('PoseKin', obj_name, world_pose, rel_pose, surface_name, surface_pose), # Fluent ('AtRelPose', obj_name, rel_pose, surface_name), ('AtWorldPose', obj_name, world_pose), ] if localized: init.append(('On', obj_name, surface_name)) poses = [rel_pose, world_pose] for pose in poses: if isinstance(pose, PoseDist): init.append(('Dist', pose)) else: init.extend([('Sample', pose)]) #, ('Value', pose)]) #for body, ty in problem.body_types: # init += [('Type', body, ty)] #bodies_from_type = get_bodies_from_type(problem) #bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(ty) else [ty] goal_formula = get_goal(belief, init) stream_pddl, stream_map = get_streams(world, teleport_base=task.teleport_base, **kwargs) print('Constants:', constant_map) print('Init:', sorted(init, key=lambda f: f[0])) print('Goal:', goal_formula) #print('Streams:', stream_map.keys()) # DEBUG return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)