Use self.sim instead of physics
This commit is contained in:
parent
27ebe4ffc1
commit
7287642a76
@ -17,7 +17,6 @@
|
|||||||
|
|
||||||
import math
|
import math
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import mujoco_py
|
|
||||||
from gym import utils
|
from gym import utils
|
||||||
from gym.envs.mujoco import mujoco_env
|
from gym.envs.mujoco import mujoco_env
|
||||||
|
|
||||||
@ -54,16 +53,6 @@ class AntEnv(mujoco_env.MujocoEnv, utils.EzPickle):
|
|||||||
mujoco_env.MujocoEnv.__init__(self, file_path, 5)
|
mujoco_env.MujocoEnv.__init__(self, file_path, 5)
|
||||||
utils.EzPickle.__init__(self)
|
utils.EzPickle.__init__(self)
|
||||||
|
|
||||||
@property
|
|
||||||
def physics(self):
|
|
||||||
# check mujoco version is greater than version 1.50 to call correct physics
|
|
||||||
# model containing PyMjData object for getting and setting position/velocity
|
|
||||||
# check https://github.com/openai/mujoco-py/issues/80 for updates to api
|
|
||||||
if mujoco_py.get_version() >= "1.50":
|
|
||||||
return self.sim
|
|
||||||
else:
|
|
||||||
return self.model
|
|
||||||
|
|
||||||
def _step(self, a):
|
def _step(self, a):
|
||||||
return self.step(a)
|
return self.step(a)
|
||||||
|
|
||||||
@ -94,13 +83,13 @@ class AntEnv(mujoco_env.MujocoEnv, utils.EzPickle):
|
|||||||
if self._expose_all_qpos:
|
if self._expose_all_qpos:
|
||||||
obs = np.concatenate(
|
obs = np.concatenate(
|
||||||
[
|
[
|
||||||
self.physics.data.qpos.flat[:15], # Ensures only ant obs.
|
self.sim.data.qpos.flat[:15], # Ensures only ant obs.
|
||||||
self.physics.data.qvel.flat[:14],
|
self.sim.data.qvel.flat[:14],
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
else:
|
else:
|
||||||
obs = np.concatenate(
|
obs = np.concatenate(
|
||||||
[self.physics.data.qpos.flat[2:15], self.physics.data.qvel.flat[:14],]
|
[self.sim.data.qpos.flat[2:15], self.sim.data.qvel.flat[:14],]
|
||||||
)
|
)
|
||||||
|
|
||||||
if self._expose_body_coms is not None:
|
if self._expose_body_coms is not None:
|
||||||
@ -137,7 +126,7 @@ class AntEnv(mujoco_env.MujocoEnv, utils.EzPickle):
|
|||||||
|
|
||||||
def get_ori(self):
|
def get_ori(self):
|
||||||
ori = [0, 1, 0, 0]
|
ori = [0, 1, 0, 0]
|
||||||
rot = self.physics.data.qpos[
|
rot = self.sim.data.qpos[
|
||||||
self.__class__.ORI_IND : self.__class__.ORI_IND + 4
|
self.__class__.ORI_IND : self.__class__.ORI_IND + 4
|
||||||
] # take the quaternion
|
] # take the quaternion
|
||||||
ori = q_mult(q_mult(rot, ori), q_inv(rot))[1:3] # project onto x-y plane
|
ori = q_mult(q_mult(rot, ori), q_inv(rot))[1:3] # project onto x-y plane
|
||||||
@ -145,12 +134,12 @@ class AntEnv(mujoco_env.MujocoEnv, utils.EzPickle):
|
|||||||
return ori
|
return ori
|
||||||
|
|
||||||
def set_xy(self, xy):
|
def set_xy(self, xy):
|
||||||
qpos = np.copy(self.physics.data.qpos)
|
qpos = np.copy(self.sim.data.qpos)
|
||||||
qpos[0] = xy[0]
|
qpos[0] = xy[0]
|
||||||
qpos[1] = xy[1]
|
qpos[1] = xy[1]
|
||||||
|
|
||||||
qvel = self.physics.data.qvel
|
qvel = self.sim.data.qvel
|
||||||
self.set_state(qpos, qvel)
|
self.set_state(qpos, qvel)
|
||||||
|
|
||||||
def get_xy(self):
|
def get_xy(self):
|
||||||
return self.physics.data.qpos[:2]
|
return self.sim.data.qpos[:2]
|
||||||
|
@ -17,7 +17,6 @@
|
|||||||
|
|
||||||
import math
|
import math
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import mujoco_py
|
|
||||||
from gym import utils
|
from gym import utils
|
||||||
from gym.envs.mujoco import mujoco_env
|
from gym.envs.mujoco import mujoco_env
|
||||||
|
|
||||||
@ -32,22 +31,11 @@ class PointEnv(mujoco_env.MujocoEnv, utils.EzPickle):
|
|||||||
mujoco_env.MujocoEnv.__init__(self, file_path, 1)
|
mujoco_env.MujocoEnv.__init__(self, file_path, 1)
|
||||||
utils.EzPickle.__init__(self)
|
utils.EzPickle.__init__(self)
|
||||||
|
|
||||||
@property
|
|
||||||
def physics(self):
|
|
||||||
# check mujoco version is greater than version 1.50 to call correct physics
|
|
||||||
# model containing PyMjData object for getting and setting position/velocity
|
|
||||||
# check https://github.com/openai/mujoco-py/issues/80 for updates to api
|
|
||||||
if mujoco_py.get_version() >= "1.50":
|
|
||||||
return self.sim
|
|
||||||
else:
|
|
||||||
return self.model
|
|
||||||
|
|
||||||
def _step(self, a):
|
def _step(self, a):
|
||||||
return self.step(a)
|
return self.step(a)
|
||||||
|
|
||||||
def step(self, action):
|
def step(self, action):
|
||||||
action[0] = 0.2 * action[0]
|
qpos = np.copy(self.sim.data.qpos)
|
||||||
qpos = np.copy(self.physics.data.qpos)
|
|
||||||
qpos[2] += action[1]
|
qpos[2] += action[1]
|
||||||
ori = qpos[2]
|
ori = qpos[2]
|
||||||
# compute increment in each direction
|
# compute increment in each direction
|
||||||
@ -56,10 +44,10 @@ class PointEnv(mujoco_env.MujocoEnv, utils.EzPickle):
|
|||||||
# ensure that the robot is within reasonable range
|
# ensure that the robot is within reasonable range
|
||||||
qpos[0] = np.clip(qpos[0] + dx, -100, 100)
|
qpos[0] = np.clip(qpos[0] + dx, -100, 100)
|
||||||
qpos[1] = np.clip(qpos[1] + dy, -100, 100)
|
qpos[1] = np.clip(qpos[1] + dy, -100, 100)
|
||||||
qvel = self.physics.data.qvel
|
qvel = self.sim.data.qvel
|
||||||
self.set_state(qpos, qvel)
|
self.set_state(qpos, qvel)
|
||||||
for _ in range(0, self.frame_skip):
|
for _ in range(0, self.frame_skip):
|
||||||
self.physics.step()
|
self.sim.step()
|
||||||
next_obs = self._get_obs()
|
next_obs = self._get_obs()
|
||||||
reward = 0
|
reward = 0
|
||||||
done = False
|
done = False
|
||||||
@ -70,19 +58,19 @@ class PointEnv(mujoco_env.MujocoEnv, utils.EzPickle):
|
|||||||
if self._expose_all_qpos:
|
if self._expose_all_qpos:
|
||||||
return np.concatenate(
|
return np.concatenate(
|
||||||
[
|
[
|
||||||
self.physics.data.qpos.flat[:3], # Only point-relevant coords.
|
self.sim.data.qpos.flat[:3], # Only point-relevant coords.
|
||||||
self.physics.data.qvel.flat[:3],
|
self.sim.data.qvel.flat[:3],
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
return np.concatenate(
|
return np.concatenate(
|
||||||
[self.physics.data.qpos.flat[2:3], self.physics.data.qvel.flat[:3]]
|
[self.sim.data.qpos.flat[2:3], self.sim.data.qvel.flat[:3]]
|
||||||
)
|
)
|
||||||
|
|
||||||
def reset_model(self):
|
def reset_model(self):
|
||||||
qpos = self.init_qpos + self.np_random.uniform(
|
qpos = self.init_qpos + self.np_random.uniform(
|
||||||
size=self.physics.model.nq, low=-0.1, high=0.1
|
size=self.sim.model.nq, low=-0.1, high=0.1
|
||||||
)
|
)
|
||||||
qvel = self.init_qvel + self.np_random.randn(self.physics.model.nv) * 0.1
|
qvel = self.init_qvel + self.np_random.randn(self.sim.model.nv) * 0.1
|
||||||
|
|
||||||
# Set everything other than point to original position and 0 velocity.
|
# Set everything other than point to original position and 0 velocity.
|
||||||
qpos[3:] = self.init_qpos[3:]
|
qpos[3:] = self.init_qpos[3:]
|
||||||
@ -90,13 +78,17 @@ class PointEnv(mujoco_env.MujocoEnv, utils.EzPickle):
|
|||||||
self.set_state(qpos, qvel)
|
self.set_state(qpos, qvel)
|
||||||
return self._get_obs()
|
return self._get_obs()
|
||||||
|
|
||||||
def get_ori(self):
|
def get_xy(self):
|
||||||
return self.physics.data.qpos[self.__class__.ORI_IND]
|
qpos = self.sim.data.qpos
|
||||||
|
return qpos[0], qpos[0]
|
||||||
|
|
||||||
def set_xy(self, xy):
|
def set_xy(self, xy):
|
||||||
qpos = np.copy(self.physics.data.qpos)
|
qpos = np.copy(self.sim.data.qpos)
|
||||||
qpos[0] = xy[0]
|
qpos[0] = xy[0]
|
||||||
qpos[1] = xy[1]
|
qpos[1] = xy[1]
|
||||||
|
|
||||||
qvel = self.physics.data.qvel
|
qvel = self.sim.data.qvel
|
||||||
self.set_state(qpos, qvel)
|
self.set_state(qpos, qvel)
|
||||||
|
|
||||||
|
def get_ori(self):
|
||||||
|
return self.sim.data.qpos[self.ORI_IND]
|
||||||
|
Loading…
Reference in New Issue
Block a user