def main(): config = parse_config('../configs/jr2_reaching.yaml') s = Simulator(mode='gui', image_width=512, image_height=512) #scene = BuildingScene('Rs', # build_graph=True, # pybullet_load_texture=True) scene = StadiumScene() s.import_scene(scene) jr = JR2_Kinova_Head(config) #turtlebot = Turtlebot(config) s.import_robot(jr) for _ in range(10): obj = YCBObject('003_cracker_box') s.import_object(obj) obj.set_position_orientation(np.random.uniform(low=0, high=2, size=3), [0, 0, 0, 1]) for i in range(10000): with Profiler('Simulator step'): jr.apply_action([0, 0, 0.1, 0.1, 0, 0, 0, 0, 0]) s.step() rgb = s.renderer.render_robot_cameras(modes=('rgb')) s.disconnect()
def main(): config_filename = os.path.join(os.path.dirname(gibson2.__file__), '../examples/configs/hand_drawer.yaml') env = HandDrawerEnv(config_file=config_filename, mode='gui') for j in range(1000): env.reset() for i in range(100): with Profiler('Environment action step'): action = env.action_space.sample() action[0:24] = 0 action[-7] = 0.05 action[-6] = 1 action[-5] = 1.05 action[-4:] = p.getQuaternionFromEuler([-np.pi / 2, 0, np.pi]) state, reward, done, info = env.step(action) # env.save_seg_image_external('seg/'+str(j*100+i)+'.jpg') # env.save_rgb_image('rgb/'+str(j*100+i)+'.jpg') # env.save_depth_image_external('depth/'+str(j*100+i)+'.jpg') # env.save_normal_image('normal/'+str(j*100+i)+'.jpg') # env.save_external_image('external/'+str(j*100+i)+'.jpg') if done: logging.info( "Episode finished after {} timesteps".format(i + 1)) break
def main(): if len(sys.argv) > 1: model_path = sys.argv[1] else: model_path = os.path.join(get_model_path('Rs'), 'mesh_z_up.obj') renderer = MeshRendererG2G(width=512, height=512, device_idx=0) renderer.load_object(model_path) renderer.add_instance(0) print(renderer.visual_objects, renderer.instances) print(renderer.materials_mapping, renderer.mesh_materials) camera_pose = np.array([0, 0, 1.2]) view_direction = np.array([1, 0, 0]) renderer.set_camera(camera_pose, camera_pose + view_direction, [0, 0, 1]) renderer.set_fov(90) for i in range(3000): with Profiler('Render'): frame = renderer.render(modes=('rgb', 'normal', '3d')) print(frame) img_np = frame[0].flip(0).data.cpu().numpy().reshape( renderer.height, renderer.width, 4) normal_np = frame[1].flip(0).data.cpu().numpy().reshape( renderer.height, renderer.width, 4) plt.imshow(np.concatenate([img_np, normal_np], axis=1)) plt.show()
def main(): config = parse_config('../configs/jr2_reaching.yaml') s = Simulator(mode='gui', image_width=512, image_height=512) scene = StadiumScene() s.import_scene(scene) jr = JR2_Kinova(config) s.import_robot(jr) marker = VisualMarker(visual_shape=p.GEOM_SPHERE, rgba_color=[0, 0, 1, 0.3], radius=0.5) s.import_object(marker) marker.set_position([0, 4, 1]) for i in range(10000): with Profiler('Simulator step'): jr.apply_action([0, 0, 0, 0, 0, 0, 0]) s.step() rgb = s.renderer.render_robot_cameras(modes=('rgb')) s.disconnect()
def main(): config_filename = os.path.join(os.path.dirname(gibson2.__file__), '../examples/configs/turtlebot_demo.yaml') nav_env = NavigateRandomEnv(config_file=config_filename, mode='gui') for j in range(10): nav_env.reset() for i in range(100): with Profiler('Env action step'): action = nav_env.action_space.sample() state, reward, done, info = nav_env.step(action) if done: print("Episode finished after {} timesteps".format(i + 1)) break
def main(): config_filename = os.path.join(os.path.dirname(gibson2.__file__), '../examples/configs/master_config.yaml') nav_env = NavigateRandomEnv(config_file=config_filename, mode='gui') for j in range(100): nav_env.reset() for i in range(1000): with Profiler('Env action step'): action = nav_env.action_space.sample() action = [1, 1, 0, 0, 0, 0, 0] state, reward, done, info = nav_env.step(action) print(state['close_to_goal']) #nav_env.set_camera([np.random.choice([0,1,2,3,4,5,6,7,8])]) if done: print("Episode finished after {} timesteps".format(i + 1)) break
def main(): config_filename = os.path.join(os.path.dirname(gibson2.__file__), '../examples/configs/jr2_reaching.yaml') nav_env = NavigateRandomHeightEnv(config_file=config_filename, mode='pbgui', random_height=True) for j in range(10): nav_env.reset() for i in range(100): with Profiler('Environment action step'): action = nav_env.action_space.sample() action = [0, 0, 1, 1, 1, 1, 1] state, reward, done, info = nav_env.step(action) if done: logging.info( "Episode finished after {} timesteps".format(i + 1)) break
def main(): config = parse_config('../configs/turtlebot_demo.yaml') s = Simulator(mode='gui', image_width=512, image_height=512) scene = BuildingScene('Rs', build_graph=True, pybullet_load_texture=True) s.import_scene(scene) turtlebot = Turtlebot(config) s.import_robot(turtlebot) for _ in range(10): obj = YCBObject('003_cracker_box') obj.load() obj.set_position_orientation(np.random.uniform(low=0, high=2, size=3), [0,0,0,1]) for i in range(10000): with Profiler('Simulator step'): turtlebot.apply_action([0.1,0.1]) s.step() rgb = s.renderer.render_robot_cameras(modes=('rgb')) s.disconnect()
import yaml from gibson2.core.physics.robot_locomotors import Turtlebot from gibson2.core.simulator import Simulator from gibson2.core.physics.scene import BuildingScene, StadiumScene from gibson2.utils.utils import parse_config import pytest import pybullet as p import numpy as np from gibson2.core.render.profiler import Profiler config = parse_config('../configs/turtlebot_p2p_nav.yaml') s = Simulator(mode='gui', resolution=512, render_to_tensor=False) scene = BuildingScene('Bolton') s.import_scene(scene) turtlebot = Turtlebot(config) s.import_robot(turtlebot) p.resetBasePositionAndOrientation(scene.ground_plane_mjcf[0], posObj=[0, 0, -10], ornObj=[0, 0, 0, 1]) for i in range(2000): with Profiler('simulator step'): turtlebot.apply_action([0.1, 0.1]) s.step() rgb = s.renderer.render_robot_cameras(modes=('rgb')) s.disconnect()
dy = (y - _mouse_iy) / 100.0 _mouse_ix = x _mouse_iy = y r1 = np.array([[np.cos(dy), 0, np.sin(dy)], [0, 1, 0], [-np.sin(dy), 0, np.cos(dy)]]) r2 = np.array([[np.cos(-dx), -np.sin(-dx), 0], [np.sin(-dx), np.cos(-dx), 0], [0, 0, 1]]) view_direction = r1.dot(r2).dot(view_direction) elif event == cv2.EVENT_LBUTTONUP: down = False cv2.namedWindow('test') cv2.setMouseCallback('test', change_dir) while True: with Profiler('Render'): frame = renderer.render(modes=('rgb', 'normal', '3d')) cv2.imshow( 'test', cv2.cvtColor(np.concatenate(frame, axis=1), cv2.COLOR_RGB2BGR)) q = cv2.waitKey(1) if q == ord('w'): px += 0.05 elif q == ord('s'): px -= 0.05 elif q == ord('a'): py += 0.05 elif q == ord('d'): py -= 0.05 elif q == ord('q'): break
def play(env, transpose=True, zoom=None, callback=None, keys_to_action=None): """Allows one to play the game using keyboard. To simply play the game use: play(gym.make("Pong-v3")) play(env) Above code works also if env is wrapped, so it's particularly useful in verifying that the frame-level preprocessing does not render the game unplayable. If you wish to plot real time statistics as you play, you can use gym.utils.play.PlayPlot. Here's a sample code for plotting the reward for last 5 second of gameplay. def callback(obs_t, obs_tp1, rew, done, info): return [rew,] env_plotter = EnvPlotter(callback, 30 * 5, ["reward"]) env = gym.make("Pong-v3") play(env, callback=env_plotter.callback) Arguments --------- env: gym.Env Environment to use for playing. transpose: bool If True the output of observation is transposed. Defaults to true. zoom: float Make screen edge this many times bigger callback: lambda or None Callback if a callback is provided it will be executed after every step. It takes the following input: obs_t: observation before performing action obs_tp1: observation after performing action action: action that was executed rew: reward that was received done: whether the environemnt is done or not info: debug info keys_to_action: dict: tuple(int) -> int or None Mapping from keys pressed to action performed. For example if pressed 'w' and space at the same time is supposed to trigger action number 2 then key_to_action dict would look like this: { # ... sorted(ord('w'), ord(' ')) -> 2 # ... } If None, default key_to_action mapping for that env is used, if provided. """ obs_s = env.observation_space #assert type(obs_s) == gym.spaces.box.Box #assert len(obs_s.shape) == 2 or (len(obs_s.shape) == 3 and obs_s.shape[2] in [1,3]) if keys_to_action is None: if hasattr(env, 'get_keys_to_action'): keys_to_action = env.get_keys_to_action() elif hasattr(env.unwrapped, 'get_keys_to_action'): keys_to_action = env.unwrapped.get_keys_to_action() relevant_keys = set(sum(map(list, keys_to_action.keys()), [])) pressed_keys = [] running = True env_done = True record_num = 0 record_total = 0 obs = env.reset() do_restart = False last_keys = [] ## Prevent overacting while running: if do_restart: do_restart = False env.reset() pressed_keys = [] continue if len(pressed_keys) == 0: action = keys_to_action[()] with Profiler("Play Env: step"): start = time.time() obs, rew, env_done, info = env.step(action) record_total += time.time() - start record_num += 1 #print(info['sensor']) print("Play mode: reward %f" % rew) for p_key in pressed_keys: action = keys_to_action[(p_key, )] prev_obs = obs with Profiler("Play Env: step"): start = time.time() obs, rew, env_done, info = env.step(action) record_total += time.time() - start record_num += 1 print("Play mode: reward %f" % rew) if callback is not None: callback(prev_obs, obs, action, rew, env_done, info) # process pygame events key_codes = env.get_key_pressed(relevant_keys) #print("Key codes", key_codes) pressed_keys = [] for key in key_codes: if key == ord('r') and key not in last_keys: do_restart = True if key == ord('j') and key not in last_keys: env.robot.turn_left() if key == ord('l') and key not in last_keys: env.robot.turn_right() if key == ord('i') and key not in last_keys: env.robot.move_forward() if key == ord('k') and key not in last_keys: env.robot.move_backward() if key not in relevant_keys: continue pressed_keys.append(key) last_keys = key_codes
def main(): global _mouse_ix, _mouse_iy, down, view_direction if len(sys.argv) > 1: model_path = sys.argv[1] else: model_path = os.path.join(get_model_path('Rs'), 'mesh_z_up.obj') renderer = MeshRenderer(width=512, height=512) renderer.load_object(model_path) renderer.add_instance(0) print(renderer.visual_objects, renderer.instances) print(renderer.materials_mapping, renderer.mesh_materials) camera_pose = np.array([0, 0, 1.2]) view_direction = np.array([1, 0, 0]) renderer.set_camera(camera_pose, camera_pose + view_direction, [0, 0, 1]) renderer.set_fov(90) px = 0 py = 0 _mouse_ix, _mouse_iy = -1, -1 down = False def change_dir(event, x, y, flags, param): global _mouse_ix, _mouse_iy, down, view_direction if event == cv2.EVENT_LBUTTONDOWN: _mouse_ix, _mouse_iy = x, y down = True if event == cv2.EVENT_MOUSEMOVE: if down: dx = (x - _mouse_ix) / 100.0 dy = (y - _mouse_iy) / 100.0 _mouse_ix = x _mouse_iy = y r1 = np.array([[np.cos(dy), 0, np.sin(dy)], [0, 1, 0], [-np.sin(dy), 0, np.cos(dy)]]) r2 = np.array([[np.cos(-dx), -np.sin(-dx), 0], [np.sin(-dx), np.cos(-dx), 0], [0, 0, 1]]) view_direction = r1.dot(r2).dot(view_direction) elif event == cv2.EVENT_LBUTTONUP: down = False cv2.namedWindow('test') cv2.setMouseCallback('test', change_dir) while True: with Profiler('Render'): frame = renderer.render(modes=('rgb', 'normal', '3d')) cv2.imshow( 'test', cv2.cvtColor(np.concatenate(frame, axis=1), cv2.COLOR_RGB2BGR)) q = cv2.waitKey(1) if q == ord('w'): px += 0.05 elif q == ord('s'): px -= 0.05 elif q == ord('a'): py += 0.05 elif q == ord('d'): py -= 0.05 elif q == ord('q'): break camera_pose = np.array([px, py, 1.2]) renderer.set_camera(camera_pose, camera_pose + view_direction, [0, 0, 1]) renderer.release()
def main(): config = parse_config('../configs/fetch_p2p_nav.yaml') s = Simulator(mode='gui', timestep=1 / 240.0) scene = EmptyScene() s.import_scene(scene) fetch = Fetch(config) s.import_robot(fetch) robot_id = fetch.robot_ids[0] arm_joints = joints_from_names(robot_id, [ 'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ]) #finger_joints = joints_from_names(robot_id, ['l_gripper_finger_joint', 'r_gripper_finger_joint']) fetch.robot_body.reset_position([0, 0, 0]) fetch.robot_body.reset_orientation([0, 0, 1, 0]) x, y, z = fetch.get_end_effector_position() #set_joint_positions(robot_id, finger_joints, [0.04,0.04]) print(x, y, z) visual_marker = p.createVisualShape(p.GEOM_SPHERE, radius=0.02) marker = p.createMultiBody(baseVisualShapeIndex=visual_marker) #max_limits = [0,0] + get_max_limits(robot_id, arm_joints) + [0.05,0.05] #min_limits = [0,0] + get_min_limits(robot_id, arm_joints) + [0,0] #rest_position = [0,0] + list(get_joint_positions(robot_id, arm_joints)) + [0.04,0.04] max_limits = [0, 0] + get_max_limits(robot_id, arm_joints) min_limits = [0, 0] + get_min_limits(robot_id, arm_joints) rest_position = [0, 0] + list(get_joint_positions(robot_id, arm_joints)) joint_range = list(np.array(max_limits) - np.array(min_limits)) joint_range = [item + 1 for item in joint_range] jd = [0.1 for item in joint_range] print(max_limits) print(min_limits) def accurateCalculateInverseKinematics(robotid, endEffectorId, targetPos, threshold, maxIter): sample_fn = get_sample_fn(robotid, arm_joints) set_joint_positions(robotid, arm_joints, sample_fn()) it = 0 while it < maxIter: jointPoses = p.calculateInverseKinematics(robotid, endEffectorId, targetPos, lowerLimits=min_limits, upperLimits=max_limits, jointRanges=joint_range, restPoses=rest_position, jointDamping=jd) set_joint_positions(robotid, arm_joints, jointPoses[2:10]) ls = p.getLinkState(robotid, endEffectorId) newPos = ls[4] dist = np.linalg.norm(np.array(targetPos) - np.array(newPos)) if dist < threshold: break it += 1 print("Num iter: " + str(it) + ", threshold: " + str(dist)) return jointPoses while True: with Profiler("Simulation step"): fetch.robot_body.reset_position([0, 0, 0]) fetch.robot_body.reset_orientation([0, 0, 1, 0]) threshold = 0.01 maxIter = 100 joint_pos = accurateCalculateInverseKinematics( robot_id, fetch.parts['gripper_link'].body_part_index, [x, y, z], threshold, maxIter)[2:10] #set_joint_positions(robot_id, finger_joints, [0.04, 0.04]) s.step() keys = p.getKeyboardEvents() for k, v in keys.items(): if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_IS_DOWN)): x += 0.01 if (k == p.B3G_LEFT_ARROW and (v & p.KEY_IS_DOWN)): x -= 0.01 if (k == p.B3G_UP_ARROW and (v & p.KEY_IS_DOWN)): y += 0.01 if (k == p.B3G_DOWN_ARROW and (v & p.KEY_IS_DOWN)): y -= 0.01 if (k == ord('z') and (v & p.KEY_IS_DOWN)): z += 0.01 if (k == ord('x') and (v & p.KEY_IS_DOWN)): z -= 0.01 p.resetBasePositionAndOrientation(marker, [x, y, z], [0, 0, 0, 1]) s.disconnect()
def main(): global _mouse_ix, _mouse_iy, down, view_direction ################################################################################### # load the scene to get traversable area (disable the whole area for performance) # ################################################################################### p.connect(p.GUI) p.setGravity(0, 0, -9.8) p.setTimeStep(1. / 240.) scene = BuildingScene('Allensville', build_graph=True, pybullet_load_texture=True) objects = scene.load() random_floor = scene.get_random_floor() p1 = scene.get_random_point_floor(random_floor)[1] p2 = scene.get_random_point_floor(random_floor)[1] shortest_path, geodesic_distance = scene.get_shortest_path( random_floor, p1[:2], p2[:2], entire_path=True) print('random point 1:', p1) print('random point 2:', p2) print('geodesic distance between p1 and p2', geodesic_distance) print('shortest path from p1 to p2:', shortest_path) p.disconnect() ################################################################################### if len(sys.argv) > 1: model_path = sys.argv[1] else: model_path = os.path.join(get_model_path('Allensville'), 'mesh_z_up.obj') # set width, height (has to be equal for panorama) renderer = MeshRenderer(width=512, height=512) renderer.load_object(model_path) renderer.add_instance(0) print(renderer.visual_objects, renderer.instances) print(renderer.materials_mapping, renderer.mesh_materials) # set camera_pose / view direction camera_pose = np.array([0, 0, 1.2]) view_direction = np.array([1, 0, 0]) renderer.set_camera(camera_pose, camera_pose + view_direction, [0, 0, 1]) renderer.set_fov(90) px = 0 py = 0 _mouse_ix, _mouse_iy = -1, -1 down = False def change_dir(event, x, y, flags, param): global _mouse_ix, _mouse_iy, down, view_direction if event == cv2.EVENT_LBUTTONDOWN: _mouse_ix, _mouse_iy = x, y down = True if event == cv2.EVENT_MOUSEMOVE: if down: dx = (x - _mouse_ix) / 100.0 dy = (y - _mouse_iy) / 100.0 _mouse_ix = x _mouse_iy = y r1 = np.array([[np.cos(dy), 0, np.sin(dy)], [0, 1, 0], [-np.sin(dy), 0, np.cos(dy)]]) r2 = np.array([[np.cos(-dx), -np.sin(-dx), 0], [np.sin(-dx), np.cos(-dx), 0], [0, 0, 1]]) view_direction = r1.dot(r2).dot(view_direction) elif event == cv2.EVENT_LBUTTONUP: down = False cv2.namedWindow('test') cv2.setMouseCallback('test', change_dir) while True: with Profiler('Render'): frame = renderer.render(modes=('rgb', 'normal', '3d')) cv2.imshow( 'test', cv2.cvtColor(np.concatenate(frame, axis=1), cv2.COLOR_RGB2BGR)) q = cv2.waitKey(1) if q == ord('w'): px += 0.05 elif q == ord('s'): px -= 0.05 elif q == ord('a'): py += 0.05 elif q == ord('d'): py -= 0.05 elif q == ord('q'): break camera_pose = np.array([px, py, 1.2]) renderer.set_camera(camera_pose, camera_pose + view_direction, [0, 0, 1]) renderer.release()
obj.set_position([-2, 0, 0.5]) obj = InteractiveObj( filename='/data4/mdv0/cabinet/0007/part_objs/cabinet_0007.urdf') s.import_interactive_object(obj) obj.set_position([-2, 2, 0.5]) obj = InteractiveObj( filename='/data4/mdv0/cabinet/0004/part_objs/cabinet_0004.urdf') s.import_interactive_object(obj) obj.set_position([-2.1, 1.6, 2]) obj = InteractiveObj( filename='/data4/mdv0/cabinet/0004/part_objs/cabinet_0004.urdf') s.import_interactive_object(obj) obj.set_position([-2.1, 0.4, 2]) obj = BoxShape([-2.05, 1, 0.5], [0.35, 0.6, 0.5]) s.import_interactive_object(obj) obj = BoxShape([-2.45, 1, 1.5], [0.01, 2, 1.5]) s.import_interactive_object(obj) p.createConstraint(0, -1, obj.body_id, -1, p.JOINT_FIXED, [0, 0, 1], [-2.55, 1, 1.5], [0, 0, 0]) obj = YCBObject('003_cracker_box') s.import_object(obj) p.resetBasePositionAndOrientation(obj.body_id, [-2, 1, 1.2], [0, 0, 0, 1]) obj = YCBObject('003_cracker_box') s.import_object(obj) p.resetBasePositionAndOrientation(obj.body_id, [-2, 2, 1.2], [0, 0, 0, 1]) for i in range(1000): with Profiler("Simulation: step"): s.step() s.disconnect()