def test_jr2(): s = Simulator(mode='gui') try: scene = BuildingScene('Ohoopee') s.import_scene(scene) jr2 = JR2_Kinova(config) s.import_robot(jr2) jr2.set_position([-6, 0, 0.1]) obj3 = InteractiveObj(os.path.join(gibson2.assets_path, 'models', 'scene_components', 'door.urdf'), scale=2) s.import_interactive_object(obj3) obj3.set_position_rotation( [-5, -1, 0], [0, 0, np.sqrt(0.5), np.sqrt(0.5)]) jr2.apply_action([0.005, 0.005, 0, 0, 0, 0, 0, 0, 0, 0]) for _ in range(400): s.step() jr2.apply_action([ 0, 0, 0, 0, 3.1408197119196117, -1.37402907967774, -0.8377005721485424, -1.9804208517373096, 0.09322135043256494, 2.62937740156038 ]) for _ in range(400): s.step() finally: s.disconnect()
def load(self): if self.config['scene'] == 'stadium': scene = StadiumScene() elif self.config['scene'] == 'building': scene = BuildingScene(self.config['model_id']) self.simulator.import_scene(scene) if self.config['robot'] == 'Turtlebot': robot = Turtlebot(self.config) elif self.config['robot'] == 'Husky': robot = Husky(self.config) elif self.config['robot'] == 'Ant': robot = Ant(self.config) elif self.config['robot'] == 'Humanoid': robot = Humanoid(self.config) elif self.config['robot'] == 'JR2': robot = JR2(self.config) elif self.config['robot'] == 'JR2_Kinova': robot = JR2_Kinova(self.config) else: raise Exception('unknown robot type: {}'.format( self.config['robot'])) self.scene = scene self.robots = [robot] for robot in self.robots: self.simulator.import_robot(robot)
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 load(self): """ Load the scene and robot """ if self.config['scene'] == 'empty': scene = EmptyScene() elif self.config['scene'] == 'stadium': scene = StadiumScene() elif self.config['scene'] == 'building': scene = BuildingScene( self.config['model_id'], waypoint_resolution=self.config.get('waypoint_resolution', 0.2), num_waypoints=self.config.get('num_waypoints', 10), build_graph=self.config.get('build_graph', False), trav_map_resolution=self.config.get('trav_map_resolution', 0.1), trav_map_erosion=self.config.get('trav_map_erosion', 2), is_interactive=self.config.get('is_interactive', False), pybullet_load_texture=self.config.get('pybullet_load_texture', False), ) self.simulator.import_scene(scene, load_texture=self.config.get( 'load_texture', True)) if self.config['robot'] == 'Turtlebot': robot = Turtlebot(self.config) elif self.config['robot'] == 'Husky': robot = Husky(self.config) elif self.config['robot'] == 'Ant': robot = Ant(self.config) elif self.config['robot'] == 'Humanoid': robot = Humanoid(self.config) elif self.config['robot'] == 'JR2': robot = JR2(self.config) elif self.config['robot'] == 'JR2_Kinova': robot = JR2_Kinova(self.config) elif self.config['robot'] == 'Freight': robot = Freight(self.config) elif self.config['robot'] == 'Fetch': robot = Fetch(self.config) elif self.config['robot'] == 'Locobot': robot = Locobot(self.config) elif self.config['robot'] == 'DexHandRobot': robot = DexHandRobot(self.config) else: raise Exception('unknown robot type: {}'.format( self.config['robot'])) self.scene = scene self.robots = [robot] for robot in self.robots: self.simulator.import_robot(robot)
def main(): p.connect(p.GUI) p.setGravity(0, 0, -9.8) p.setTimeStep(1. / 240.) #floor = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml") #p.loadMJCF(floor) scene = StadiumScene() scene.load() robots = [] #config = parse_config('../configs/fetch_p2p_nav.yaml') #fetch = Fetch(config) #robots.append(fetch) config = parse_config('../configs/jr_p2p_nav.yaml') jr = JR2_Kinova(config) robots.append(jr) #config = parse_config('../configs/locobot_p2p_nav.yaml') #locobot = Locobot(config) #robots.append(locobot) #config = parse_config('../configs/turtlebot_p2p_nav.yaml') #turtlebot = Turtlebot(config) #robots.append(turtlebot) positions = [[0, 0, 0] #[1, 0, 0], #[0, 1, 0], #[1, 1, 0] ] for robot, position in zip(robots, positions): robot.load() robot.set_position(position) robot.robot_specific_reset() robot.keep_still() for _ in range(2400): # keep still for 10 seconds p.stepSimulation() time.sleep(1. / 240.) for _ in range(2400): # move with small random actions for 10 seconds for robot, position in zip(robots, positions): action = np.random.uniform(-1, 1, robot.action_dim) #robot.apply_action(action) robot.apply_action([0, 0, 0, 0, 0, 0, 0]) p.stepSimulation() time.sleep(1. / 240.0) p.disconnect()
def load(self): """ Load the scene and robot """ if self.config['scene'] == 'stadium': scene = StadiumScene() elif self.config['scene'] == 'building': scene = BuildingScene( self.config['model_id'], build_graph=self.config.get('build_graph', False), trav_map_erosion=self.config.get('trav_map_erosion', 2), should_load_replaced_objects=self.config.get( 'should_load_replaced_objects', False)) # scene: class_id = 0 # robot: class_id = 1 # objects: class_id > 1 self.simulator.import_scene(scene, load_texture=self.config.get( 'load_texture', True), class_id=0) if self.config['robot'] == 'Turtlebot': robot = Turtlebot(self.config) elif self.config['robot'] == 'Husky': robot = Husky(self.config) elif self.config['robot'] == 'Ant': robot = Ant(self.config) elif self.config['robot'] == 'Humanoid': robot = Humanoid(self.config) elif self.config['robot'] == 'JR2': robot = JR2(self.config) elif self.config['robot'] == 'JR2_Kinova': robot = JR2_Kinova(self.config) elif self.config['robot'] == 'Freight': robot = Freight(self.config) elif self.config['robot'] == 'Fetch': robot = Fetch(self.config) else: raise Exception('unknown robot type: {}'.format( self.config['robot'])) self.scene = scene self.robots = [robot] for robot in self.robots: self.simulator.import_robot(robot, class_id=1)
def show_action_sensor_space(): s = Simulator(mode='headless') scene = StadiumScene() s.import_scene(scene) turtlebot = Turtlebot(config) s.import_robot(turtlebot) turtlebot.set_position([0, 1, 0.5]) ant = Ant(config) s.import_robot(ant) ant.set_position([0, 2, 0.5]) h = Humanoid(config) s.import_robot(h) h.set_position([0, 3, 2]) jr = JR2(config) s.import_robot(jr) jr.set_position([0, 4, 0.5]) jr2 = JR2_Kinova(config) s.import_robot(jr2) jr2.set_position([0, 5, 0.5]) husky = Husky(config) s.import_robot(husky) husky.set_position([0, 6, 0.5]) quad = Quadrotor(config) s.import_robot(quad) quad.set_position([0, 7, 0.5]) for robot in s.robots: print(type(robot), len(robot.ordered_joints), robot.calc_state().shape) for i in range(100): s.step() s.disconnect()
from gibson2.core.physics.robot_locomotors import Turtlebot, JR2_Kinova, Fetch from gibson2.core.simulator import Simulator from gibson2.core.physics.scene import EmptyScene from gibson2.core.physics.interactive_objects import InteractiveObj, BoxShape, YCBObject from gibson2.utils.utils import parse_config from gibson2.core.render.profiler import Profiler import pytest import pybullet as p import numpy as np config = parse_config('../configs/jr_interactive_nav.yaml') s = Simulator(mode='headless', timestep=1 / 240.0) scene = EmptyScene() s.import_scene(scene) jr = JR2_Kinova(config) s.import_robot(jr) jr.robot_body.reset_position([0, 0, 0]) jr.robot_body.reset_orientation([0, 0, 1, 0]) fetch = Fetch(config) s.import_robot(fetch) fetch.robot_body.reset_position([0, 1, 0]) fetch.robot_body.reset_orientation([0, 0, 1, 0]) obj = InteractiveObj( filename='/data4/mdv0/cabinet/0007/part_objs/cabinet_0007.urdf') s.import_interactive_object(obj) 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])