class Environment: def __init__(self, sim): self.sim = sim self.viewer = None self.initial_state = self.sim.get_state() def is_done(self): x_pos = self.sim.get_state().qpos[0] angle = degrees(self.sim.get_state().qpos[1]) if (12.5 > angle > -12.5) and (0.99 > x_pos > -0.99): return False else: return True def get_reward(self): if not self.is_done(): return 1 else: return -1 def render(self): if not self.viewer: self.viewer = MjViewer(self.sim) self.viewer.render() def reset(self): random_pos = np.random.uniform(0., 0.05, size=(2)) random_vel = np.random.uniform(0., 0.05, size=(2)) self.initial_state = MjSimState(time=0.0, qpos=random_pos, qvel=random_vel, act=None, udd_state={}) self.sim.set_state(self.initial_state) return np.array([self.sim.get_state().qpos.tolist() + self.sim.get_state().qvel.tolist()])
def get_viewer(self): if self.viewer is None: self.viewer = MjViewer( title='Simulate: target = {}'.format(self.target)) self.viewer.start() self.viewer.set_model(self.model) return self.viewer
def start_simulation(self): """ Starts simulation of the test world """ model = self.world.get_model(mode="mujoco_py") self.sim = MjSim(model) if self.render: self.viewer = MjViewer(self.sim) self.sim_state = self.sim.get_state() # For gravity correction gravity_corrected = ["gripper_z_joint"] self._gravity_corrected_qvels = [self.sim.model.get_joint_qvel_addr(x) for x in gravity_corrected] self.gripper_z_id = self.sim.model.actuator_name2id("gripper_z") self.gripper_z_is_low = False self.gripper_actuator_ids = [self.sim.model.actuator_name2id(x) for x in self.gripper.actuators] self.gripper_is_closed = True self.object_id = self.sim.model.body_name2id(self.cube.root_body) object_default_pos = self.sim.data.body_xpos[self.object_id] self.object_default_pos = np.array(object_default_pos, copy=True) self.reset() self.simulation_ready = True
def __init__(self, filepath, random_params={}, gpu_render=False, gui=False, display_data=False): self.model = load_model_from_path(filepath) self.sim = MjSim(self.model) self.filepath = filepath self.gui = gui self.display_data = display_data # Take the default random params and update anything we need self.RANDOM_PARAMS = {} self.RANDOM_PARAMS.update(random_params) if gpu_render: self.viewer = MjViewer(self.sim) else: self.viewer = None # Get start state of params to slightly jitter later self.START_GEOM_POS = self.model.geom_pos.copy() self.START_GEOM_SIZE = self.model.geom_size.copy() self.START_GEOM_QUAT = self.model.geom_quat.copy() self.START_BODY_POS = self.model.body_pos.copy() self.START_BODY_QUAT = self.model.body_quat.copy() self.START_MATID = self.model.geom_matid.copy() #self.FLOOR_OFFSET = self.model.body_pos[self.model.body_name2id('floor')] self.tex_modder = TextureModder(self.sim) self.tex_modder.whiten_materials() # ensures materials won't impact colors self.cam_modder = CameraModder(self.sim) self.light_modder = LightModder(self.sim)
def drop_cube_on_body_demo(): world = MujocoWorldBase() arena = EmptyArena() arena.set_origin([0, 0, 0]) world.merge(arena) soft_torso = SoftTorsoObject() obj = soft_torso.get_collision() box = BoxObject() box_obj = box.get_collision() obj.append(new_joint(name='soft_torso_free_joint', type='free')) box_obj.append(new_joint(name='box_free_joint', type='free')) world.merge_asset(soft_torso) world.worldbody.append(obj) world.worldbody.append(box_obj) # Place torso on ground collision_soft_torso = world.worldbody.find("./body") collision_soft_torso.set("pos", array_to_string(np.array([-0.1, 0, 0.1]))) model = world.get_model(mode="mujoco_py") sim = MjSim(model) viewer = MjViewer(sim) for _ in range(10000): sim.step() viewer.render()
def test(): import cv2 from mujoco_py import load_model_from_xml, MjSim, MjViewer from mujoco_py.modder import TextureModder for i in range(100): l, w, h, t, left, m = sample_refrigerator(False) fridge = build_refrigerator(l, w, h, t, left, set_pose=(3.0, 0.0, -1.0), set_rot=(0, 0, 0, 1)) model = load_model_from_xml(fridge.xml) sim = MjSim(model) viewer = MjViewer(sim) modder = TextureModder(sim) set_two_door_control(sim, 'refrigerator') t = 0 while t < 2000: sim.step() viewer.render() t += 1
def __init__(self, torque_multiplier=50., init_mean=(-0.2, -0.2), render=False): dir_path = os.path.dirname(os.path.realpath(__file__)) path = os.path.join(dir_path, 'asset/point_mass.xml') model = load_model_from_path(path) self.sim = MjSim(model) self.render = render self.init_mean = init_mean self.viewer = MjViewer(self.sim) # Config self.env_name = "Point-Mass-Environment" self.target_position = np.array([0., 0.]) self.target_tolerance = 0.01 self.init_noise = 0.05 self.max_absolute_torque = 5. self.torque_multiplier = torque_multiplier self._initialize_mujoco_state() self.init_state = self.get_state() MDP.__init__(self, [0, 1], self._transition_func, self._reward_func, self.init_state)
def get_viewer(self): if self.viewer is None: self.viewer = MjViewer() self.viewer.start() self.viewer.set_model(self.model) return self.viewer
def test(k=0): from mujoco_py import load_model_from_xml, MjSim, MjViewer from mujoco_py.modder import TextureModder l, w, h, t, left, m = sample_cabinet2() cab = build_cabinet2(l, w, h, t, left) # print(cab.xml) model = load_model_from_xml(cab.xml) sim = MjSim(model) viewer = MjViewer(sim) modder = TextureModder(sim) for name in sim.model.geom_names: modder.rand_all(name) set_two_door_control(sim) q1s = [] q2s = [] t = 0 # mode 0 indicates starting lc while t < 4000: # for name in sim.model.geom_names: # modder.rand_all(name) viewer.render() if t % 250 == 0: q1 = sim.data.qpos[0] q2 = sim.data.qpos[1] print(sim.data.qpos) q1s.append(q1) q2s.append(q2) sim.step() t += 1 # print(q1s) np.save('devdata/q1_' + str(k).zfill(3), q1s) np.save('devdata/q2_' + str(k).zfill(3), q2s)
def __init__(self, arm_or_end_effector): file_dir = os.path.dirname(os.path.realpath(__file__)) if arm_or_end_effector == "arm": self._model = load_model_from_path(file_dir + "/kinova_description/j2s7s300.xml") elif arm_or_end_effector == "hand": self._model = load_model_from_path(file_dir + "/kinova_description/j2s7s300_end_effector.xml") else: print("CHOOSE EITHER HAND OR ARM") raise ValueError # self._model = load_model_from_path("/home/graspinglab/NearContactStudy/MDP/jaco/jaco.xml") self._sim = MjSim(self._model) self._viewer = MjViewer(self._sim) self._timestep = self._sim.model.opt.timestep # print(self._timestep) # self._sim.model.opt.timestep = self._timestep self._torque = [0,0,0,0,0,0,0,0,0,0] self._velocity = [0,0,0,0,0,0,0,0,0,0] self._jointAngle = [0,0,0,0,0,0,0,0,0,0] self._positions = [] # ?? self._numSteps = 0 self._simulator = "Mujoco" # define pid controllers for all joints # self.pid = [PID_(1.0,0.0,0.0), PID_(1.5,0.0,0.0), PID_(1.0,0.0,0.0),PID_(3,0.0,0.0), PID_(1.0,0.0,0.0), # PID_(3.0,0.0,0.0),PID_(1.0,0.0,0.0), PID_(2.0,0.05,0.0), PID_(2.0,0.05,0.0), PID_(2.0,0.05,0.0)] # self.pid = [PID_(185,0.025,0.0), PID_(135,0.02,0.0), PID_(135,0.02,0.0)] self.pid = [PID_(65,0.04,0.0), PID_(10,0.01,0.0), PID_(10,0.01,0.0), PID_(10,0.01,0.0)]
def play(self, mocap_filepath): from mujoco_py import load_model_from_xml, MjSim, MjViewer curr_path = getcwd() xmlpath = '/mujoco/humanoid_deepmimic/envs/asset/dp_env_v2.xml' with open(curr_path + xmlpath) as fin: MODEL_XML = fin.read() model = load_model_from_xml(MODEL_XML) sim = MjSim(model) viewer = MjViewer(sim) self.read_raw_data(mocap_filepath) self.convert_raw_data() from time import sleep phase_offset = np.array([0.0, 0.0, 0.0]) while True: for k in range(len(self.data)): tmp_val = self.data_config[k] sim_state = sim.get_state() sim_state.qpos[:] = tmp_val[:] sim_state.qpos[:3] += phase_offset[:] sim.set_state(sim_state) sim.forward() viewer.render() sim_state = sim.get_state() phase_offset = sim_state.qpos[:3] phase_offset[2] = 0
def __init__(self, init_mean=(-0.2, -0.2), control_cost=False, dense_reward=False, render=False): xml = os.path.join( os.path.expanduser("~"), "git-repos/dm_control/dm_control/suite/point_mass.xml") model = load_model_from_path(xml) self.sim = MjSim(model) self.render = render self.init_mean = init_mean self.control_cost = control_cost self.dense_reward = dense_reward if self.render: self.viewer = MjViewer(self.sim) # Config self.env_name = "Point-Mass-Environment" self.target_position = np.array([0., 0.]) self.target_tolerance = 0.02 self.init_noise = 0.05 self._initialize_mujoco_state() self.init_state = self.get_state() print("Loaded {} with dense_reward={}".format(self.env_name, self.dense_reward)) MDP.__init__(self, [0, 1], self._transition_func, self._reward_func, self.init_state)
def __init__(self, load_path: str, viewer: bool = True): """Initialize a hex robot in mujoco. Parameters --------- load_path : str Path to the xml file. viewer : bool Flag to create a MjViewer for the robot. By default a viewer will be created. """ model = load_model_from_path(load_path) self._sim = MjSim(model) self.num_joints = len(self.joint_names) self._joint_qpos_ids = [ self._sim.model.get_joint_qpos_addr(x) for x in self.joint_names ] self._joint_qvel_ids = [ self._sim.model.get_joint_qvel_addr(x) for x in self.joint_names ] self._joint_actuator_id_map = dict( zip(self.joint_names, range(self.num_joints))) if viewer: self._viewer = MjViewer(self._sim) else: self._viewer = None
def test(): from mujoco_py import load_model_from_xml, MjSim, MjViewer from mujoco_py.modder import TextureModder l, w, h, t, left, m = sample_cabinet() cab = build_cabinet(l, w, h, t, left) filename = 'test.urdf' with open(filename, "w") as text_file: text_file.write(cab.xml) # print(cab.xml) model = load_model_from_xml(cab.xml) sim = MjSim(model) viewer = MjViewer(sim) modder = TextureModder(sim) for name in sim.model.geom_names: modder.rand_all(name) t = 0 if cab.geom[3] == 1: sim.data.ctrl[0] = -0.2 else: sim.data.ctrl[0] = 0.2 while t < 1000: # for name in sim.model.geom_names: # modder.rand_all(name) sim.step() viewer.render() t += 1
def __init__(self, filepath, blender_path=None, visualize=False, num_sims=1): self.filepath = filepath self.blender_path = blender_path self.visualize = visualize self.num_sims = num_sims self.base_model = load_model_from_path(filepath) if self.num_sims > 1: self.pool = MjRenderPool(self.base_model, n_workers=num_sims, modder=ArenaModder) else: self.sim = MjSim(self.base_model) self.arena_modder = ArenaModder(self.sim) if self.visualize: self.arena_modder.visualize = True self.viewer = MjViewer(self.sim) self.arena_modder.viewer = self.viewer else: self.viewer = None
def main(): parent_dir_path = str(pathlib.Path(__file__).parent.absolute()) fname = parent_dir_path + '/kinova_j2s6s300/j2s6s300.xml' model = load_model_from_path(fname) sim = MjSim(model) viewer = MjViewer(sim) t = 0 while True: if t == 1000: ndof = len(sim.data.qpos) captured_state = copy.deepcopy(sim.data.qpos) desired_vel = [0] * len(captured_state) kv = np.eye(ndof) * 10 if t < 1000: sim.data.ctrl[:] = sim.data.qfrc_bias[:] else: sim.data.ctrl[:] = mjc.pd(None, desired_vel, captured_state, sim, ndof=len(captured_state), kv=kv) sim.forward() sim.step() viewer.render() t += 1
def __init__(self): file_path = '/home/mingfei/Documents/DeepMimic/mujoco/humanoid_deepmimic/envs/asset/dp_env_v1.xml' with open(file_path, 'r') as fin: MODEL_XML = fin.read() self.model = load_model_from_xml(MODEL_XML) self.sim = MjSim(self.model) self.viewer = MjViewer(self.sim) mocap_filepath = '/home/mingfei/Documents/DeepMimic/mujoco/motions/humanoid3d_backflip.txt' self.mocap = MocapDM() self.mocap.load_mocap(mocap_filepath) self.interface = MujocoInterface() self.interface.init(self.sim, self.mocap.dt) total_length = 1 # phase variable total_length += self.mocap.num_bodies * (self.mocap.pos_dim + self.mocap.rot_dim) + 1 total_length += self.mocap.num_bodies * (self.mocap.pos_dim + self.mocap.rot_dim - 1) self.state_size = total_length self.action_size = self.interface.action_size
def mj_viewer_setup(sim): viewer = MjViewer(sim) viewer.cam.azimuth = 0 sim.forward() viewer.cam.distance = 1 viewer._run_speed = 0.25 return viewer
def __init__(self, vis=False): super(MjJacoEnv, self).__init__() parent_dir_path = str(pathlib.Path(__file__).parent.absolute()) self.fname = parent_dir_path + '/jaco/jaco.xml' self.model = load_model_from_path(self.fname) self.sim = MjSim(self.model) self.viewer = MjViewer(self.sim) self.vis = vis
def __init__(self): self.model = load_model_from_path("./xml/world3.xml") self.sim = MjSim(self.model) # self.viewer = MyMjViewer(self.sim) self.viewer = MjViewer(self.sim) self.max_vel = [-1000, 1000] self.x_motor = 0 self.y_motor = 0
def get_img(self, w, h, camera_name, depth=False): if self.mj_viewer is None: self.mj_viewer = MjViewer(self.mj_sim) img = self.mj_sim.render(width=w, height=h, camera_name=camera_name, depth=depth) img = self.apply_noise(w, h, img, depth) return img
def model(self, model): self._model = model self.sim = MjSim(model, udd_callback=self.callback) self.data = self.sim.data if self.should_render and not self.viewer: self.viewer = MjViewer(self.sim) self.viewer.cam.azimuth = 180 self.viewer.cam.elevation = -15 self.viewer._hide_overlay = True
def get_viewer(self, config=None): if self.viewer is None: self.viewer = MjViewer() self.viewer.start() self.viewer.set_model(self.model) if config is not None: self.viewer.set_window_pose(config["xpos"], config["ypos"]) self.viewer.set_window_size(config["width"], config["height"]) self.viewer.set_window_title(config["title"]) return self.viewer
def reset_robot(self, robot_id): self.robot_folder_id = self.dir2id[self.robots[robot_id]] robot_file = os.path.join(self.robots[robot_id], 'robot.xml') self.model = load_model_from_path(robot_file) self.sim = MjSim(self.model, nsubsteps=self.nsubsteps) self.update_action_space() self.sim.reset() self.sim.step() if self.viewer is not None: self.viewer = MjViewer(self.sim)
def reset_robot(self, robot_id): self.robot_folder_id = self.dir2id[self.robots[robot_id]] robot_file = os.path.join(self.robots[robot_id], 'robot.xml') self.model = load_model_from_path(robot_file) self.sim = MjSim(self.model, nsubsteps=self.nsubsteps) self.sim.reset() if self.viewer is not None: self.viewer = MjViewer(self.sim) self.init_qpos = self.sim.data.qpos.ravel().copy() self.init_qvel = self.sim.data.qvel.ravel().copy()
def run_activations_fcn(est_activations, model_ver=0, timestep=0.005, Mj_render=False): #!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! the q0 is now the chasis pos. needs to be fixed """ this function runs the predicted activations generatred from running the inverse map on the desired task kinematics """ #loading data #print("loading data") #task_kinematics=np.load("task_kinematics.npy") #est_task_activations=np.load("est_task_activations.npy") model = load_model_from_path( "./models/nmi_leg_w_chassis_v{}.xml".format(model_ver)) sim = MjSim(model) if Mj_render: viewer = MjViewer(sim) # viewer.cam.fixedcamid += 1 # viewer.cam.type = const.CAMERA_FIXED sim_state = sim.get_state() control_vector_length = sim.data.ctrl.__len__() print("control_vector_length: " + str(control_vector_length)) number_of_task_samples = est_activations.shape[0] real_attempt_positions = np.zeros((number_of_task_samples, 2)) real_attempt_activations = np.zeros((number_of_task_samples, 3)) chassis_pos = np.zeros(number_of_task_samples, ) sim.set_state(sim_state) for ii in range(number_of_task_samples): sim.data.ctrl[:] = est_activations[ii, :] sim.step() current_positions_array = sim.data.qpos[-2:] # current_kinematics_array=np.array( # [sim.data.qpos[0], # sim.data.qvel[0], # sim.data.qacc[0], # sim.data.qpos[1], # sim.data.qvel[1], # sim.data.qacc[1]] # ) chassis_pos[ii] = sim.data.get_geom_xpos("Chassis_frame")[0] real_attempt_positions[ii, :] = current_positions_array real_attempt_activations[ii, :] = sim.data.ctrl if Mj_render: viewer.render() real_attempt_kinematics = positions_to_kinematics_fcn( real_attempt_positions[:, 0], real_attempt_positions[:, 1], timestep=0.005) return real_attempt_kinematics, real_attempt_activations, chassis_pos
def initialize(self, use_cur_pos): tmp = MODEL_XML_BASE.format(self.get_asset_mesh_str(), self.get_asset_material_str(), self.get_body_str()) model = load_model_from_xml(tmp) self.sim = MjSim(model) if self.view: self.viewer = MjViewer(self.sim) else: self.viewer = mujoco_py.MjRenderContextOffscreen(self.sim, -1) self._get_starting_step(use_cur_pos)
def test_dampingControl(): model = load_model_from_path("robot.xml") sim = MjSim(model) viewer = MjViewer(sim) timestep = generatePatternedTrajectories.TIMESTEP control_freq = 1/timestep total_time = 2 num_cycles = int(total_time * control_freq) plt.ion() LW = 1.0 fig = plt.figure(figsize=(4,15)) axes = [] lines = [] goals = [] for i in range(7): axes.append(fig.add_subplot(7,1,i+1)) lines.append(axes[i].plot([],[],'b-', lw=LW)[0]) goals.append(axes[i].plot([],[],'r-', lw=LW)[0]) axes[i].set_ylim([-1, 1]) axes[i].set_xlim([0,total_time]) axes[i].set_ylabel("Angle{}(rad)".format(i), fontsize=8) axes[i].set_xlabel("Time(s)", fontsize=8) for test in range(5): q_init = bounds.getRandPosInBounds() qd_goal = np.zeros(7) qd_init = np.random.rand(7) for g in range(7): goals[g].set_ydata(np.ones(num_cycles) * qd_goal[g]) goals[g].set_xdata(np.linspace(0,3,num_cycles)) sim.set_state(MjSimState(time=0,qpos=q_init,qvel=qd_init,act=None,udd_state={})) sim.step() sim_time = 0 for i in range(num_cycles): state = sim.get_state() q = state[1] qd = state[2] sim.data.ctrl[:] = controllers.dampingControl(qd=qd) sim.step() viewer.render() if i % 35 == 0: for a in range(7): lines[a].set_xdata(np.append(lines[a].get_xdata(), sim_time)) lines[a].set_ydata(np.append(lines[a].get_ydata(), qd[a])) fig.canvas.draw() fig.canvas.flush_events() sim_time += timestep if bounds.outOfBounds(q): break for i in range(7): lines[i].set_xdata([]) lines[i].set_ydata([]) time.sleep(1)
def run_activations_fcn(MuJoCo_model_name, est_activations, timestep=0.01, Mj_render=False): #!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! the q0 is now the chasis pos. needs to be fixed """ this function runs the predicted activations generatred from running the inverse map on the desired task kinematics """ #loading data #print("loading data") #task_kinematics=np.load("task_kinematics.npy") #est_task_activations=np.load("est_task_activations.npy") MuJoCo_model = load_model_from_path("./models/"+MuJoCo_model_name) sim = MjSim(MuJoCo_model) if Mj_render: viewer = MjViewer(sim) # to move it to the mounted camera viewer.cam.fixedcamid += 1 viewer.cam.type = const.CAMERA_FIXED # # to record the video # viewer._record_video = True # # recording path # viewer._video_path = "~/Documents/"+str(time.localtime()[3])+str(time.localtime()[4])+str(time.localtime()[5]) sim_state = sim.get_state() control_vector_length=sim.data.ctrl.__len__() #print("control_vector_length: "+str(control_vector_length)) number_of_task_samples=est_activations.shape[0] real_attempt_positions = np.zeros((number_of_task_samples,2)) real_attempt_activations = np.zeros((number_of_task_samples,3)) chassis_pos=np.zeros(number_of_task_samples,) sim.set_state(sim_state) for ii in range(number_of_task_samples): sim.data.ctrl[:] = est_activations[ii,:] sim.step() current_positions_array = sim.data.qpos[-2:] # current_kinematics_array=np.array( # [sim.data.qpos[0], # sim.data.qvel[0], # sim.data.qacc[0], # sim.data.qpos[1], # sim.data.qvel[1], # sim.data.qacc[1]] # ) chassis_pos[ii]=sim.data.get_geom_xpos("Chassis_frame")[0] real_attempt_positions[ii,:] = current_positions_array real_attempt_activations[ii,:] = sim.data.ctrl if Mj_render: viewer.render() real_attempt_kinematics = positions_to_kinematics_fcn( real_attempt_positions[:,0], real_attempt_positions[:,1], timestep = 0.01) return real_attempt_kinematics, real_attempt_activations, chassis_pos
def watch(self, repeat_count=4): if self.viewer is None: self.viewer = MjViewer(self.sim) for _ in range(repeat_count): self.sim.set_state(self.initial_state) for i in range(self.total_time): self.sim.data.ctrl[:] = self.ctrl_array[i] self.sim.step() self.viewer.render() self.run( reset=False ) # so that the trajectory will be compatible with what user watches
MODEL_XML = """ <?xml version="1.0" ?> <mujoco> <worldbody> <body name="box" pos="0 0 0.2"> <geom size="0.15 0.15 0.15" type="box"/> <joint axis="1 0 0" name="box:x" type="slide"/> <joint axis="0 1 0" name="box:y" type="slide"/> </body> <body name="floor" pos="0 0 0.025"> <geom size="1.0 1.0 0.02" rgba="0 1 0 1" type="box"/> </body> </worldbody> </mujoco> """ model = load_model_from_xml(MODEL_XML) sim = MjSim(model) viewer = MjViewer(sim) step = 0 while True: t = time.time() x, y = math.cos(t), math.sin(t) viewer.add_marker(pos=np.array([x, y, 1]), label=str(t)) viewer.render() step += 1 if step > 100 and os.getenv('TESTING') is not None: break
#!/usr/bin/env python3 """ Shows how to use render callback. """ from mujoco_py import load_model_from_path, MjSim, MjViewer from mujoco_py.modder import TextureModder import os modder = None def render_callback(sim, viewer): global modder if modder is None: modder = TextureModder(sim) for name in sim.model.geom_names: modder.rand_all(name) model = load_model_from_path("xmls/fetch/main.xml") sim = MjSim(model, render_callback=render_callback) viewer = MjViewer(sim) t = 0 while True: viewer.render() t += 1 if t > 100 and os.getenv('TESTING') is not None: break