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 test_arm_control(pr2, left_joints, arm_start): wait_for_user('Control Arm?') real_time = False enable_gravity() p.setRealTimeSimulation(real_time) for _ in joint_controller(pr2, left_joints, arm_start): if not real_time: p.stepSimulation()
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 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()
def main(): # https://github.com/ros-teleop/teleop_twist_keyboard # http://openrave.org/docs/latest_stable/_modules/openravepy/misc/#SetViewerUserThread connect(use_gui=True) add_data_path() load_pybullet("plane.urdf") #load_pybullet("models/table_collision/table.urdf") with HideOutput(): pr2 = load_model(DRAKE_PR2_URDF, fixed_base=True) enable_gravity() enable_real_time( ) # TODO: won't work as well on OS X due to simulation thread #run_simulate(pr2) run_thread(pr2) # TODO: keep working on this #userthread = threading.Thread(target=run_thread, args=[pr2]) #userthread.start() #userthread.join() disconnect()
def simulate_plan(plan, time_step=0.0, real_time=False): #time_step=np.inf wait_for_interrupt() enable_gravity() if real_time: enable_real_time() for action, args in plan: trajectory = args[-1] if action == 'move': simulate_trajectory(trajectory, time_step) elif action == 'pick': attachment = trajectory.attachments.pop() simulate_trajectory(trajectory, time_step) add_fixed_constraint(attachment.child, attachment.parent, attachment.parent_link) simulate_trajectory(trajectory.reverse(), time_step) elif action == 'place': ttachment = trajectory.attachments.pop() simulate_trajectory(trajectory, time_step) remove_fixed_constraint(attachment.child, attachment.parent, attachment.parent_link) simulate_trajectory(trajectory.reverse(), time_step) else: raise NotImplementedError(action) wait_for_interrupt()
def simulate(controller=None, max_duration=INF, max_steps=INF, print_rate=1., sleep=None): enable_gravity() dt = get_time_step() print('Time step: {:.6f} sec'.format(dt)) start_time = last_print = time.time() for step in irange(max_steps): duration = step * dt if duration >= max_duration: break if (controller is not None) and is_empty(controller): break step_simulation() synchronize_viewer() if elapsed_time(last_print) >= print_rate: print( 'Sim step: {} | Sim time: {:.3f} sec | Elapsed time: {:.3f} sec' .format(step, duration, elapsed_time(start_time))) last_print = time.time() if sleep is not None: time.sleep(sleep)
def set_gravity(self): with ClientSaver(self.client): enable_gravity()
def main(): connect(use_gui=True) add_data_path() set_camera(0, -30, 1) plane = load_pybullet('plane.urdf', fixed_base=True) #plane = load_model('plane.urdf') cup = load_model('models/cup.urdf', fixed_base=True) #set_point(cup, Point(z=stable_z(cup, plane))) set_point(cup, Point(z=.2)) set_color(cup, (1, 0, 0, .4)) num_droplets = 100 #radius = 0.025 #radius = 0.005 radius = 0.0025 # TODO: more efficient ways to make all of these droplets = [create_sphere(radius, mass=0.01) for _ in range(num_droplets)] # kg cup_thickness = 0.001 lower, upper = get_lower_upper(cup) print(lower, upper) buffer = cup_thickness + radius lower = np.array(lower) + buffer * np.ones(len(lower)) upper = np.array(upper) - buffer * np.ones(len(upper)) limits = zip(lower, upper) x_range, y_range = limits[:2] z = upper[2] + 0.1 #x_range = [-1, 1] #y_range = [-1, 1] #z = 1 for droplet in droplets: x = np.random.uniform(*x_range) y = np.random.uniform(*y_range) set_point(droplet, Point(x, y, z)) for i, droplet in enumerate(droplets): x, y = np.random.normal(0, 1e-3, 2) set_point(droplet, Point(x, y, z + i * (2 * radius + 1e-3))) #dump_world() wait_for_user() #user_input('Start?') enable_gravity() simulate_for_duration(5.0) # enable_real_time() # try: # while True: # enable_gravity() # enable_real_time requires a command # #time.sleep(dt) # except KeyboardInterrupt: # pass # print() #time.sleep(1.0) wait_for_user('Finish?') disconnect()
def close_gripper_test(problem): joints = joints_from_names(problem.robot, PR2_GROUPS['left_gripper']) values = [get_min_limit(problem.robot, joint) for joint in joints] for _ in joint_controller_hold(problem.robot, joints, values): enable_gravity() step_simulation()