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
