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():
connect(use_gui=True)
disable_real_time()
set_default_camera()
problem = pick_problem(arm='left', grasp_type='side')
grasp_gen = get_grasp_gen(problem, collisions=False)
ik_ir_fn = get_ik_ir_gen(problem, max_attempts=100, teleport=False)
pose = Pose(problem.movable[0], support=problem.surfaces[0])
base_conf = Conf(problem.robot, get_group_joints(problem.robot, 'base'))
ik_fn = get_ik_fn(problem)
found = False
saved_world = WorldSaver()
for grasp, in grasp_gen(problem.movable[0]):
print(grasp.value)
# confs_cmds = ik_ir_fn(problem.arms[0], problem.movable[0], pose, grasp)
# for conf, cmds in confs_cmds:
# found = True
cmds = ik_fn(problem.arms[0], problem.movable[0], pose, grasp,
base_conf)
if cmds is not None:
cmds = cmds[0]
found = True
if found:
break
if not found:
raise Exception('No grasp found')
saved_world.restore()
for cmd in cmds.commands:
print(cmd)
control_commands(cmds.commands)
print('Quit?')
wait_for_user()
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 load_world():
#print(get_data_path())
#p.loadURDF("samurai.urdf", useFixedBase=True) # World
#p.loadURDF("kuka_lwr/kuka.urdf", useFixedBase=True)
#p.loadURDF("kuka_iiwa/model_free_base.urdf", useFixedBase=True)
# TODO: store internal world info here to be reloaded
robot = load_model(DRAKE_IIWA_URDF)
# robot = load_model(KUKA_IIWA_URDF)
floor = load_model('models/short_floor.urdf')
sink = load_model(SINK_URDF, pose=Pose(Point(x=-0.5)))
stove = load_model(STOVE_URDF, pose=Pose(Point(x=+0.5)))
block = load_model(BLOCK_URDF, fixed_base=False)
#cup = load_model('models/dinnerware/cup/cup_small.urdf',
# Pose(Point(x=+0.5, y=+0.5, z=0.5)), fixed_base=False)
set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
# print(get_camera())
set_default_camera()
return robot, block