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