class BaseEnv: def __init__(self, scene_file="", headless=False): self.pr = PyRep() # Launch the application with a scene file in headless mode self.pr.launch(scene_file, headless=headless) self.pr.start() # Start the simulation def step(self): self.pr.step() def stop(self): self.pr.stop() self.pr.shutdown() def load_scene_object_from_file(self, file_path): respondable = self.pr.import_model(file_path) visible = respondable.get_objects_in_tree(exclude_base=True)[0] return SceneObject(respondable_part=respondable, visible_part=visible) def load_mesh_from_file(self, file_path, scaling_factor=1): shape = Shape.import_shape(filename=file_path, scaling_factor=scaling_factor) self.pr.step() return shape
def cop_from_prs(pr: PyRep, scene: Scene) -> Dict[str, list]: cop_obs = defaultdict(list) primitives = (p for p in scene.objects if hasattr(p, 'shape_type')) models = (m for m in scene.objects if hasattr(m, 'model_name')) for p in primitives: c_obj = Shape.create(type=PrimitiveShape[p.shape_type], position=p.position, color=[0.5, 0.0, 0.0], size=[p.width, p.length, p.height], visible_edges=True, mass=0.01) cop_obs[p.shape_type].append(c_obj) for m in models: # print(m.model_name) if m.model_name == "Camera": c_obj = VisionSensor.create([256, 256], position=m.position, orientation=[np.pi, 0.0, 0.0]) elif m.model_name == "Table": c_obj = create_table(pr, m.width, m.length, m.height) c_obj.set_position(prs_to_coppelia_pos(m.position, c_obj)) c_obj.set_orientation(reversed(m.orientation)) elif m.model_name == "Panda": c_obj = pr.import_model(f'models/{m.model_name}.ttm') bounds = c_obj.get_model_bounding_box() dims = np.array(bounds[1::2]) - np.array(bounds[0::2]) adjusted_position = prs_to_coppelia_pos( m.position, c_obj) + np.array( [0, dims[1] / 2.0 - m.length / 2.0, 0.0]) c_obj.set_position(adjusted_position) # c_obj.set_position(scenic_to_coppelia_pos(m.position, c_obj)) c_obj.set_orientation(reversed(m.orientation)) elif m.model_name == "RopeBucket": c_obj = create_rope(pr, m.num_rope_links) c_obj.set_position(prs_to_coppelia_pos(m.position, c_obj)) c_obj.set_orientation(reversed(m.orientation)) c_obj_two = pr.import_model('models/Bucket.ttm') attach_to_rope(pr, c_obj, c_obj_two) else: c_obj = pr.import_model(f'models/{m.model_name}.ttm') c_obj.set_position(prs_to_coppelia_pos(m.position, c_obj)) c_obj.set_orientation(reversed(m.orientation)) cop_obs[m.model_name].append(c_obj) return cop_obs
class Environment(object): """Each environment has a scene.""" def __init__( self, action_mode: ActionMode, dataset_root: str = '', obs_config=ObservationConfig(), headless=False, static_positions: bool = False, robot_configuration='panda', randomize_every: RandomizeEvery = None, frequency: int = 1, visual_randomization_config: VisualRandomizationConfig = None, dynamics_randomization_config: DynamicsRandomizationConfig = None, attach_grasped_objects: bool = True): self._dataset_root = dataset_root self._action_mode = action_mode self._obs_config = obs_config self._headless = headless self._static_positions = static_positions self._robot_configuration = robot_configuration self._randomize_every = randomize_every self._frequency = frequency self._visual_randomization_config = visual_randomization_config self._dynamics_randomization_config = dynamics_randomization_config self._attach_grasped_objects = attach_grasped_objects if robot_configuration not in SUPPORTED_ROBOTS.keys(): raise ValueError('robot_configuration must be one of %s' % str(SUPPORTED_ROBOTS.keys())) if (randomize_every is not None and visual_randomization_config is None and dynamics_randomization_config is None): raise ValueError( 'If domain randomization is enabled, must supply either ' 'visual_randomization_config or dynamics_randomization_config') self._check_dataset_structure() self._pyrep = None self._robot = None self._scene = None self._prev_task = None def _set_arm_control_action(self): self._robot.arm.set_control_loop_enabled(True) if (self._action_mode.arm == ArmActionMode.ABS_JOINT_VELOCITY or self._action_mode.arm == ArmActionMode.DELTA_JOINT_VELOCITY): self._robot.arm.set_control_loop_enabled(False) self._robot.arm.set_motor_locked_at_zero_velocity(True) elif (self._action_mode.arm == ArmActionMode.ABS_JOINT_POSITION or self._action_mode.arm == ArmActionMode.DELTA_JOINT_POSITION or self._action_mode.arm == ArmActionMode.ABS_EE_POSE_WORLD_FRAME or self._action_mode.arm == ArmActionMode.DELTA_EE_POSE_WORLD_FRAME or self._action_mode.arm == ArmActionMode.ABS_EE_POSE_PLAN_WORLD_FRAME or self._action_mode.arm == ArmActionMode.DELTA_EE_POSE_PLAN_WORLD_FRAME or self._action_mode.arm == ArmActionMode.EE_POSE_PLAN_EE_FRAME or self._action_mode.arm == ArmActionMode.EE_POSE_EE_FRAME): self._robot.arm.set_control_loop_enabled(True) elif (self._action_mode.arm == ArmActionMode.ABS_JOINT_TORQUE or self._action_mode.arm == ArmActionMode.DELTA_JOINT_TORQUE): self._robot.arm.set_control_loop_enabled(False) else: raise RuntimeError('Unrecognised action mode.') def _check_dataset_structure(self): if len(self._dataset_root) > 0 and not exists(self._dataset_root): raise RuntimeError('Data set root does not exists: %s' % self._dataset_root) def _string_to_task(self, task_name: str): task_name = task_name.replace('.py', '') try: class_name = ''.join( [w[0].upper() + w[1:] for w in task_name.split('_')]) mod = importlib.import_module("rlbench.tasks.%s" % task_name) except Exception as e: raise RuntimeError( 'Tried to interpret %s as a task, but failed. Only valid tasks ' 'should belong in the tasks/ folder' % task_name) from e return getattr(mod, class_name) def launch(self): if self._pyrep is not None: raise RuntimeError('Already called launch!') self._pyrep = PyRep() self._pyrep.launch(join(DIR_PATH, TTT_FILE), headless=self._headless) arm_class, gripper_class, _ = SUPPORTED_ROBOTS[ self._robot_configuration] # We assume the panda is already loaded in the scene. if self._robot_configuration is not 'panda': # Remove the panda from the scene panda_arm = Panda() panda_pos = panda_arm.get_position() panda_arm.remove() arm_path = join(DIR_PATH, 'robot_ttms', self._robot_configuration + '.ttm') self._pyrep.import_model(arm_path) arm, gripper = arm_class(), gripper_class() arm.set_position(panda_pos) else: arm, gripper = arm_class(), gripper_class() self._robot = Robot(arm, gripper) if self._randomize_every is None: self._scene = Scene(self._pyrep, self._robot, self._obs_config) else: self._scene = DomainRandomizationScene( self._pyrep, self._robot, self._obs_config, self._randomize_every, self._frequency, self._visual_randomization_config, self._dynamics_randomization_config) self._set_arm_control_action() def shutdown(self): if self._pyrep is not None: self._pyrep.shutdown() self._pyrep = None def get_task(self, task_class: Type[Task]) -> TaskEnvironment: # If user hasn't called launch, implicitly call it. if self._pyrep is None: self.launch() self._scene.unload() task = task_class(self._pyrep, self._robot) self._prev_task = task return TaskEnvironment(self._pyrep, self._robot, self._scene, task, self._action_mode, self._dataset_root, self._obs_config, self._static_positions, self._attach_grasped_objects) @property def action_size(self): arm_action_size = 0 gripper_action_size = 1 # Only one gripper style atm if (self._action_mode.arm == ArmActionMode.ABS_JOINT_VELOCITY or self._action_mode.arm == ArmActionMode.DELTA_JOINT_VELOCITY or self._action_mode.arm == ArmActionMode.ABS_JOINT_POSITION or self._action_mode.arm == ArmActionMode.DELTA_JOINT_POSITION or self._action_mode.arm == ArmActionMode.ABS_JOINT_TORQUE or self._action_mode.arm == ArmActionMode.DELTA_JOINT_TORQUE): arm_action_size = SUPPORTED_ROBOTS[self._robot_configuration][2] elif (self._action_mode.arm == ArmActionMode.ABS_EE_POSE_WORLD_FRAME or self._action_mode.arm == ArmActionMode.DELTA_EE_POSE_WORLD_FRAME or self._action_mode.arm == ArmActionMode.ABS_EE_POSE_PLAN_WORLD_FRAME or self._action_mode.arm == ArmActionMode.DELTA_EE_POSE_PLAN_WORLD_FRAME or self._action_mode.arm == ArmActionMode.EE_POSE_PLAN_EE_FRAME or self._action_mode.arm == ArmActionMode.EE_POSE_EE_FRAME): arm_action_size = 7 # pose is always 7 return arm_action_size + gripper_action_size def get_demos(self, task_name: str, amount: int, variation_number=0, image_paths=False) -> List[Demo]: if self._dataset_root is None or len(self._dataset_root) == 0: raise RuntimeError( "Can't ask for a stored demo when no dataset root provided.") demos = utils.get_stored_demos(amount, image_paths, self._dataset_root, variation_number, task_name, self._obs_config) return demos
class CoppeliaSimEnv(gym.Env): """ Superclass for CoppeliaSim gym environments. """ metadata = {"render.modes": ["human"]} def __init__(self, scene: str, dt: float, model: str = None, headless_mode: bool = False): """ Class constructor Args: scene: String, name of the scene to be loaded dt: Float, a time step of the simulation model: Optional[String], name of the model that to be imported headless_mode: Bool, define mode of the simulation """ self.seed() self._assets_path = os.path.join( os.path.dirname(os.path.realpath(__file__)), "assets") self._scenes_path = os.path.join(self._assets_path, "scenes") self._models_path = os.path.join(self._assets_path, "models") self._pr = PyRep() self._launch_scene(scene, headless_mode) self._import_model(model) if not headless_mode: self._clear_gui() self._pr.set_simulation_timestep(dt) self._pr.start() def _launch_scene(self, scene: str, headless_mode: bool): assert os.path.splitext(scene)[1] == ".ttt" assert scene in os.listdir(self._scenes_path) scene_path = os.path.join(self._scenes_path, scene) self._pr.launch(scene_path, headless=headless_mode) def _import_model(self, model: str): if model is not None: assert os.path.splitext(model)[1] == ".ttm" assert model in os.listdir(self._models_path) model_path = os.path.join(self._models_path, model) self._pr.import_model(model_path) @staticmethod def _clear_gui(): sim.simSetBoolParameter(simConst.sim_boolparam_browser_visible, False) sim.simSetBoolParameter(simConst.sim_boolparam_hierarchy_visible, False) sim.simSetBoolParameter(simConst.sim_boolparam_console_visible, False) def step(self, action): return NotImplementedError def reset(self): return NotImplementedError def close(self): self._pr.stop() self._pr.shutdown() def seed(self, seed: int = None): self.np_random, seed = seeding.np_random(seed) return [seed] def render(self, mode: str = "human") -> NoReturn: print("Not implemented yet")
class robotEnv(object): def __init__(self, scene_name, reward_dense, boundary): self.pr = PyRep() SCENE_FILE = join(dirname(abspath(__file__)), scene_name) self.pr.launch(SCENE_FILE, headless=True) self.pr.start() self.pr.set_simulation_timestep(0.05) if scene_name != 'robot_scene.ttt': #youbot_navig2 home_dir = os.path.expanduser('~') os.chdir(join(home_dir, 'robotics_drl')) self.pr.import_model('robot.ttm') # Vision sensor handles self.camera_top = VisionSensor('Vision_sensor') # self.camera_top.set_render_mode(RenderMode.OPENGL3) # self.camera_top.set_resolution([256,256]) self.camera_arm = VisionSensor('Vision_sensor1') self.camera_arm.set_render_mode(RenderMode.OPENGL3) self.camera_arm.set_resolution([256, 256]) self.reward_dense = reward_dense self.reward_termination = 1 if self.reward_dense else 0 self.boundary = boundary # Robot links robot_links = [] links_size = [3, 5, 5, 1] for i in range(len(links_size)): robot_links.append( [Shape('link%s_%s' % (i, j)) for j in range(links_size[i])]) links_color = [[0, 0, 1], [0, 1, 0], [1, 0, 0]] color_i = 0 for j in robot_links: if color_i > 2: color_i = 0 for i in j: i.remove_texture() i.set_color(links_color[color_i]) color_i += 1 def render(self, view='top'): if view == 'top': img = self.camera_top.capture_rgb() * 256 # (dim,dim,3) elif view == 'arm': img = self.camera_arm.capture_rgb() return img def terminate(self): self.pr.stop() # Stop the simulation self.pr.shutdown() def sample_action(self): return [(2 * random.random() - 1) for _ in range(self.action_space)] def rand_bound(self): x = random.uniform(-self.boundary, self.boundary) y = random.uniform(-self.boundary, self.boundary) orientation = random.random() * 2 * pi return x, y, orientation def action_type(self): return 'continuous' def observation_space(self): _, obs = self.get_observation() return obs.shape def step_limit(self): return 250