def set_state(self, qpos):
old_state = self.sim.get_state()
new_state = mujoco_py.MjSimState(old_state.time, qpos, old_state.qvel,
old_state.act, old_state.udd_state)
self.sim.set_state(new_state)
self.sim.forward()
def reset(self):
print("resetting environment. Average speed/desired speed:",
self.avg_spd / self.step_ctr, " / ", self.desired_speed)
self.desired_speed = random.choice([-2, 2])
self.avg_spd = 0
self.step_ctr = 1
self.sim.reset()
qpos = self.sim.data.qpos
qvel = self.sim.data.qvel
if STOCHASTIC:
qpos += np.random.uniform(low=-.005, high=.005, size=self.model.nq)
qvel += np.random.uniform(low=-.005, high=.005, size=self.model.nv)
old_state = self.sim.get_state()
tmp = mj.MjSimState(old_state.time, qpos, qvel, old_state.act,
old_state.udd_state)
self.sim.set_state(tmp)
self.sim.forward()
#obs_vec = np.concatenate([self.sim.data.qpos.flat[1:], np.clip(self.sim.data.qvel.flat, -10, 10)], [self.desired_speed])
obs_vec = np.concatenate([
self.sim.data.qpos.flat[1:],
np.clip(self.sim.data.qvel.flat, -10, 10)
])
obs_vec = np.append(obs_vec, self.desired_speed)
return obs_vec
def set_state(self, qpos, qvel=None):
qvel = np.zeros(self.q_dim) if qvel is None else qvel
old_state = self.sim.get_state()
new_state = mujoco_py.MjSimState(old_state.time, qpos, qvel,
old_state.act, old_state.udd_state)
self.sim.set_state(new_state)
self.sim.forward()
def set_state(self, qpos, qvel):
assert qpos.shape == (self.model.nq,) and qvel.shape == (self.model.nv,)
old_state = self.sim.get_state()
new_state = mujoco_py.MjSimState(old_state.time, qpos, qvel,
old_state.act, old_state.udd_state)
self.sim.set_state(new_state)
self.sim.forward()
def set_state(sim, x):
state = sim.get_state()
qpos = x[:sim.model.nq]
qvel = x[sim.model.nq:]
state = mujoco_py.MjSimState(state.time, qpos, qvel,
state.act, state.udd_state)
sim.set_state(state)
sim.forward()
def set_state(self, qpos, qvel):
""" Set MjState given the position and velocity"""
assert qpos.shape == (self.sim.model.nq,) and qvel.shape == (self.sim.model.nv,)
old_state = self.sim.get_state()
new_state = mujoco_py.MjSimState(old_state.time, qpos, qvel,
old_state.act, old_state.udd_state)
self.sim.set_state(new_state)
self.sim.forward()
def set_state(self, qpos):
'''Sets the state of the simulated model given the joint angles qpos'''
#assert qpos.shape == (model.nq,)
old_state = self.sim.get_state()
new_state = mujoco_py.MjSimState(old_state.time, qpos, old_state.qvel,
old_state.act, old_state.udd_state)
self.sim.set_state(new_state)
self.sim.forward()
def set_state(self, qpos=None, qvel=None):
old_state = self.sim.get_state()
qpos = old_state.qpos if qpos is None else qpos
qvel = old_state.qvel if qvel is None else qvel
new_state = mujoco_py.MjSimState(old_state.time, qpos, qvel,
old_state.act, old_state.udd_state)
self.sim.set_state(new_state)
self.sim.forward()
self.sim.step()
def set_joint_kinematics_in_sim(self, qpos=None, qvel=None):
""" Specify the desired qpos and qvel and do a forward step in the simulation
to set the specified qpos and qvel values. """
old_state = self.sim.get_state()
if qpos is None or qvel is None:
qpos, qvel = self.refs.get_ref_kinmeatics()
new_state = mujoco_py.MjSimState(old_state.time, qpos, qvel,
old_state.act, old_state.udd_state)
self.sim.set_state(new_state)
def set_state(self, qpos, qvel, sim_ind):
assert qpos.shape == (self.model.nq, ) and qvel.shape == (
self.model.nv, )
sim = self.pool.sims[sim_ind]
old_state = sim.get_state()
new_state = mjc.MjSimState(old_state.time, qpos, qvel, old_state.act,
old_state.udd_state)
sim.set_state(new_state)
sim.forward()
def reset(self,
init_state: np.ndarray = None,
domain_param: dict = None) -> np.ndarray:
# Reset time
self._curr_step = 0
# Reset the domain parameters
if domain_param is not None:
self.domain_param = domain_param
# Sample or set the initial simulation state
if init_state is None:
# Sample init state from init state space
init_state = self.init_space.sample_uniform()
elif not isinstance(init_state, np.ndarray):
# Make sure init state is a numpy array
try:
init_state = np.asarray(init_state)
except Exception:
raise pyrado.TypeErr(given=init_state,
expected_type=np.ndarray)
if not self.init_space.contains(init_state, verbose=True):
raise pyrado.ValueErr(
msg="The init state must be within init state space!")
# Update the state attribute
self.state = init_state.copy()
# Reset the task which also resets the reward function if necessary
self._task.reset(env_spec=self.spec, init_state=init_state.copy())
# Reset MuJoCo simulation model (only reset the joint configuration)
self.sim.reset()
old_state = self.sim.get_state()
nq = self.init_qpos.size
if not init_state[:nq].shape == old_state.qpos.shape: # check joint positions dimension
raise pyrado.ShapeErr(given=init_state[:nq],
expected_match=old_state.qpos)
# Exclude everything that is appended to the state (at the end), e.g. the ball position for WAMBallInCupSim
if not init_state[
nq:2 *
nq].shape == old_state.qvel.shape: # check joint velocities dimension
raise pyrado.ShapeErr(given=init_state[nq:2 * nq],
expected_match=old_state.qvel)
new_state = mujoco_py.MjSimState(
# Exclude everything that is appended to the state (at the end), e.g. the ball position for WAMBallInCupSim
old_state.time,
init_state[:nq],
init_state[nq:2 * nq],
old_state.act,
old_state.udd_state,
)
self.sim.set_state(new_state)
self.sim.forward()
# Return an observation
return self.observe(self.state)
def reset(self):
qpos = self.init_qpos + np.random.uniform(size = self.sim.model.nq, low = 0.01, high = 0.01)
qvel = self.init_qvel + np.random.uniform(size = self.sim.model.nv, low = 0.01, high = 0.01)
old_state = self.sim.get_state()
new_state = mjc.MjSimState(old_state.time, qpos, qvel, old_state.act, old_state.udd_state)
self.sim.set_state(new_state)
self.sim.forward()
return self._get_obs()
def set_state(self, qpos, qvel=None):
qpos, qvel = map(np.array, [qpos, qvel])
assert qpos.shape == (self.model.nq, ) and (qvel is None or qvel.shape
== (self.model.nv, ))
old_state = self.sim.get_state()
qvel = old_state.qvel if qvel is None else qvel
new_state = mujoco_py.MjSimState(old_state.time, qpos, qvel,
old_state.act, old_state.udd_state)
self.sim.set_state(new_state)
self.sim.forward()
self.sim.step()
def do_simulation(self, qvel, n_frames):
assert qvel.shape == (self.model.nv, )
old_state = self.sim.get_state()
new_state = mujoco_py.MjSimState(old_state.time, old_state.qpos, qvel,
old_state.act, old_state.udd_state)
self.sim.set_state(new_state)
self.sim.forward()
for _ in range(n_frames):
self.sim.step()
def set_state(self, qpos, qvel):
assert qpos.shape == (self.model.nq, ) and qvel.shape == (
self.model.nv, )
if self._use_dm_backend:
self.sim.set_state(np.concatenate((qpos, qvel)))
else:
old_state = self.sim.get_state()
new_state = mujoco_py.MjSimState(old_state.time, qpos, qvel,
old_state.act,
old_state.udd_state)
self.sim.set_state(new_state)
self.sim.forward()
def set_state(self, state, posx=None, time=None):
oldstate = self.sim.get_state()
# first state is x position, second is none
posx = 0. if posx is None else posx
qpos = np.concatenate([np.array([posx]), state[:self.model.nq - 1]])
qvel = state[self.model.nq - 1:]
#time = oldstate.time if time is None else time
time = 0. if time is None else time
new_state = mujoco_py.MjSimState(time, qpos, qvel, oldstate.act,
oldstate.udd_state)
#print('udd_state is :: ------------------------- ', oldstate.act)
self.sim.set_state(new_state)
self.sim.forward()
def set_state_with_goal(self, qpos, qvel, goal, object_pos=None):
old_state = self.sim.get_state()
assert qpos.shape == old_state.qpos.shape and qvel.shape == old_state.qvel.shape
new_state = mujoco_py.MjSimState(old_state.time, qpos, qvel,
old_state.act, old_state.udd_state)
self.sim.set_state(new_state)
self.goal = goal.copy()
if self.has_object and object_pos is not None:
assert object_pos.shape == (7,)
self.sim.data.set_joint_qpos('object0:joint', object_pos)
self.sim.forward()
def _env_setup(self, initial_qpos, initial_qvel=None, initial_mpos=None):
# init qpos and qvel
qpos = initial_qpos
qvel = initial_qvel
if initial_qvel is None:
qvel = self.init_qvel
assert qpos.shape == (self.model.nq,) and qvel.shape == (self.model.nv,)
old_state = self.sim.get_state()
new_state = mujoco_py.MjSimState(old_state.time, qpos, qvel,
old_state.act, old_state.udd_state)
self.sim.set_state(new_state)
# init mpos
self.sim.data.mocap_pos[:, :] = initial_mpos
# sim forward
self.sim.forward()
def qvel_incr(i, viewer, sim, num_steps, initial_state, speed, step_flag):
zero_state = get_zero_state(initial_state)
delta_state = copy.deepcopy(zero_state)
delta_state.qvel[i] = speed
for j in range(0, num_steps):
viewer.render()
old_state = sim.get_state()
new_state = mujoco_py.MjSimState(old_state.time, old_state.qpos,
old_state.qvel + delta_state.qvel,
old_state.act, old_state.udd_state)
sim.set_state(new_state)
sim.forward()
if step_flag:
sim.step()
# print(new_state.qvel)
sim.set_state(initial_state)
sim.forward()
viewer.render()
def set_by_type(self, value, data_type="qpos"):
data = self.sim.get_state()
# use another data = transfer
data = np.array(data)
# assign data
if data_type == "qpos":
data[1] = value
elif data_type == "time":
data[0] = value
elif data_type == "qvel":
data[2] = value
elif data_type == "action":
data[3] = value
else:
pass
# transfer back to 5 values:
time, qpos, qvel, act, udd_state = data
self.sim.set_state(mj.MjSimState(time, qpos, qvel, act, udd_state))
self.sim.forward()
def reset(self):
self.sim.reset()
qpos = self.sim.data.qpos
qvel = self.sim.data.qvel
if STOCHASTIC:
qpos += np.random.uniform(low=-.005, high=.005, size=self.model.nq)
qvel += np.random.uniform(low=-.005, high=.005, size=self.model.nv)
old_state = self.sim.get_state()
tmp = mj.MjSimState(old_state.time, qpos, qvel, old_state.act,
old_state.udd_state)
self.sim.set_state(tmp)
self.sim.forward()
obs_vec = np.concatenate([
self.sim.data.qpos.flat[1:],
np.clip(self.sim.data.qvel.flat, -10, 10)
])
return obs_vec
def qpos_incr(i, viewer, sim, num_steps, initial_state, speed, step_flag):
zero_state = get_zero_state(initial_state)
# delta state has speed in "i"th dimension and 0 in all others
delta_state = copy.deepcopy(zero_state)
delta_state.qpos[i] = speed
for j in range(0, num_steps):
viewer.render()
old_state = sim.get_state()
new_state = mujoco_py.MjSimState(old_state.time,
old_state.qpos + delta_state.qpos,
old_state.qvel, old_state.act,
old_state.udd_state)
sim.set_state(new_state)
sim.forward()
if step_flag:
sim.step()
# print(new_state.qpos)
sim.set_state(initial_state)
sim.forward()
viewer.render()
def __init__(self):
mat = scipy.io.loadmat('Trajectory/Traj_Step_old_Fil.mat')
self.mat = mat['Data']
# mat_dist = scipy.io.loadmat('Trajectory/Phi_Traj_Step_12.mat')
# self.Dist = mat_dist['Dist']
# self.mu_r = self.Dist[0][0]
# self.mu_l = self.Dist[1][0]
# self.sigma_r = self.Dist[2][0]
# self.sigma_l = self.Dist[3][0]
self.max_traj = np.zeros(29)
self.x_dist = 0
self.data_length = 426
for i in range(29):
for j in range(self.data_length):
Traj = self.mat[j][0]
self.max_traj[i] = max(self.max_traj[i],
max(np.absolute(Traj[i])))
for i in range(29):
if self.max_traj[i] == 0:
self.max_traj[i] = 1
self.v_max = 2 #self.max_traj[15]
self.traj_id = 0
self.phi = 0
self.Traj = self.mat[self.traj_id][0]
self.traj_length = np.shape(self.Traj)[1]
self.target_vel = self.Traj[:, self.traj_length - 1][
15:18] #np.array(np.mean(self.Traj[15:18,:], axis =1))
self.step_no = 0
# self.start_no =self.step_no
self.err_limit = 14
self.alive_bonus = 5.0
self.max_frc = 2000
model_path = 'human7segment.xml'
frame_skip = 5
if model_path.startswith("/"):
fullpath = model_path
else:
fullpath = os.path.join(os.path.dirname(__file__), "assets",
model_path)
if not path.exists(fullpath):
raise IOError("File %s does not exist" % fullpath)
self.frame_skip = frame_skip
self.model = mujoco_py.load_model_from_path(fullpath)
self.sim = mujoco_py.MjSim(self.model)
self.data = self.sim.data
self.viewer = None
self._viewers = {}
self.metadata = {
'render.modes': ['human', 'rgb_array'],
'videos.frames_per_second': int(np.round(1.0 / self.dt))
}
self.init_qpos = self.sim.data.qpos.ravel().copy()
self.init_qvel = self.sim.data.qvel.ravel().copy()
# ---------------------- initialize the model ----------------------------
# self.ixqposTru = list(range(0, 3))
# self.ixqqutTru = list(range(3, 7))
# self.ixqHipFroR, self.ixqHipSagR, self.ixqHipVerR, self.ixqKneR, self.ixqAnkR, self.ixqHipFroL, self.ixqHipSagL, self.ixqHipVerL, self.ixqKneL, self.ixqAnkL = list(
# range(7, 17))
# # set initial joint angle
# # trunk CoM initial height
# # posTrunk = self.init_qpos[self.ixqposTru]
# # posTru = np.array((0.0, 0.0, 1.02))
# # trunk angle, lean a bit forward
# vecRot = np.array((0, 1, 0))
# angRot = 11.0 / 180 * np.pi # 10 degree
# qutTru = np.append(np.cos(angRot / 2), vecRot * np.sin(angRot / 2))
# # qutTru = quaternion.as_quat_array(qutRot)
self.init_qpos[0:3] = self.Traj[:, 0][0:3]
self.init_qpos[3:15] = self.Traj[:, 0][3:15]
self.init_qvel[0:14] = self.Traj[:, 0][15:29]
angHipFroR, angHipSagR, angKneR, angAnkR, angHipFroL, angHipSagL, angKneL, angAnkL = self.init_qpos[
7:15]
angHipAbdR = -angHipFroR
angHipAbdL = angHipFroL
angHipSagR = angHipSagR + np.pi
angHipSagL = angHipSagL + np.pi
angKneR = np.pi - angKneR
angKneL = np.pi - angKneL
angAnkR = angAnkR + np.pi / 2.0
angAnkL = angAnkL + np.pi / 2.0
# leva qvel as default
old_state = self.sim.get_state()
new_state = mujoco_py.MjSimState(old_state.time, self.init_qpos,
self.init_qvel, old_state.act,
old_state.udd_state)
self.sim.set_state(new_state)
self.sim.forward()
# create 11 muscles for each leg
# NOTE: the timestep has to be the same as in the xml file
timestep = 5e-4
humanmuscle.timestep = timestep
nMus = 22
self.musHABR = humanmuscle.HAB(angHipAbdR)
self.musHADR = humanmuscle.HAD(angHipAbdR)
self.musGLUR = humanmuscle.GLU(angHipSagR)
self.musHFLR = humanmuscle.HFL(angHipSagR)
self.musHAMR = humanmuscle.HAM(angHipSagR, angKneR)
self.musREFR = humanmuscle.REF(angHipSagR, angKneR)
self.musVASR = humanmuscle.VAS(angKneR)
self.musBFSHR = humanmuscle.BFSH(angKneR)
self.musGASR = humanmuscle.GAS(angKneR, angAnkR)
self.musSOLR = humanmuscle.SOL(angAnkR)
self.musTIAR = humanmuscle.TIA(angAnkR)
self.musHABL = humanmuscle.HAB(angHipAbdL)
self.musHADL = humanmuscle.HAD(angHipAbdL)
self.musGLUL = humanmuscle.GLU(angHipSagL)
self.musHFLL = humanmuscle.HFL(angHipSagL)
self.musHAML = humanmuscle.HAM(angHipSagL, angKneL)
self.musREFL = humanmuscle.REF(angHipSagL, angKneL)
self.musVASL = humanmuscle.VAS(angKneL)
self.musBFSHL = humanmuscle.BFSH(angKneL)
self.musGASL = humanmuscle.GAS(angKneL, angAnkL)
self.musSOLL = humanmuscle.SOL(angAnkL)
self.musTIAL = humanmuscle.TIA(angAnkL)
self.frcmtc_buffer = np.zeros((22, 7))
self.vce_buffer = np.zeros((22, 7))
self.lce_buffer = np.array([[self.musHABR.lce, self.musHADR.lce, self.musGLUR.lce, self.musHFLR.lce, self.musHAMR.lce, self.musREFR.lce,\
self.musBFSHR.lce,self.musVASR.lce, self.musGASR.lce, self.musSOLR.lce, self.musTIAR.lce, self.musHABL.lce, self.musHADL.lce,\
self.musGLUL.lce, self.musHFLL.lce, self.musHAML.lce,self.musREFL.lce, self.musBFSHL.lce, self.musVASL.lce, self.musGASL.lce,\
self.musSOLL.lce, self.musTIAL.lce] for i in range(7)]).transpose()
# -------------- run step ----------------------
observation, _reward, done, _info = self.step(np.zeros(nMus))
# observation, _reward, done, _info = self.step(np.zeros(self.model.nu))
#assert not done
self.obs_dim = observation.size
print(self.obs_dim)
# -------------- set actuator control range ---------------
# bounds = self.model.actuator_ctrlrange.copy()
# self.bounds = bounds
# low = bounds[:, 0]
# high = bounds[:, 1]
# self.action_space = spaces.Box(low=low, high=high)
# -------------- set muscle stimulation range ---------------
low = np.zeros((nMus))
high = np.ones((nMus))
self.action_space = spaces.Box(low=low, high=high)
high = np.inf * np.ones(self.obs_dim)
low = -high
self.observation_space = spaces.Box(low, high)
self.seed()
utils.EzPickle.__init__(self)
model = mujoco_py.load_model_from_path('pioneer/envs/assets/pioneer2.xml')
sim = mujoco_py.MjSim(model)
print(f'timestep: {model.opt.timestep}')
bounds = model.jnt_range.copy().astype(np.float32)
low, high = bounds.T
position_space = spaces.Box(low=low, high=high, dtype=np.float32)
print(f'bounds: {bounds}')
print(f'nq={model.nq}, nv={model.nv}')
a0 = sim.get_state()
print(f'qpos={a0.qpos}, nv={a0.qvel}')
a1 = mujoco_py.MjSimState(a0.time, a0.qpos, [0.2, -0.2], a0.act, a0.udd_state)
sim.set_state(a1)
sim.step()
sim.forward()
print(sim.data.qpos.flat[:])
print(sim.data.qvel.flat[:2])
exit(0)
#
# print(position_space.sample())
#
# sim.step()
#
# <site name="tip" pos="0 0 .5" size="0.01 0.01"/>
model = load_model_from_xml(MODEL_LEG_XML)
sim = MjSim(model)
viewer = MjViewer(sim)
qpos = np.zeros(2)
qvel = np.zeros(2)
old_state = sim.get_state()
angHip = 120.0 / 180 * np.pi
angKne = (180.0 - 40.0) / 180 * np.pi
qpos[0] = angHip # hip joint initial angle
qpos[1] = np.pi - angKne # knee joint initial angle
qvel[0] = 0.0 # hip joint initial velocity
qvel[1] = 0.0 # knee joint initial velocity
new_state = mujoco_py.MjSimState(old_state.time, qpos, qvel,
old_state.act, old_state.udd_state)
sim.set_state(new_state)
sim.forward()
# viewer.render()
# create muscle
timestep = 5e-4
# Series elastic element (SE) force-length relationship
eref = 0.04 # [lslack] tendon reference strain
# excitation-contraction coupling
preAct = 0.01 # [] preactivation
tau = 0.01 # [s] delay time constant
# contractile element (CE) force-length relationship
w = 0.56 # [lopt] width
c = 0.05 # []; remaining force at +/- width