How to use robosuite - 10 common examples

def configure_location(self):
        self.bottom_pos = np.array([0, 0, 0])
        self.floor.set("pos", array_to_string(self.bottom_pos))

        self.center_pos = self.bottom_pos + np.array([0, 0, self.table_half_size[2]])
        self.table_body.set("pos", array_to_string(self.center_pos))
        self.table_collision.set("size", array_to_string(self.table_half_size))
        self.table_collision.set("friction", array_to_string(self.table_friction))
        self.table_visual.set("size", array_to_string(self.table_half_size))

        self.table_top.set(
            "pos", array_to_string(np.array([0, 0, self.table_half_size[2]]))
        )

def configure_location(self):
        self.bottom_pos = np.array([0, 0, 0])
        self.floor.set("pos", array_to_string(self.bottom_pos))

        self.center_pos = self.bottom_pos + np.array([0, 0, self.table_half_size[2]])
        self.table_body.set("pos", array_to_string(self.center_pos))
        self.table_collision.set("size", array_to_string(self.table_half_size))
        self.table_collision.set("friction", array_to_string(self.table_friction))
        self.table_visual.set("size", array_to_string(self.table_half_size))

        self.table_top.set(
            "pos", array_to_string(np.array([0, 0, self.table_half_size[2]]))
        )

def configure_location(self):
        self.bottom_pos = np.array([0, 0, 0])
        self.floor.set("pos", array_to_string(self.bottom_pos))

        self.center_pos = self.bottom_pos + np.array([0, 0, self.table_half_size[2]])
        self.table_body.set("pos", array_to_string(self.center_pos))
        self.table_collision.set("size", array_to_string(self.table_half_size))
        self.table_collision.set("friction", array_to_string(self.table_friction))
        self.table_visual.set("size", array_to_string(self.table_half_size))

        self.table_top.set(
            "pos", array_to_string(np.array([0, 0, self.table_half_size[2]]))
        )

def configure_location(self):
        self.bottom_pos = np.array([0, 0, 0])
        self.floor.set("pos", array_to_string(self.bottom_pos))

        self.center_pos = self.bottom_pos + np.array([0, 0, self.table_half_size[2]])
        self.table_body.set("pos", array_to_string(self.center_pos))
        self.table_collision.set("size", array_to_string(self.table_half_size))
        self.table_collision.set("friction", array_to_string(self.table_friction))
        self.table_visual.set("size", array_to_string(self.table_half_size))

        self.table_top.set(
            "pos", array_to_string(np.array([0, 0, self.table_half_size[2]]))
        )

def configure_location(self):
        self.bottom_pos = np.array([0, 0, 0])
        self.floor.set("pos", array_to_string(self.bottom_pos))

        self.center_pos = self.bottom_pos + np.array([0, 0, self.table_half_size[2]])
        self.table_body.set("pos", array_to_string(self.center_pos))
        self.table_collision.set("size", array_to_string(self.table_half_size))
        self.table_collision.set("friction", array_to_string(self.table_friction))
        self.table_visual.set("size", array_to_string(self.table_half_size))

        self.table_top.set(
            "pos", array_to_string(np.array([0, 0, self.table_half_size[2]]))
        )

def make_robosuite(env_name, env_config):
    import robosuite

    env = robosuite.make(
        env_name,
        has_renderer=env_config.render,
        ignore_done=True,
        use_camera_obs=env_config.pixel_input,
        has_offscreen_renderer=env_config.pixel_input,
        camera_height=84,
        camera_width=84,
        render_collision_mesh=False,
        render_visual_mesh=True,
        camera_name='agentview',
        use_object_obs=(not env_config.pixel_input),
        camera_depth=env_config.use_depth,
        reward_shaping=True,
        # demo_config=env_config.demonstration,
    )
    env = RobosuiteWrapper(env, env_config)

