def main(display='execute'): # control | execute | step
connect(use_gui=True)
disable_real_time()
draw_global_system()
with HideOutput():
robot = load_model(DRAKE_IIWA_URDF) # KUKA_IIWA_URDF | DRAKE_IIWA_URDF
floor = load_model('models/short_floor.urdf')
block = load_model(BLOCK_URDF, fixed_base=False)
set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
set_default_camera(distance=2)
dump_world()
saved_world = WorldSaver()
command = plan(robot, block, fixed=[floor], teleport=False)
if (command is None) or (display is None):
print('Unable to find a plan!')
return
saved_world.restore()
update_state()
wait_if_gui('{}?'.format(display))
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.005)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_if_gui()
disconnect()
def main(display='control'): # control | execute | step
connect(use_gui=True)
disable_real_time()
# KUKA_IIWA_URDF | DRAKE_IIWA_URDF
robot = load_model(DRAKE_IIWA_URDF, fixed_base=True)
# floor = load_model('models/short_floor.urdf')
floor = p.loadURDF("plane.urdf")
block = load_model(
"models/drake/objects/block_for_pick_and_place_heavy.urdf", fixed_base=False)
set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
set_default_camera()
dump_world()
saved_world = WorldSaver()
command = plan(robot, block, fixed=[floor], teleport=False)
if (command is None) or (display is None):
print('Unable to find a plan!')
return
saved_world.restore()
update_state()
user_input('{}?'.format(display))
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.005)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_for_user()
disconnect()
def main(execute='apply'):
#parser = argparse.ArgumentParser() # Automatically includes help
#parser.add_argument('-display', action='store_true', help='enable viewer.')
#args = parser.parse_args()
#display = args.display
display = True
#display = False
#filename = 'transporter.json' # simple | simple2 | cook | invert | kitchen | nonmonotonic_4_1
#problem_fn = lambda: load_json_problem(filename)
problem_fn = cooking_problem
# holding_problem | stacking_problem | cleaning_problem | cooking_problem
# cleaning_button_problem | cooking_button_problem
with HideOutput():
sim_world = connect(use_gui=False)
set_client(sim_world)
add_data_path()
problem = problem_fn()
print(problem)
#state_id = save_state()
if display:
with HideOutput():
real_world = connect(use_gui=True)
add_data_path()
with ClientSaver(real_world):
problem_fn() # TODO: way of doing this without reloading?
update_state()
wait_for_duration(0.1)
#wait_for_interrupt()
commands = plan_commands(problem)
if (commands is None) or not display:
with HideOutput():
disconnect()
return
time_step = 0.01
set_client(real_world)
if execute == 'control':
enable_gravity()
control_commands(commands)
elif execute == 'execute':
step_commands(commands, time_step=time_step)
elif execute == 'step':
step_commands(commands)
elif execute == 'apply':
state = State()
apply_commands(state, commands, time_step=time_step)
else:
raise ValueError(execute)
wait_for_interrupt()
with HideOutput():
disconnect()
def main(display='execute'): # control | execute | step
# One of the arm's gantry carriage is fixed when the other is moving.
connect(use_gui=True)
set_camera(yaw=-90, pitch=-40, distance=10, target_position=(0, 7.5, 0))
draw_pose(unit_pose(), length=1.0)
disable_real_time()
with HideOutput():
root_directory = os.path.dirname(os.path.abspath(__file__))
robot = load_pybullet(os.path.join(root_directory, ETH_RFL_URDF), fixed_base=True)
# floor = load_model('models/short_floor.urdf')
block = load_model(BLOCK_URDF, fixed_base=False)
#link = link_from_name(robot, 'gantry_base_link')
#print(get_aabb(robot, link))
block_x = -0.2
#block_y = 1 if ARM == 'right' else 13.5
#block_x = 10
block_y = 5.
# set_pose(floor, Pose(Point(x=floor_x, y=1, z=1.3)))
# set_pose(block, Pose(Point(x=floor_x, y=0.6, z=stable_z(block, floor))))
set_pose(block, Pose(Point(x=block_x, y=block_y, z=3.5)))
# set_default_camera()
dump_world()
#print(get_camera())
saved_world = WorldSaver()
with LockRenderer():
command = plan(robot, block, fixed=[], teleport=False) # fixed=[floor],
if (command is None) or (display is None):
print('Unable to find a plan! Quit?')
wait_for_interrupt()
disconnect()
return
saved_world.restore()
update_state()
print('{}?'.format(display))
wait_for_interrupt()
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.002)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_for_interrupt()
disconnect()
def clone_real_world_test(problem_fn):
real_world = connect(use_gui=True)
add_data_path()
# world_file = 'test_world.py'
# p.saveWorld(world_file) # Saves a Python file to be executed
# state_id = p.saveState()
# test_bullet = 'test_world.bullet'
# save_bullet(test_bullet)
# clone_world(real_world)
with ClientSaver(real_world):
# pass
# restore_bullet(test_bullet)
problem_fn() # TODO: way of doing this without reloading?
update_state()
def main(display='execute'): # control | execute | step
connect(use_gui=True)
disable_real_time()
with HideOutput():
root_directory = os.path.dirname(os.path.abspath(__file__))
robot = load_pybullet(os.path.join(root_directory, IRB6600_TRACK_URDF), fixed_base=True)
floor = load_model('models/short_floor.urdf')
block = load_model(BLOCK_URDF, fixed_base=False)
floor_x = 2
set_pose(floor, Pose(Point(x=floor_x, z=0.5)))
set_pose(block, Pose(Point(x=floor_x, y=0, z=stable_z(block, floor))))
# set_default_camera()
dump_world()
saved_world = WorldSaver()
with LockRenderer():
command = plan(robot, block, fixed=[floor], teleport=False)
if (command is None) or (display is None):
print('Unable to find a plan!')
print('Quit?')
wait_for_interrupt()
disconnect()
return
saved_world.restore()
update_state()
user_input('{}?'.format(display))
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.002)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_for_interrupt()
disconnect()