def cook_block(world, fixed=False, **kwargs):
add_kinect(world) # previously needed to be after set_all_static?
if fixed:
set_fixed_base(world)
entity_name = add_block(world, idx=0, pose2d=BOX_POSE2D)
set_all_static()
initial_surface = 'indigo_tmp'
sample_placement(world, entity_name, initial_surface, learned=True)
prior = {
entity_name: UniformDist([initial_surface]),
}
return Task(
world,
prior=prior,
movable_base=not fixed,
grasp_types=[TOP_GRASP],
#goal_detected=[entity_name],
#goal_holding=entity_name,
goal_cooked=[entity_name],
#goal_on={entity_name: goal_surface},
return_init_bq=True,
return_init_aq=True,
#goal_open=[joint_name],
#goal_closed=ALL_JOINTS,
)
def sugar_drawer(world, fixed=False, **kwargs):
add_kinect(world)
if fixed:
set_fixed_base(world)
open_drawer = random.choice(ZED_DRAWERS)
world.open_door(
joint_from_name(world.kitchen, JOINT_TEMPLATE.format(open_drawer)))
# open_all_doors(world)
prior = {}
block_name = add_block(world, idx=0, pose2d=BOX_POSE2D)
prior[block_name] = DeltaDist('indigo_tmp')
sugar_name = add_sugar_box(world, idx=0)
prior[sugar_name] = DeltaDist(open_drawer)
sample_placement(world, sugar_name, open_drawer, learned=True)
set_all_static()
return Task(
world,
prior=prior,
movable_base=not fixed,
grasp_types=[TOP_GRASP],
goal_on={block_name: open_drawer},
return_init_bq=True,
return_init_aq=True,
#goal_open=[JOINT_TEMPLATE.format(open_drawer)],
goal_closed=ALL_JOINTS,
)
def regrasp_block(world, fixed=False, **kwargs):
add_kinect(world)
if fixed:
set_fixed_base(world)
entity_name = add_block(world, idx=0)
set_all_static()
#world.open_door(joint_from_name(world.kitchen, JOINT_TEMPLATE.format(LEFT_DOOR)))
#drawer = random.choice(ZED_DRAWERS)
drawer = 'indigo_tmp'
sample_placement(world, entity_name, drawer, learned=True)
prior = {
entity_name: UniformDist(drawer),
}
return Task(
world,
prior=prior,
movable_base=not fixed,
#grasp_types=[SIDE_GRASP], #, TOP_GRASP],
#goal_holding=entity_name,
goal_on={entity_name: LEFT_DOOR},
#return_init_bq=True, return_init_aq=True,
#goal_open=[JOINT_TEMPLATE.format(LEFT_DOOR)]
#goal_closed=ALL_JOINTS,
)
def _update_initial(self):
# TODO: store initial poses as well?
self.initial_saver = WorldSaver()
self.goal_bq = FConf(self.robot, self.base_joints)
self.goal_aq = FConf(self.robot, self.arm_joints)
if are_confs_close(self.goal_aq, self.carry_conf):
self.goal_aq = self.carry_conf
self.goal_gq = FConf(self.robot, self.gripper_joints)
self.initial_confs = [self.goal_bq, self.goal_aq, self.goal_gq]
set_all_static()
def stow_block(world, num=1, fixed=False, **kwargs):
add_kinect(world) # previously needed to be after set_all_static?
if fixed:
set_fixed_base(world)
# initial_surface = random.choice(DRAWERS) # COUNTERS | DRAWERS | SURFACES | CABINETS
initial_surface = 'indigo_tmp' # hitman_tmp | indigo_tmp | range | front_right_stove
# initial_surface = 'indigo_drawer_top'
goal_surface = 'indigo_drawer_top' # baker | hitman_drawer_top | indigo_drawer_top | hitman_tmp | indigo_tmp
print('Initial surface: | Goal surface: ', initial_surface,
initial_surface)
prior = {}
goal_on = {}
for idx in range(num):
entity_name = add_block(world, idx=idx, pose2d=SPAM_POSE2D)
prior[entity_name] = DeltaDist(initial_surface)
goal_on[entity_name] = goal_surface
if not fixed:
sample_placement(world, entity_name, initial_surface, learned=True)
#obstruction_name = add_box(world, idx=0)
#sample_placement(world, obstruction_name, 'hitman_tmp')
set_all_static()
# dump_world()
# open_names = [
# 'chewie_door_right_joint',
# 'dagger_door_right_joint',
# 'hitman_drawer_bottom_joint',
# # 'indigo_drawer_top_joint',
# ]
# for joint_name in open_names:
# world.open_door(joint_from_name(world.kitchen, joint_name))
return Task(
world,
prior=prior,
movable_base=not fixed,
grasp_types=[TOP_GRASP],
#goal_holding=list(prior)[0],
goal_hand_empty=True,
goal_on=goal_on,
#goal_cooked=list(prior),
return_init_bq=True,
return_init_aq=True,
#goal_open=[joint_name],
goal_closed=ALL_JOINTS)
def detect_block(world, fixed=False, **kwargs):
add_kinect(world)
#for side in CAMERAS[:1]:
# add_kinect(world, side)
#add_kinects(world)
if fixed:
set_fixed_base(world)
#plane = create_plane([1, 0, 0])
#set_point(plane, [MIN_PLACEMENT_X, 0, 0])
#wait_for_user()
block_poses = [(0.1, 1.05, 0.), (0.1, 1.3, 0.)]
entity_name = add_block(world, idx=0, pose2d=random.choice(block_poses))
sugar_name = add_sugar_box(world, idx=0, pose2d=(0.2, 1.35, np.pi / 4))
cracker_name = add_cracker_box(world, idx=1, pose2d=(0.2, 1.1, np.pi / 4))
#other_name = add_box(world, idx=1)
set_all_static()
#goal_surface = 'indigo_drawer_top'
#initial_distribution = UniformDist([goal_surface]) # indigo_tmp
#initial_surface = initial_distribution.sample()
#if random.random() < 0.0:
# # TODO: sometimes base/arm failure causes the planner to freeze
# # Freezing is because the planner is struggling to find new samples
# sample_placement(world, entity_name, initial_surface, learned=True)
#sample_placement(world, other_name, 'hitman_tmp', learned=True)
prior = {
entity_name:
UniformDist(['indigo_tmp']), # 'indigo_tmp', 'indigo_drawer_top'
sugar_name: DeltaDist('indigo_tmp'),
cracker_name: DeltaDist('indigo_tmp'),
}
return Task(
world,
prior=prior,
movable_base=not fixed,
grasp_types=[TOP_GRASP],
return_init_bq=True,
return_init_aq=True,
#goal_detected=[entity_name],
goal_holding=cracker_name,
#goal_on={entity_name: random.choice(ZED_DRAWERS)},
goal_cooked=[entity_name],
goal_closed=ALL_JOINTS,
)
def cook_meal(world, fixed=False, **kwargs):
add_kinect(world) # previously needed to be after set_all_static?
if fixed:
set_fixed_base(world)
prior = {}
soup_name = add_ycb(world,
'tomato_soup_can',
pose2d=[0.1, 0.9, +np.pi / 8])
prior[soup_name] = DeltaDist('indigo_tmp')
if not fixed:
sample_placement(world, soup_name, 'indigo_tmp', learned=True)
mustard_name = add_ycb(world,
'mustard_bottle',
pose2d=[0.25, 1.2, -np.pi / 8])
prior[mustard_name] = DeltaDist('indigo_tmp')
if not fixed:
sample_placement(world, mustard_name, 'indigo_tmp', learned=True)
stove = STOVES[-1]
bowl_name = add_ycb(world, 'bowl')
prior[bowl_name] = DeltaDist(stove)
sample_placement(world, bowl_name, stove, learned=True)
#print(get_pose(world.get_body(bowl_name)))
set_all_static()
return Task(
world,
prior=prior,
movable_base=not fixed,
#grasp_types=[TOP_GRASP],
init_liquid=[(soup_name, 'tomato'), (mustard_name, 'mustard')],
#goal_liquid=[(bowl_name, 'tomato'), (bowl_name, 'mustard')],
#goal_holding=soup_name,
goal_hand_empty=True,
#goal_cooked=[bowl_name],
goal_cooked=['tomato', 'mustard'],
return_init_bq=True,
return_init_aq=True,
#goal_open=[joint_name],
goal_closed=ALL_JOINTS)
def swap_drawers(world, fixed=False, **kwargs):
# Starts in the incorrect drawer
add_kinect(world) # previously needed to be after set_all_static?
if fixed:
set_fixed_base(world)
entity_name = add_block(world, idx=0, pose2d=BOX_POSE2D)
set_all_static()
#open_all_doors(world)
#initial_surface, goal_surface = 'indigo_tmp', 'indigo_drawer_top'
#initial_surface, goal_surface = 'indigo_drawer_top', 'indigo_drawer_top'
#initial_surface, goal_surface = 'indigo_drawer_bottom', 'indigo_drawer_bottom'
#initial_surface, goal_surface = ZED_DRAWERS
#initial_surface, goal_surface = reversed(ZED_DRAWERS)
initial_surface, goal_surface = randomize(ZED_DRAWERS)
if initial_surface != 'indigo_tmp':
sample_placement(world, entity_name, initial_surface, learned=True)
#joint_name = JOINT_TEMPLATE.format(goal_surface)
#world.open_door(joint_from_name(world.kitchen, JOINT_TEMPLATE.format(goal_surface)))
# TODO: declare success if already believe it's in the drawer or require detection?
prior = {
#entity_name: UniformDist([initial_surface]),
entity_name: UniformDist(ZED_DRAWERS),
#entity_name: UniformDist(['indigo_tmp', 'indigo_drawer_top', 'indigo_drawer_bottom']),
}
return Task(
world,
prior=prior,
movable_base=not fixed,
grasp_types=[TOP_GRASP],
#goal_detected=[entity_name],
#goal_holding=entity_name,
#goal_cooked=[entity_name],
goal_on={entity_name: goal_surface},
return_init_bq=True,
return_init_aq=True,
#goal_open=[joint_name],
#goal_closed=[JOINT_TEMPLATE.format(initial_surface)],
#goal_closed=[JOINT_TEMPLATE.format(goal_surface)], # TODO: this caused non-fixed base planning to fail due to cost
goal_closed=ALL_JOINTS,
)
def inspect_drawer(world, fixed=False, **kwargs):
# Starts in the correct drawer
add_kinect(world)
if fixed:
set_fixed_base(world)
entity_name = add_block(world, idx=0)
set_all_static()
drawer = random.choice(ZED_DRAWERS)
sample_placement(world, entity_name, drawer, learned=True)
prior = {
entity_name: UniformDist(ZED_DRAWERS),
}
return Task(
world,
prior=prior,
movable_base=not fixed,
grasp_types=[TOP_GRASP],
goal_on={entity_name: drawer},
return_init_bq=True,
return_init_aq=True,
goal_closed=ALL_JOINTS,
)
def hold_block(world, num=5, fixed=False, **kwargs):
add_kinect(world)
if fixed:
set_fixed_base(world)
# TODO: compare with the NN grasp prediction in clutter
# TODO: consider a task where most directions are blocked except for one
initial_surface = 'indigo_tmp'
# initial_surface = 'dagger_door_left'
# joint_name = JOINT_TEMPLATE.format(initial_surface)
#world.open_door(joint_from_name(world.kitchen, joint_name))
#open_all_doors(world)
prior = {}
# green_name = add_block(world, idx=0, pose2d=BOX_POSE2D)
green_name = add_box(world, 'green', idx=0)
prior[green_name] = DeltaDist(initial_surface)
sample_placement(world, green_name, initial_surface, learned=True)
for idx in range(num):
red_name = add_box(world, 'red', idx=idx)
prior[red_name] = DeltaDist(initial_surface)
sample_placement(world, red_name, initial_surface, learned=True)
set_all_static()
return Task(
world,
prior=prior,
movable_base=not fixed,
# grasp_types=GRASP_TYPES,
grasp_types=[SIDE_GRASP],
return_init_bq=True,
return_init_aq=True,
goal_holding=green_name,
#goal_closed=ALL_JOINTS,
)
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 main():
parser = argparse.ArgumentParser()
parser.add_argument('-c', '--cfree', action='store_true',
help='When enabled, disables collision checking.')
# parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name),
# help='The name of the problem to solve.')
parser.add_argument('--holonomic', action='store_true', # '-h',
help='')
parser.add_argument('-l', '--lock', action='store_false',
help='')
parser.add_argument('-s', '--seed', default=None, type=int,
help='The random seed to use.')
parser.add_argument('-v', '--viewer', action='store_false',
help='')
args = parser.parse_args()
connect(use_gui=args.viewer)
seed = args.seed
#seed = 0
#seed = time.time()
set_random_seed(seed=seed) # None: 2147483648 = 2**31
set_numpy_seed(seed=seed)
print('Random seed:', get_random_seed(), random.random())
print('Numpy seed:', get_numpy_seed(), np.random.random())
#########################
robot, base_limits, goal_conf, obstacles = problem1()
draw_base_limits(base_limits)
custom_limits = create_custom_base_limits(robot, base_limits)
base_joints = joints_from_names(robot, BASE_JOINTS)
base_link = link_from_name(robot, BASE_LINK_NAME)
if args.cfree:
obstacles = []
# for obstacle in obstacles:
# draw_aabb(get_aabb(obstacle)) # Updates automatically
resolutions = None
#resolutions = np.array([0.05, 0.05, math.radians(10)])
set_all_static() # Doesn't seem to affect
region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3))
draw_aabb(region_aabb)
step_simulation() # Need to call before get_bodies_in_region
#update_scene() # TODO: https://github.com/bulletphysics/bullet3/pull/3331
bodies = get_bodies_in_region(region_aabb)
print(len(bodies), bodies)
# https://github.com/bulletphysics/bullet3/search?q=broadphase
# https://github.com/bulletphysics/bullet3/search?p=1&q=getCachedOverlappingObjects&type=&utf8=%E2%9C%93
# https://andysomogyi.github.io/mechanica/bullet.html
# http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual
#draw_pose(get_link_pose(robot, base_link), length=0.5)
for conf in [get_joint_positions(robot, base_joints), goal_conf]:
draw_pose(pose_from_pose2d(conf, z=DRAW_Z), length=DRAW_LENGTH)
aabb = get_aabb(robot)
#aabb = get_subtree_aabb(robot, base_link)
draw_aabb(aabb)
for link in [BASE_LINK, base_link]:
print(link, get_collision_data(robot, link), pairwise_link_collision(robot, link, robot, link))
#########################
saver = WorldSaver()
start_time = time.time()
profiler = Profiler(field='tottime', num=50) # tottime | cumtime | None
profiler.save()
with LockRenderer(lock=args.lock):
path = plan_motion(robot, base_joints, goal_conf, holonomic=args.holonomic, obstacles=obstacles,
custom_limits=custom_limits, resolutions=resolutions,
use_aabb=True, cache=True, max_distance=0.,
restarts=2, iterations=20, smooth=20) # 20 | None
saver.restore()
#wait_for_duration(duration=1e-3)
profiler.restore()
#########################
solved = path is not None
length = INF if path is None else len(path)
cost = sum(get_distance_fn(robot, base_joints, weights=resolutions)(*pair) for pair in get_pairs(path))
print('Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format(
solved, length, cost, elapsed_time(start_time)))
if path is None:
disconnect()
return
iterate_path(robot, base_joints, path)
disconnect()