def _step(self, target_qpos):
        o = None
        delta_xyz = (target_qpos[:3] - self._eef_pos) / self._hp.substeps
        for i in range(self._hp.substeps):
            current_rot = self._env._right_hand_orn

            pitch, roll, yaw = 0, 0, target_qpos[3]
            drot1 = rotation_matrix(angle=-pitch, direction=[1., 0, 0], point=None)[:3, :3]
            drot2 = rotation_matrix(angle=roll, direction=[0, 1., 0], point=None)[:3, :3]
            drot3 = rotation_matrix(angle=yaw, direction=[0, 0, 1.], point=None)[:3, :3]
            desired_rot = start_rot.dot(drot1.dot(drot2.dot(drot3)))
            drotation = current_rot.T.dot(desired_rot)
            dquat = mat2quat(drotation)

            o = self._env.step(np.concatenate((delta_xyz, dquat, [target_qpos[-1]])))[0]
        self._previous_target_qpos = target_qpos
        return self._proc_obs(o)

def _step(self, target_qpos):
        o = None
        delta_xyz = (target_qpos[:3] - self._eef_pos) / self._hp.substeps
        for i in range(self._hp.substeps):
            current_rot = self._env._right_hand_orn

            pitch, roll, yaw = 0, 0, target_qpos[3]
            drot1 = rotation_matrix(angle=-pitch, direction=[1., 0, 0], point=None)[:3, :3]
            drot2 = rotation_matrix(angle=roll, direction=[0, 1., 0], point=None)[:3, :3]
            drot3 = rotation_matrix(angle=yaw, direction=[0, 0, 1.], point=None)[:3, :3]
            desired_rot = start_rot.dot(drot1.dot(drot2.dot(drot3)))
            drotation = current_rot.T.dot(desired_rot)
            dquat = mat2quat(drotation)

            o = self._env.step(np.concatenate((delta_xyz, dquat, [target_qpos[-1]])))[0]
        self._previous_target_qpos = target_qpos
        return self._proc_obs(o)

def _step(self, target_qpos):
        o = None
        delta_xyz = (target_qpos[:3] - self._eef_pos) / self._hp.substeps
        for i in range(self._hp.substeps):
            current_rot = self._env._right_hand_orn

            pitch, roll, yaw = 0, 0, target_qpos[3]
            drot1 = rotation_matrix(angle=-pitch, direction=[1., 0, 0], point=None)[:3, :3]
            drot2 = rotation_matrix(angle=roll, direction=[0, 1., 0], point=None)[:3, :3]
            drot3 = rotation_matrix(angle=yaw, direction=[0, 0, 1.], point=None)[:3, :3]
            desired_rot = start_rot.dot(drot1.dot(drot2.dot(drot3)))
            drotation = current_rot.T.dot(desired_rot)
            dquat = mat2quat(drotation)

            o = self._env.step(np.concatenate((delta_xyz, dquat, [target_qpos[-1]])))[0]
        self._previous_target_qpos = target_qpos
        return self._proc_obs(o)

from metaworld.envs.mujoco.sawyer_xyz.sawyer_peg_insertion_side import SawyerPegInsertionSideEnv
from metaworld.envs.mujoco.sawyer_xyz.sawyer_peg_insertion_topdown import SawyerPegInsertionTopdownEnv
from metaworld.envs.mujoco.sawyer_xyz.sawyer_peg_unplug_side import SawyerPegUnplugSideEnv



from robosuite.devices import SpaceMouse
from metaworld.envs.mujoco.utils import rotation
from robosuite.utils.transform_utils import mat2quat
from metaworld.envs.env_util import quat_to_zangle, zangle_to_quat


import gym
import metaworld

space_mouse = SpaceMouse()
env = SawyerPegInsertionTopdownEnv(random_init=True, obs_type='with_goal')
NDIM = env.action_space.low.size
lock_action = False
obs = env.reset()
action = np.zeros(10)
closed = False

while True:
    done = False
    env.render()

    state = space_mouse.get_controller_state()
    dpos, rotation, grasp, reset = (
        state["dpos"],
        state["rotation"],
        state["grasp"],