def _setup_world(self):
"""
Helper method for handling setup of the MuJoCo world.
Args:
filename: Path to XML file containing the world information.
"""
if "varying_mass" in self._hyperparams:
self.create_xml()
pdb.set_trace()
self._model = mujoco_py.MjModel(self._hyperparams['filename'])
self.model_nomarkers = mujoco_py.MjModel(
self._hyperparams['filename_nomarkers'])
gofast = True
self._small_viewer = mujoco_py.MjViewer(
visible=True,
init_width=self._hyperparams['image_width'],
init_height=self._hyperparams['image_height'],
go_fast=gofast)
self._small_viewer.start()
self._small_viewer.cam.camid = 0
if self._hyperparams['additional_viewer']:
self._large_viewer = mujoco_py.MjViewer(visible=True,
init_width=480,
init_height=480,
go_fast=gofast)
self._large_viewer.start()
self._large_viewer.cam.camid = 0
def _setup_world(self, filename):
"""
Helper method for handling setup of the MuJoCo world.
Args:
filename: Path to XML file containing the world information.
"""
self._model= mujoco_py.MjModel(filename)
self.model_nomarkers = mujoco_py.MjModel(self._hyperparams['filename_nomarkers'])
# changes here:
self._joint_idx = range(self._hyperparams['joint_angles'])
self._vel_idx = range( self._hyperparams['joint_angles'], self._hyperparams['joint_velocities'] + self._hyperparams['joint_angles'])
gofast = True
self._small_viewer = mujoco_py.MjViewer(visible=True,
init_width=self._hyperparams['image_width'],
init_height=self._hyperparams['image_height'],
go_fast=gofast)
self._small_viewer.start()
self._small_viewer.cam.camid = 0
if self._hyperparams['additional_viewer']:
self._large_viewer = mujoco_py.MjViewer(visible=True, init_width=480,
init_height=480, go_fast=gofast)
self._large_viewer.start()
self._large_viewer.cam.camid = 0
def _get_viewer(self, mode='human'):
viewer = self._viewers.get(mode)
if viewer is None:
if mode == 'human':
viewer = mujoco_py.MjViewer(self.sim)
elif mode == 'rgb_array' or mode == 'depth_array':
viewer = mujoco_py.MjViewer(self.sim)
# The following should work but it does not. Therefore, replaced by human rendering (with MjViewer, the line above) now.
# viewer = mujoco_py.MjRenderContextOffscreen(self.sim, -1)
self._viewers[mode] = viewer
self._viewer_setup(mode=mode)
return self._viewers[mode]
def render(self, mode: RenderMode = RenderMode(), render_step: int = 1):
if self._curr_step % render_step == 0:
# Call base class
super().render(mode)
# Print to console
if mode.text:
print(
f"step: {self._curr_step:4d} | r_t: {self._curr_rew: 1.3f} | a_t: {self._curr_act} | s_t+1: {self.state}"
)
# Forward to MuJoCo viewer
if mode.video:
if self.viewer is None:
# Create viewer if not existent (see 'human' mode of OpenAI Gym's MujocoEnv)
self.viewer = mujoco_py.MjViewer(self.sim)
# Adjust window size and position to custom values
import glfw
glfw.make_context_current(self.viewer.window)
glfw.set_window_size(self.viewer.window, 1280, 720)
glfw.set_window_pos(self.viewer.window, 50, 50)
self.configure_viewer()
self.viewer.render()
def vis_impedance_random_setpoint(collision=False):
options = dict()
options['model_path'] = 'full_kuka_no_collision.xml'
options['rot_scale'] = .3
options['stiffness'] = np.array([1., 1., 1., 3., 3., 3.])
sim = create_sim(collision=collision)
controller = ImpedanceControllerV2(sim, **options)
frame_skip = 50
high = np.array([.1, .1, .1, 2, 2, 2])
low = -np.array([.1, .1, .1, 2, 2, 2])
viewer = mujoco_py.MjViewer(sim)
for i in range(10):
# Set a different random state and run the controller.
qpos = np.random.uniform(-1., 1., size=7)
qvel = np.zeros(7)
state = np.concatenate([qpos, qvel])
sim_state = sim.get_state()
sim_state.qpos[:] = qpos
sim_state.qvel[:] = qvel
sim.set_state(sim_state)
sim.forward()
for i in range(3000):
controller.set_action(np.random.uniform(high, low))
for i in range(frame_skip):
sim.data.ctrl[:] = controller.get_torque()
sim.step()
render_frame(viewer, controller.pos_set, controller.quat_set)
viewer.render()
def __init__(self, write_assets=True):
if write_assets:
# generate and save pyramids
for num_sides in range(3, 7):
pyramid_mesh = create_pyramid_mesh(num_sides=num_sides)
pyramid_mesh.save(
os.path.join(
gym.__path__[0],
'envs/mujoco/assets/pyramid%d.stl' % num_sides))
# generate and save prisms
for num_sides in range(3, 7):
prism_mesh = create_prism_mesh(num_sides=num_sides)
prism_mesh.save(
os.path.join(gym.__path__[0],
'envs/mujoco/assets/prism%d.stl' % num_sides))
# generate and save pusher
model = create_shape_pusher()
model.save(
os.path.join(gym.__path__[0],
'envs/mujoco/assets/shape_pusher.xml'))
# MujocoEnv's superclass init
self.frame_skip = 5
self.model = mujoco_py.MjModel(
os.path.join(gym.__path__[0],
'envs/mujoco/assets/shape_pusher.xml'))
self.data = self.model.data
self.width = 256
self.height = 256
self.viewer = mujoco_py.MjViewer(init_width=self.width,
init_height=self.height)
self.viewer.start()
self.viewer.set_model(self.model)
self.viewer_setup()
self.metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second': int(np.round(1.0 / self.dt))
}
self.action_space = spaces.Box(-1, 1, shape=(2, ))
joints_range = self.model.jnt_range.copy()
self.observation_space = {
'image': spaces.Box(0, 255, shape=(self.width, self.height)),
'manip_xy': spaces.Box(joints_range[:2, 0], joints_range[:2, 1]),
'obj_pose': spaces.Box(-np.inf, np.inf, shape=(7, ))
}
self._seed()
self.num_objs = len([
body_name for body_name in self.model.body_names
if body_name.startswith(six.b('obj'))
])
self.orig_body_mass = self.model.body_mass.copy()
self.obj_idx = None
def _get_viewer(self):
if self.viewer is None:
self.viewer = mujoco_py.MjViewer(init_width=500, init_height=500)
self.viewer.start()
self.viewer.set_model(self.model)
self.viewer_setup()
return self.viewer
def _get_viewer(self):
if self.viewer is None:
self.viewer = mujoco_py.MjViewer()
self.viewer.start()
self.viewer.set_model(self.model)
self.viewer_setup()
return self.viewer
def _get_viewer(self):
if self.viewer is None:
if self.from_vision:
visible = (VISIBLE is not None)
self.viewer = mujoco_py.MjViewer(visible=visible,
init_width=self.width,
init_height=self.height,
go_fast=(not visible))
else:
self.viewer = mujoco_py.MjViewer(visible=True)
self.viewer.start()
self.viewer.set_model(self.model)
self.viewer_setup()
return self.viewer
def run(self):
(_, _, obs_rgb_view2) = self.env.reset()
if self.render:
viewer = mujoco_py.MjViewer(self.env.sim)
else:
f, ax = plt.subplots()
im = ax.imshow(obs_rgb_view2)
while True:
self.env.reset()
while True:
# random action selection
action = np.random.choice([0, 1, 2, 3, 4], 6)
# take the random action and observe the reward and next state (2 rgb views and proprioception)
(obs_joint, obs_rgb_view1,
obs_rgb_view2), reward, done = self.env.step(action)
# print("action : ", action)
# print("reward : ", reward)
if done:
break
if self.render:
viewer.render()
else:
im.set_data(obs_rgb_view2)
plt.draw()
plt.pause(0.1)
def get_img(env_name, seed):
env = gym.make(env_name)
env.seed(int(seed))
env.reset()
env_scene = env.env_scene
env_scene.viewer = mujoco_py.MjViewer(init_width=1000, init_height=750)
env_scene.viewer.start()
env_scene.viewer.set_model(env_scene.model)
env_scene.viewer_setup()
print("Type save to save the image, step to take one timestep.")
running = True
while running:
img = None
while sys.stdin not in select.select([sys.stdin], [], [], 0)[0]:
env.render()
img = env.render(mode='rgb_array')
input = sys.stdin.readline().strip()
if input == 'save':
running = False
elif input == 'step':
action = tuple(np.zeros(space.shape) for space in env.action_space.spaces)
env.step(action)
else:
print(f"Unrecognized command '{input}'")
return img
def __init__(self, model=None, simulation=None, viewer=None):
path = os.path.realpath(__file__)
path = str(Path(path).parent.parent.parent)
if model == None:
self.model = mp.load_model_from_path(
path + '/UR5+gripper/UR5gripper_2_finger.xml')
else:
self.model = model
if simulation == None:
self.sim = mp.MjSim(self.model)
else:
self.sim = simulation
if viewer == None:
self.viewer = mp.MjViewer(self.sim)
else:
self.viewer = viewer
self.create_lists()
self.groups = defaultdict(list)
self.groups['All'] = [i for i in range(len(self.sim.data.ctrl))]
self.create_group('Arm', [i for i in range(6)])
self.create_group('Gripper', [6])
self.actuated_joint_ids = np.array([i[2] for i in self.actuators])
self.reached_target = False
self.current_output = np.zeros(len(self.sim.data.ctrl))
self.image_counter = 0
self.ee_chain = ikpy.chain.Chain.from_urdf_file(
path + '/UR5+gripper/ur5_gripper.urdf')
self.cam_matrix = None
self.cam_init = False
self.last_movement_steps = 0
def render_to_window(self):
"""Renders the simulation to a window."""
if not self._onscreen_renderer:
self._onscreen_renderer = mujoco_py.MjViewer(self._sim)
self._update_camera_properties(self._onscreen_renderer.cam)
self._onscreen_renderer.render()
def __init__(self,
high_ctrl,
low_ctrl,
env_path,
low_ctrl_config=None,
arm_name="RightArm",
desired_joints=None,
isdraw=False):
self.world = mujoco_py.load_model_from_path(env_path)
self.sim = mujoco_py.MjSim(self.world)
high_ctrl.motion_duration = 1.8
self.high_ctrl = high_ctrl
self.high_ctrl.low_ctrl = low_ctrl(model=self.world, sim=self.sim, config=low_ctrl_config, \
arm_name=arm_name, desired_joints=desired_joints)
self.isdraw = isdraw
if self.isdraw:
self.viewer = mujoco_py.MjViewer(self.sim)
self.viewer._paused = True
self.joint_ctrl = JointController(model=self.world,
sim=self.sim,
arm_name=arm_name,
ctrl_mode="Torque")
self.init_joints = np.array([0, -0.2, 0, 0, 1.8, 3.14, 0, 0])
self.is_ball_pos_change = False
def render_mujoco(model_path):
model = mujoco_py.load_model_from_path(model_path)
sim = mujoco_py.MjSim(model)
viewer = mujoco_py.MjViewer(sim)
while True:
viewer.render()
time.sleep(.01)
def _get_viewer(self):
if self._viewer is None:
self._viewer = mujoco_py.MjViewer(self.sim)
self._viewer.cam.fixedcamid = self._camera_id
self._viewer.cam.type = mujoco_py.generated.const.CAMERA_FIXED
self._viewer_reset()
return self._viewer
def __init__(self, fullpath="/home/brhm/OPENAI/baxter/baxter.xml", visible= True):
self.model = load_model_from_path(fullpath)
self.sim = MjSim(self.model)
self.data = self.sim.data
self.idx = {
"right" : [1,7],
"left" : [10,16]
}
self.gripper_idx = {
"right" : [8,9],
"left" : [17,18]
}
self.tuck_pose = {
'left': [[-0.08, -1.0, -1.19, 1.94, 0.67, 1.03, -0.50]],
'right': [[0.08, -1.0, 1.19, 1.94, -0.67, 1.03, 0.50]]
}
ctrl = self.data.ctrl.copy()
self.data.ctrl[-1] = 10
# self.apply_action(action={"left": np.array(self.tuck_pose["left"] ), "right"=[]})
self.viewer = mujoco_py.MjViewer(self.sim)
self.viewer.render()
#self.viewer.set_model(self.model)
# cam_pos = np.array([0.1, 0.0, 0.7, 0.01, -45., 0.])
cam_pos = np.array([1.0, 0.0, 0.7, 0.5, -45, 180])
self.set_cam_position(self.viewer, cam_pos)
def __init__(self, model_path):
# self.model = mujoco_py.load_model_from_xml(model_path)
self.model = dy.make_model(model_path)
self.sim = mujoco_py.MjSim(self.model)
self.viewer = mujoco_py.MjViewer(self.sim)
self.qdim = self.model.nq
self.udim = self.model.nu
self.pdim = 2 * self.qdim + self.udim
def setupcam(self):
self.viewer = mujoco_py.MjViewer(self.sim)
self.viewer.cam.distance = self.model.stat.extent * .3
self.viewer.cam.lookat[0] = 2.
self.viewer.cam.lookat[1] = 0.3
self.viewer.cam.lookat[2] = 0.9
self.viewer.cam.elevation = -30
self.viewer.cam.azimuth = -10
def render(self, on=True):
if on:
if self.viewer is None:
self.viewer = mjc.MjViewer(self.sim)
self.viewer.render()
if not on:
if self.viewer:
self.viewer = None
def __init__(self, wid):
self.wid = wid
#self.env = gym.make(GAME).unwrapped
self.env = JacoEnv(64, 64, 100)
self.ppo = GLOBAL_PPO
if self.wid == 0:
self.viewer = mujoco_py.MjViewer(self.env.sim)
def setupcam(self):
self.viewer = mujoco_py.MjViewer(self.sim)
self.viewer.cam.trackbodyid = -1
self.viewer.cam.distance = self.model.stat.extent * .3
self.viewer.cam.lookat[0] = -0.1
self.viewer.cam.lookat[1] = -1
self.viewer.cam.lookat[2] = 0.5
self.viewer.cam.elevation = -30
def __init__(self, model_path, initial_qpos, n_actions, n_substeps):
if model_path.startswith('/'):
fullpath = model_path
else:
fullpath = os.path.join(os.path.dirname(__file__), 'assets',
model_path)
if not os.path.exists(fullpath):
raise IOError('File {} does not exist'.format(fullpath))
model = mujoco_py.load_model_from_path(fullpath)
self.sim = mujoco_py.MjSim(model, nsubsteps=n_substeps)
#self.viewer = None # comment when using "human"
self.viewer = mujoco_py.MjViewer(
self.sim) #comment when using "rgb_array"
self._viewers = {}
self.modder = TextureModder(self.sim)
self.visual_randomize = True
self.mat_modder = MaterialModder(self.sim)
self.light_modder = LightModder(self.sim)
self.metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second': int(np.round(1.0 / self.dt))
}
self.visual_data_recording = True
self._index = 0
self._label_matrix = []
self.seed()
self._env_setup(initial_qpos=initial_qpos)
self.initial_state = copy.deepcopy(self.sim.get_state())
self.goal = self._sample_goal()
obs = self._get_obs()
self.action_space = spaces.Box(-1.,
1.,
shape=(n_actions, ),
dtype='float32')
self.observation_space = spaces.Dict(
dict(
desired_goal=spaces.Box(-np.inf,
np.inf,
shape=obs['achieved_goal'].shape,
dtype='float32'),
achieved_goal=spaces.Box(-np.inf,
np.inf,
shape=obs['achieved_goal'].shape,
dtype='float32'),
observation=spaces.Box(-np.inf,
np.inf,
shape=obs['observation'].shape,
dtype='float32'),
))
if self.viewer:
self._viewer_setup()
def __init__(self, simulation,numSimulationFrames, visualize,isTerminal, reshapeActionList):
self.simulation = simulation
self.isTerminal = isTerminal
self.numSimulationFrames = numSimulationFrames
self.numJointEachSite = int(self.simulation.model.njnt/self.simulation.model.nsite)
self.reshapeActionList=reshapeActionList
self.visualize=visualize
if visualize:
self.physicsViewer = mujoco.MjViewer(simulation)
def load_env(self, num):
if num < len(self.env_paths):
new_scene = self.env_paths[num]
scene = mujoco_py.load_model_from_path(new_scene)
self.env = mujoco_py.MjSim(scene)
if self.is_vis:
self.viewer = mujoco_py.MjViewer(self.env)
else:
print("Wrong number,")
def __init__(self, xml_specification, mesh_specification=None):
ref_angles = {
0: -np.pi / 2,
1: 0,
2: np.pi / 2,
3: -np.pi / 2,
4: np.pi / 2,
5: 0
}
self.ref_angles = ref_angles
self.xml_specification = xml_specification
self.mesh_specification = mesh_specification
if not path.isfile(self.xml_specification):
raise Exception('Missing XML specification at: {}'.format(
self.xml_specification))
if mesh_specification is not None:
if not path.isdir(self.mesh_specification):
raise Exception('Missing mesh specification at: {}'.format(
self.mesh_specification))
print('Arm model is specified at: {}'.format(self.xml_specification))
try:
self.mjc_model = mjc.load_model_from_path(self.xml_specification)
except:
raise Exception('Mujoco was unable to load the model')
# Initializing joint dictionary
joint_ids, joint_names = self.get_joints_info()
joint_positions_addr = [
self.mjc_model.get_joint_qpos_addr(name) for name in joint_names
]
joint_velocity_addr = [
self.mjc_model.get_joint_qvel_addr(name) for name in joint_names
]
self.joint_dict = {}
for i, ii in enumerate(joint_ids):
self.joint_dict[ii] = {
'name': joint_names[i],
'position_address': joint_positions_addr[i],
'velocity_address': joint_velocity_addr[i]
}
if not np.all(np.array(self.mjc_model.jnt_type) ==
3): # 3 stands for revolute joint
raise Exception('Revolute joints are assumed')
self.n_joints = len(self.joint_dict.items())
# Initialize simulator
self.simulation = mjc.MjSim(self.mjc_model)
self.viewer = mjc.MjViewer(self.simulation)
'''
def _get_viewer(self, mode):
self.viewer = self._viewers.get(mode)
if self.viewer is None:
if mode == 'human':
self.viewer = mujoco_py.MjViewer(self.sim)
self.viewer_setup()
self._viewers[mode] = self.viewer
self.viewer_setup()
return self.viewer
def _get_viewer(self):
if self.viewer is None:
self.viewer = mujoco_py.MjViewer(
init_height=Context.config.get('view.height', 400),
init_width=Context.config.get('view.width', 600))
self.viewer.start()
self.viewer.set_model(self.model)
self.viewer_setup()
return self.viewer
def _get_viewer(self, mode):
self.viewer = self._viewers.get(mode)
if self.viewer is None:
if mode == 'human':
self.viewer = mujoco_py.MjViewer(self.sim)
elif mode == 'rgb_array':
self.viewer = mujoco_py.MjRenderContextOffscreen(self.sim, 0)
self.viewer_setup()
self._viewers[mode] = self.viewer
return self.viewer
def _get_viewer(self):
if self.viewer is None:
self.viewer = mujoco_py.MjViewer(visible=True,
init_width=500,
init_height=500,
go_fast=False)
self.viewer.start()
self.viewer.set_model(self.model)
self.viewer_setup()
return self.viewer
